├── action └── FollowPath.action ├── src ├── follower.cpp ├── configurator.cpp └── lib │ ├── common.h │ ├── horizon_finder.h │ ├── robot.h │ ├── segmenter.h │ ├── robot.cpp │ ├── disjoint_set.h │ ├── configurator.h │ ├── disjoint_set.cpp │ ├── detector.h │ ├── path_follower.h │ ├── classifier.h │ ├── horizon_finder.cpp │ ├── configurator.cpp │ ├── segmenter.cpp │ ├── path_follower.cpp │ ├── detector.cpp │ └── classifier.cpp ├── launch ├── configurator.launch └── follower.launch ├── README.md ├── package.xml └── CMakeLists.txt /action/FollowPath.action: -------------------------------------------------------------------------------- 1 | float32 distance 2 | --- 3 | float32 final_distance 4 | --- 5 | float32 distance_traveled 6 | -------------------------------------------------------------------------------- /src/follower.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "lib/path_follower.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "follower"); 7 | ros::NodeHandle n("~"); 8 | 9 | floordetection::PathFollower f(n); 10 | ros::spin(); 11 | } 12 | -------------------------------------------------------------------------------- /src/configurator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "lib/configurator.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "configurator"); 7 | ros::NodeHandle n("~"); 8 | 9 | floordetection::Configurator c(n); 10 | ros::spin(); 11 | } 12 | -------------------------------------------------------------------------------- /src/lib/common.h: -------------------------------------------------------------------------------- 1 | #ifndef __COMMON_H__ 2 | #define __COMMON_H__ 3 | 4 | #include 5 | 6 | namespace floordetection { 7 | cv::Vec3f hsv_diff(const cv::Vec3f& v1, const cv::Vec3f& v2); 8 | void rgb2hsv(const cv::Vec3f& rgb, cv::Vec3f& hsv); 9 | } 10 | 11 | 12 | #endif 13 | -------------------------------------------------------------------------------- /src/lib/horizon_finder.h: -------------------------------------------------------------------------------- 1 | #ifndef __HORIZON_FINDER_H__ 2 | #define __HORIZON_FINDER_H__ 3 | 4 | #include 5 | #include 6 | 7 | namespace floordetection { 8 | class HorizonFinder { 9 | public: 10 | HorizonFinder(ros::NodeHandle& ros_node); 11 | 12 | int find(const cv::Mat& input); 13 | 14 | private: 15 | int subimages; 16 | int last_horizon; 17 | 18 | }; 19 | } 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /src/lib/robot.h: -------------------------------------------------------------------------------- 1 | #ifndef __ROBOT_H__ 2 | #define __ROBOT_H__ 3 | 4 | #include 5 | 6 | namespace floordetection { 7 | class Robot { 8 | public: 9 | Robot(ros::NodeHandle& ros_node); 10 | ~Robot(void); 11 | 12 | void set_speeds(float xspeed, float aspeed); 13 | void stop(void); 14 | 15 | private: 16 | ros::Publisher vel_pub; 17 | ros::Subscriber odo_sub; 18 | }; 19 | } 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /src/lib/segmenter.h: -------------------------------------------------------------------------------- 1 | #ifndef __SEGMENTER_H__ 2 | #define __SEGMENTER_H__ 3 | 4 | #include 5 | #include 6 | #include "segmenter.h" 7 | 8 | namespace floordetection { 9 | class Segmenter { 10 | public: 11 | Segmenter(ros::NodeHandle& ros_node); 12 | 13 | void save_parameters(void); 14 | 15 | void segment(const cv::Mat& intput, cv::Mat& output); 16 | 17 | double c_threshold; 18 | int min_segment; 19 | 20 | private: 21 | ros::NodeHandle& ros_node; 22 | }; 23 | } 24 | 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /src/lib/robot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "robot.h" 3 | namespace fd = floordetection; 4 | 5 | fd::Robot::Robot(ros::NodeHandle& ros_node) 6 | { 7 | vel_pub = ros_node.advertise("/robot/cmd_vel", 1); 8 | } 9 | 10 | fd::Robot::~Robot(void) 11 | { 12 | stop(); 13 | } 14 | 15 | void fd::Robot::stop(void) 16 | { 17 | set_speeds(0,0); 18 | } 19 | 20 | void fd::Robot::set_speeds(float xspeed, float aspeed) 21 | { 22 | geometry_msgs::Twist t; 23 | t.linear.x = xspeed; 24 | t.angular.z = aspeed; 25 | vel_pub.publish(t); 26 | } 27 | 28 | -------------------------------------------------------------------------------- /src/lib/disjoint_set.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __DISJOINT_SET__ 3 | #define __DISJOINT_SET__ 4 | 5 | // disjoint-set forests using union-by-rank and path compression (sort of). 6 | 7 | typedef enum {GROUND, NON_GROUND} type; 8 | 9 | typedef struct { 10 | int rank; 11 | int p; 12 | int size; 13 | type type_of; 14 | int total_pixels; 15 | int marked_pixels; 16 | } uni_elt; 17 | 18 | class universe { 19 | public: 20 | universe(int elements); 21 | ~universe(); 22 | int find(int x); 23 | void join(int x, int y); 24 | int size(int x) const { return elts[x].size; } 25 | int num_sets() const { return num; } 26 | uni_elt* get_elt(int x) { return (uni_elt*) &elts[x];} 27 | 28 | private: 29 | uni_elt *elts; 30 | int num; 31 | }; 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /src/lib/configurator.h: -------------------------------------------------------------------------------- 1 | #ifndef __CONFIGURATOR_H__ 2 | #define __CONFIGURATOR_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include "detector.h" 8 | 9 | namespace floordetection { 10 | class Configurator { 11 | public: 12 | Configurator(ros::NodeHandle& n); 13 | 14 | void on_image(const sensor_msgs::Image::ConstPtr& msg); 15 | 16 | void configure(void); 17 | void create_variables(void); 18 | void create_trackbars(void); 19 | void read_variables(void); 20 | 21 | struct Variable { 22 | int value, max; 23 | }; 24 | 25 | private: 26 | Detector detector; 27 | std::map vars; 28 | 29 | ros::Subscriber image_sub; 30 | ros::NodeHandle& ros_node; 31 | }; 32 | } 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /src/lib/disjoint_set.cpp: -------------------------------------------------------------------------------- 1 | #include "disjoint_set.h" 2 | 3 | universe::universe(int elements) { 4 | elts = new uni_elt[elements]; 5 | num = elements; 6 | for (int i = 0; i < elements; i++) { 7 | elts[i].rank = 0; 8 | elts[i].size = 1; 9 | elts[i].p = i; 10 | elts[i].total_pixels = 0; 11 | elts[i].marked_pixels = 0; 12 | } 13 | } 14 | 15 | universe::~universe() { 16 | delete [] elts; 17 | } 18 | 19 | int universe::find(int x) { 20 | int y = x; 21 | while (y != elts[y].p) 22 | y = elts[y].p; 23 | elts[x].p = y; 24 | return y; 25 | } 26 | 27 | void universe::join(int x, int y) { 28 | if (elts[x].rank > elts[y].rank) { 29 | elts[y].p = x; 30 | elts[x].size += elts[y].size; 31 | } else { 32 | elts[x].p = y; 33 | elts[y].size += elts[x].size; 34 | if (elts[x].rank == elts[y].rank) 35 | elts[y].rank++; 36 | } 37 | num--; 38 | } 39 | -------------------------------------------------------------------------------- /launch/configurator.launch: -------------------------------------------------------------------------------- 1 | 2 | 10 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Description 2 | =========== 3 | 4 | This is a ROS node that performs image-processing in order to detect a traversable path and commands a mobile robot to follow it. 5 | The documentation at the moment is incomplete but will soon be updated. 6 | 7 | Citation 8 | ======== 9 | 10 | This code is the implementation of the method presented in several scientific works. If this code is used either as-is or as part of a larger project, 11 | it is mandatory to cite the following works: 12 | 13 | [1] “Real-time monocular image based path detection”, Pablo De Cristóforis, Matias Nitsche; Tomas Krajnik, Marta Mejail, Journal of Real Time Image Processing, Special Issue, Junio de 2013, p1-14, Springer-Verlag 14 | 15 | [2] “Real-time on-board image processing using an embedded GPU for monocular vision-based navigation”, Matías Nitsche, Pablo De Cristóforis, Lectures Notes in Computer Science. Springer-Verlag, vol 7441, 2012, pp 591-598, ISSN 0302-9743. 16 | -------------------------------------------------------------------------------- /launch/follower.launch: -------------------------------------------------------------------------------- 1 | 2 | 10 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/lib/detector.h: -------------------------------------------------------------------------------- 1 | #ifndef __DETECTOR_H__ 2 | #define __DETECTOR_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "classifier.h" 9 | #include "segmenter.h" 10 | #include "horizon_finder.h" 11 | 12 | namespace floordetection 13 | { 14 | class Detector { 15 | public: 16 | Detector(ros::NodeHandle& ros_node, bool show_windows = false); 17 | 18 | void configure(void); 19 | void detect(const cv::Mat& image, std::vector& frontier, int& horizon_level, cv::Rect& roi); 20 | 21 | void save_parameters(void); 22 | 23 | /*cv::Mat input, input_hsv, avg_rgb, avg_hsv, blured, blured_hsv, labeled, colored_labels, classified, eroded, final; 24 | cv::Mat input_original; 25 | cv::Mat hue, saturation, value, hue_segments, saturation_segments, value_segments;*/ 26 | 27 | Classifier classifier; 28 | Segmenter segmenter; 29 | HorizonFinder horizon_finder; 30 | 31 | int median_blur; 32 | int erode_size; 33 | 34 | private: 35 | ros::NodeHandle& ros_node; 36 | bool show_windows; 37 | 38 | void extract_segments(const cv::Mat& input, const cv::Mat& input_hsv, cv::Mat& labeled, std::vector& label2segment); 39 | void extract_contour(const cv::Mat& classified, std::vector& frontier, const cv::Rect& roi); 40 | }; 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /src/lib/path_follower.h: -------------------------------------------------------------------------------- 1 | #ifndef __PATH_FOLLOWER_H__ 2 | #define __PATH_FOLLOWER_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include "detector.h" 8 | #include "robot.h" 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace floordetection { 15 | class PathFollower { 16 | public: 17 | PathFollower(ros::NodeHandle& ros_node); 18 | 19 | void on_image(const sensor_msgs::Image::ConstPtr& msg); 20 | 21 | void follow(const cv::Mat& input); 22 | void compute_control(std::vector& frontier, const cv::Mat& input, int horizon_level, const cv::Rect& roi); 23 | 24 | ros::Subscriber image_sub; 25 | ros::Subscriber event_sub; 26 | ros::Subscriber odo_sub; 27 | 28 | void on_event(const universal_teleop::Event::ConstPtr& msg); 29 | void on_odometry(const nav_msgs::Odometry::ConstPtr& msg); 30 | 31 | ros::ServiceServer start_service, stop_service; 32 | bool start_request(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); 33 | bool stop_request(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); 34 | 35 | typedef actionlib::SimpleActionServer ActionServer; 36 | boost::shared_ptr action_server; 37 | floordetection::FollowPathFeedback action_feedback; 38 | floordetection::FollowPathGoal current_goal; 39 | void new_goal(void); 40 | bool has_last_odometry; 41 | nav_msgs::Odometry last_odometry; 42 | 43 | 44 | Detector detector; 45 | Robot robot; 46 | float accum_xspeed, accum_aspeed; 47 | double xspeed_scale, aspeed_scale; 48 | bool started; 49 | }; 50 | } 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /src/lib/classifier.h: -------------------------------------------------------------------------------- 1 | #ifndef __CLASSIFIER_H__ 2 | #define __CLASSIFIER_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace floordetection { 9 | class Segment { 10 | public: 11 | Segment(void); 12 | 13 | float distance(const Segment& m, const cv::Vec3f& minimums = cv::Vec3f(FLT_MAX, FLT_MAX, FLT_MAX)) const; 14 | float distance(const cv::Vec3f& elem, const cv::Vec3f& minimums = cv::Vec3f(FLT_MAX, FLT_MAX, FLT_MAX)) const; 15 | void mix(const Segment& other, float alpha = -1); 16 | 17 | cv::Vec3f mu; 18 | cv::Matx33f sigma; 19 | 20 | cv::Vec3f mu_hsv; 21 | cv::Matx33f sigma_hsv; 22 | 23 | int floor_pixels; 24 | int mass; 25 | 26 | float floor_certainty; 27 | }; 28 | 29 | class Classifier { 30 | public: 31 | Classifier(ros::NodeHandle& ros_node); 32 | 33 | void save_parameters(void); 34 | 35 | void classify(const cv::Mat& input, const cv::Mat& input_hsv, const cv::Mat& labeled, 36 | std::vector& label2segment, cv::Mat& output, cv::Rect& roi); 37 | 38 | void extract_training_models(const cv::Rect& roi, const cv::Mat& labeled, 39 | const std::vector& label2segment, std::list& training_models); 40 | void update_learnt_models(const std::list& training_models); 41 | void classify_segments(std::vector& label2segment, int floor_mass); 42 | void classify_pixels(const cv::Mat& input_hsv, int floor_mass); 43 | 44 | static void set_minimum_cov(cv::Matx33f& cov, const cv::Vec3f& minimums); 45 | 46 | //Segment floor_segment, classified_segment; 47 | std::list learnt_models, training_models; 48 | 49 | cv::Mat training_model_colors; 50 | cv::Mat debug; 51 | 52 | double rectangle_width, rectangle_height; 53 | cv::Vec3f threshold, threshold_merge; 54 | int models; 55 | 56 | private: 57 | ros::NodeHandle& ros_node; 58 | }; 59 | } 60 | 61 | std::ostream& operator<<(std::ostream& out, const floordetection::Segment& s); 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | floordetection 4 | 0.0.0 5 | The floordetection package 6 | 7 | 8 | 9 | 10 | v01d 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | cv_bridge 44 | geometry_msgs 45 | nav_msgs 46 | roscpp 47 | std_msgs 48 | universal_teleop 49 | actionlib 50 | actionlib_msgs 51 | universal_teleop 52 | cv_bridge 53 | geometry_msgs 54 | nav_msgs 55 | roscpp 56 | std_msgs 57 | actionlib 58 | actionlib_msgs 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /src/lib/horizon_finder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "horizon_finder.h" 3 | namespace fd = floordetection; 4 | 5 | fd::HorizonFinder::HorizonFinder(ros::NodeHandle& ros_node) 6 | { 7 | last_horizon = -1; 8 | ros_node.param("horizon_finder_subimages", subimages, 20); 9 | } 10 | 11 | int fd::HorizonFinder::find(const cv::Mat& blured) 12 | { 13 | // subimg height, for the upper half of the image 14 | uint height = (int)(blured.rows / (float)(2 * subimages)); 15 | 16 | // convert half of image to gray scale 17 | cv::Mat blured_gray; 18 | cv::Mat blured_half(blured, cv::Range(0, (int)(blured.rows * 0.5) + (height / 2) + 1), cv::Range::all()); 19 | cv::cvtColor(blured_half, blured_gray, CV_RGB2GRAY); 20 | 21 | // Third apply a Sobel filter to obtain the Y derived image 22 | cv::Mat sobel; 23 | cv::Sobel(blured_gray, sobel, -1, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT); 24 | 25 | // Then apply the OTSU threshold to obtain a binary image 26 | cv::Mat otsu; 27 | cv::threshold(sobel, otsu, 0, 255, cv::THRESH_OTSU); 28 | 29 | // Then erode the binary image to reduce noise 30 | int erosion_size = 1; 31 | cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), cv::Point(erosion_size, erosion_size)); 32 | cv::Mat dst; 33 | cv::erode(otsu, dst, kernel); 34 | 35 | // Compute the amount of foregrounds pixels for each window. 36 | int maxforeground1 = 0; 37 | int index1 = 0; 38 | std::vector foregrounds1(subimages); 39 | for(uint i = 0; i < subimages; i++){ 40 | foregrounds1[i] = 0; 41 | cv::Mat subimg = dst(cv::Range(i * height, (i + 1) * height), cv::Range::all()); 42 | uchar* ptrData = subimg.ptr(0); 43 | for(int j = 0; j < subimg.rows * subimg.cols; j++, ++ptrData) { 44 | if (*ptrData == 255) foregrounds1[i]++; 45 | } 46 | 47 | if (foregrounds1[i] > maxforeground1) { 48 | maxforeground1 = foregrounds1[i]; 49 | index1 = i; 50 | } 51 | } 52 | 53 | // The same as before, but for each window slip half a height to overlap them 54 | int maxforeground2 = 0; 55 | int index2 = 0; 56 | std::vector foregrounds2(subimages); 57 | for(unsigned i = 0; i < subimages; i++){ 58 | foregrounds2[i] = 0; 59 | cv::Mat subimg = dst(cv::Range(i * height + (height / 2), (i + 1) * height + (height / 2)), cv::Range::all()); 60 | uchar* ptrData = subimg.ptr(0); 61 | for(int j = 0; j < subimg.rows * subimg.cols; j++, ++ptrData) { 62 | if (*ptrData == 255) foregrounds2[i]++; 63 | } 64 | if (foregrounds2[i] > maxforeground2) { 65 | maxforeground2 = foregrounds2[i]; 66 | index2 = i; 67 | } 68 | } 69 | 70 | // The max horizon between windows and overlap windows is the winner 71 | int horizon; 72 | if (maxforeground1 > maxforeground2) horizon = index1 * height; 73 | else horizon = index2 * height + (height / 2); 74 | 75 | float alpha = 0.5; 76 | if (last_horizon == -1) last_horizon = horizon; 77 | else last_horizon = (int)roundf(alpha * horizon + (1 - alpha) * last_horizon); 78 | 79 | return last_horizon; 80 | } 81 | 82 | -------------------------------------------------------------------------------- /src/lib/configurator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "configurator.h" 3 | namespace fd = floordetection; 4 | 5 | fd::Configurator::Configurator(ros::NodeHandle& _ros_node) 6 | : detector(_ros_node, true), ros_node(_ros_node) 7 | { 8 | cv::namedWindow("options"); 9 | 10 | create_variables(); 11 | create_trackbars(); 12 | 13 | image_sub = ros_node.subscribe("/camera/image_raw", 1, &Configurator::on_image, this); 14 | } 15 | 16 | void fd::Configurator::on_image(const sensor_msgs::Image::ConstPtr& msg) 17 | { 18 | cv_bridge::CvImageConstPtr bridge_ptr = cv_bridge::toCvShare(msg, "rgb8"); 19 | std::vector frontier; 20 | int horizon_level; 21 | cv::Rect roi; 22 | detector.detect(bridge_ptr->image, frontier, horizon_level, roi); 23 | 24 | cv::imshow("options", cv::Mat::zeros(1, 256, CV_8UC3)); 25 | read_variables(); 26 | } 27 | 28 | 29 | void fd::Configurator::create_variables(void) 30 | { 31 | vars["median_blur"].value = (detector.median_blur - 1) / 2; 32 | vars["median_blur"].max = 11; 33 | 34 | vars["erode_size"].value = detector.erode_size; 35 | vars["erode_size"].max = 20; 36 | 37 | vars["c_threshold"].value = detector.segmenter.c_threshold * 1000; 38 | vars["c_threshold"].max = 1000; 39 | 40 | vars["min_segment"].value = detector.segmenter.min_segment; 41 | vars["min_segment"].max = 1000; 42 | 43 | vars["rectangle_height"].value = detector.classifier.rectangle_height * 1000; 44 | vars["rectangle_height"].max = 1000; 45 | 46 | vars["rectangle_width"].value = detector.classifier.rectangle_width * 1000; 47 | vars["rectangle_width"].max = 1000; 48 | 49 | vars["models"].value = detector.classifier.models; 50 | vars["models"].max = 10; 51 | 52 | vars["threshold_h"].value = detector.classifier.threshold(0) * 1000; 53 | vars["threshold_s"].value = detector.classifier.threshold(1) * 1000; 54 | vars["threshold_v"].value = detector.classifier.threshold(2) * 1000; 55 | 56 | vars["threshold_h_merge"].value = detector.classifier.threshold_merge(0) * 1000; 57 | vars["threshold_s_merge"].value = detector.classifier.threshold_merge(1) * 1000; 58 | vars["threshold_v_merge"].value = detector.classifier.threshold_merge(2) * 1000; 59 | 60 | vars["threshold_h"].max = vars["threshold_s"].max = vars["threshold_v"].max = 1000; 61 | vars["threshold_h_merge"].max = vars["threshold_s_merge"].max = vars["threshold_v_merge"].max = 1000; 62 | } 63 | 64 | void fd::Configurator::read_variables(void) 65 | { 66 | detector.median_blur = vars["median_blur"].value * 2 + 1; 67 | detector.erode_size = vars["erode_size"].value; 68 | //detector.save_parameters(); 69 | 70 | detector.segmenter.c_threshold = vars["c_threshold"].value / 1000.0; 71 | detector.segmenter.min_segment = vars["min_segment"].value; 72 | //detector.segmenter.save_parameters(); 73 | 74 | detector.classifier.rectangle_height = vars["rectangle_height"].value / 1000.0; 75 | detector.classifier.rectangle_width = vars["rectangle_width"].value / 1000.0; 76 | detector.classifier.models = vars["models"].value; 77 | detector.classifier.threshold(0) = vars["threshold_h"].value / 1000.0; 78 | detector.classifier.threshold(1) = vars["threshold_s"].value / 1000.0; 79 | detector.classifier.threshold(2) = vars["threshold_v"].value / 1000.0; 80 | detector.classifier.threshold_merge(0) = vars["threshold_h_merge"].value / 1000.0; 81 | detector.classifier.threshold_merge(1) = vars["threshold_s_merge"].value / 1000.0; 82 | detector.classifier.threshold_merge(2) = vars["threshold_v_merge"].value / 1000.0; 83 | //detector.classifier.save_parameters(); 84 | } 85 | 86 | void fd::Configurator::create_trackbars(void) 87 | { 88 | for (auto& kv : vars) cv::createTrackbar(kv.first, "options", &kv.second.value, kv.second.max); 89 | } 90 | -------------------------------------------------------------------------------- /src/lib/segmenter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "segmenter.h" 4 | #include "disjoint_set.h" 5 | namespace fd = floordetection; 6 | 7 | /* edge */ 8 | typedef struct { 9 | float w; 10 | int a, b; 11 | } edge; 12 | 13 | bool operator<(const edge &a, const edge &b) { 14 | return a.w < b.w; 15 | } 16 | 17 | /* segmenter */ 18 | 19 | fd::Segmenter::Segmenter(ros::NodeHandle& _ros_node) : 20 | ros_node(_ros_node) 21 | { 22 | ros_node.param("segmenter_threshold", c_threshold, 0.5); 23 | ros_node.param("min_segment", min_segment, 500); 24 | } 25 | 26 | void fd::Segmenter::save_parameters(void) 27 | { 28 | ros_node.setParam("segmenter_threshold", c_threshold); 29 | ros_node.setParam("min_segment", min_segment); 30 | } 31 | 32 | void fd::Segmenter::segment(const cv::Mat& input, cv::Mat& output) 33 | { 34 | int width = input.size().width; 35 | int height = input.size().height; 36 | 37 | /* build graph */ 38 | std::vector edges(width*height*4); 39 | int num_edges = 0; 40 | for (int y = 0; y < height; y++) { 41 | for (int x = 0; x < width; x++) { 42 | if (x < width-1) { 43 | edges[num_edges].a = y * width + x; 44 | edges[num_edges].b = y * width + (x+1); 45 | edges[num_edges].w = cv::norm(input.at(y,x), input.at(y,x+1)); 46 | num_edges++; 47 | } 48 | if (y < height-1) { 49 | edges[num_edges].a = y * width + x; 50 | edges[num_edges].b = (y+1) * width + x; 51 | edges[num_edges].w = cv::norm(input.at(y,x), input.at(y+1,x)); 52 | num_edges++; 53 | } 54 | if ((x < width-1) && (y < height-1)) { 55 | edges[num_edges].a = y * width + x; 56 | edges[num_edges].b = (y+1) * width + (x+1); 57 | edges[num_edges].w = cv::norm(input.at(y,x), input.at(y+1,x+1)); 58 | num_edges++; 59 | } 60 | if ((x < width-1) && (y > 0)) { 61 | edges[num_edges].a = y * width + x; 62 | edges[num_edges].b = (y-1) * width + (x+1); 63 | edges[num_edges].w = cv::norm(input.at(y,x), input.at(y-1,x+1)); 64 | num_edges++; 65 | } 66 | } 67 | } 68 | 69 | /* segment graph */ 70 | std::sort(edges.begin(), edges.end()); // sort edges by weight 71 | 72 | int num_vertices = width * height; 73 | universe u(num_vertices); // make a disjoint-set forest 74 | std::vector threshold(num_vertices, c_threshold); // // init thresholds = c / 1 75 | 76 | // for each edge, in non-decreasing weight order... 77 | for (int i = 0; i < num_edges; i++) { 78 | edge* pedge = &edges[i]; 79 | 80 | // components conected by this edge 81 | int a = u.find(pedge->a); 82 | int b = u.find(pedge->b); 83 | if (a != b) { 84 | if ((pedge->w <= threshold[a]) && (pedge->w <= threshold[b])) { 85 | u.join(a, b); 86 | a = u.find(a); 87 | threshold[a] = pedge->w + (c_threshold / u.size(a)); // w + threshold 88 | } 89 | } 90 | } 91 | 92 | // post process small components 93 | for (int i = 0; i < num_edges; i++) { 94 | int a = u.find(edges[i].a); 95 | int b = u.find(edges[i].b); 96 | if ((a != b) && ((u.size(a) < min_segment) || (u.size(b) < min_segment))) 97 | u.join(a, b); 98 | } 99 | 100 | // create labels 101 | output.create(input.rows, input.cols, CV_32S); 102 | int* output_ptr = output.ptr(0); 103 | for (int y = 0; y < height; y++) { 104 | for (int x = 0; x < width; x++, output_ptr++) { 105 | int comp = u.find((y * width) + x); 106 | u.get_elt(comp)->total_pixels++; 107 | *output_ptr = comp; 108 | } 109 | } 110 | } 111 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(floordetection) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wfatal-errors -O4 -march=native") 5 | add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS cv_bridge geometry_msgs universal_teleop nav_msgs roscpp std_msgs genmsg actionlib_msgs actionlib message_generation) 11 | 12 | find_package(OpenCV REQUIRED) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ####################################### 24 | ## Declare ROS messages and services ## 25 | ####################################### 26 | 27 | ## Generate messages in the 'msg' folder 28 | # add_message_files( 29 | # FILES 30 | # Message1.msg 31 | # Message2.msg 32 | # ) 33 | 34 | ## Generate services in the 'srv' folder 35 | # add_service_files( 36 | # FILES 37 | # Service1.srv 38 | # Service2.srv 39 | # ) 40 | 41 | ## Generate added messages and services with any dependencies listed here 42 | # generate_messages( 43 | # DEPENDENCIES 44 | # geometry_msgs# nav_msgs# std_msgs 45 | # ) 46 | 47 | add_action_files(DIRECTORY action FILES FollowPath.action) 48 | generate_messages(DEPENDENCIES std_msgs actionlib_msgs) 49 | 50 | ################################### 51 | ## catkin specific configuration ## 52 | ################################### 53 | ## The catkin_package macro generates cmake config files for your package 54 | ## Declare things to be passed to dependent projects 55 | ## LIBRARIES: libraries you create in this project that dependent projects also need 56 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 57 | ## DEPENDS: system dependencies of this project that dependent projects also need 58 | catkin_package( 59 | # INCLUDE_DIRS include 60 | # LIBRARIES floordetection 61 | CATKIN_DEPENDS cv_bridge geometry_msgs nav_msgs roscpp std_msgs universal_teleop 62 | # DEPENDS system_lib 63 | ) 64 | 65 | ########### 66 | ## Build ## 67 | ########### 68 | 69 | ## Specify additional locations of header files 70 | ## Your package locations should be listed before other locations 71 | include_directories(include 72 | ${CMAKE_SOURCE_DIRECTORY}/src/lib 73 | ${catkin_INCLUDE_DIRS} 74 | ) 75 | 76 | ## Declare a cpp library 77 | file(GLOB LIB_SOURCES src/lib/*.cpp) 78 | add_library(floordetection ${LIB_SOURCES}) 79 | 80 | target_link_libraries(floordetection 81 | ${OpenCV_LIBRARIES} 82 | ${catkin_LIBRARIES} 83 | ) 84 | 85 | ## Declare a cpp executable 86 | add_executable(follower src/follower.cpp) 87 | target_link_libraries(follower floordetection) 88 | add_dependencies(follower floordetection) 89 | 90 | add_executable(configurator src/configurator.cpp) 91 | target_link_libraries(configurator floordetection) 92 | add_dependencies(configurator floordetection) 93 | 94 | 95 | ############# 96 | ## Install ## 97 | ############# 98 | 99 | # all install targets should use catkin DESTINATION variables 100 | # See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html 101 | 102 | ## Mark executable scripts (Python etc.) for installation 103 | ## in contrast to setup.py, you can choose the destination 104 | # install(PROGRAMS 105 | # scripts/my_python_script 106 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 107 | # ) 108 | 109 | ## Mark executables and/or libraries for installation 110 | # install(TARGETS floordetection floordetection_node 111 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 112 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 113 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 114 | # ) 115 | 116 | ## Mark cpp header files for installation 117 | # install(DIRECTORY include/${PROJECT_NAME}/ 118 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 119 | # FILES_MATCHING PATTERN "*.h" 120 | # PATTERN ".svn" EXCLUDE 121 | # ) 122 | 123 | ## Mark other files for installation (e.g. launch and bag files, etc.) 124 | # install(FILES 125 | # # myfile1 126 | # # myfile2 127 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 128 | # ) 129 | 130 | ############# 131 | ## Testing ## 132 | ############# 133 | 134 | ## Add gtest based cpp test target and link libraries 135 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_floordetection.cpp) 136 | # if(TARGET ${PROJECT_NAME}-test) 137 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 138 | # endif() 139 | 140 | ## Add folders to be run by python nosetests 141 | # catkin_add_nosetests(test) 142 | -------------------------------------------------------------------------------- /src/lib/path_follower.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "path_follower.h" 3 | namespace fd = floordetection; 4 | 5 | fd::PathFollower::PathFollower(ros::NodeHandle& ros_node) : detector(ros_node, true), robot(ros_node) 6 | { 7 | has_last_odometry = false; 8 | started = false; 9 | image_sub = ros_node.subscribe("/camera/image_raw", 1, &PathFollower::on_image, this); 10 | event_sub = ros_node.subscribe("/universal_teleop/events", 1, &PathFollower::on_event, this); 11 | odo_sub = ros_node.subscribe("/robot/pose", 1, &PathFollower::on_odometry, this); 12 | accum_xspeed = accum_aspeed = 0; 13 | cv::namedWindow("output"); 14 | 15 | ros_node.param("xspeed_scale", xspeed_scale, 1.0); 16 | ros_node.param("aspeed_scale", aspeed_scale, 1.0); 17 | 18 | start_service = ros_node.advertiseService("start", &PathFollower::start_request, this); 19 | stop_service = ros_node.advertiseService("stop", &PathFollower::stop_request, this); 20 | 21 | action_server = boost::shared_ptr(new ActionServer(ros_node, "follow_path", false)); 22 | action_server->registerGoalCallback(boost::bind(&PathFollower::new_goal, this)); 23 | //action_server.registerPreemptCallback(boost::bind(&PathFollower::cancel, this)); 24 | action_server->start(); 25 | } 26 | 27 | void fd::PathFollower::new_goal(void) 28 | { 29 | if (!action_server->isActive() && started) return; // service running 30 | 31 | current_goal = *action_server->acceptNewGoal(); 32 | ROS_INFO_STREAM("started new goal: " << current_goal.distance); 33 | started = true; 34 | action_feedback.distance_traveled = 0; 35 | } 36 | 37 | bool fd::PathFollower::start_request(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) 38 | { 39 | if (action_server->isActive() || started) return false; 40 | started = true; 41 | return true; 42 | } 43 | 44 | bool fd::PathFollower::stop_request(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) 45 | { 46 | if (action_server->isActive() || !started) return false; 47 | started = false; 48 | return true; 49 | } 50 | 51 | void fd::PathFollower::on_odometry(const nav_msgs::Odometry::ConstPtr& msg) 52 | { 53 | ROS_INFO_STREAM("odo " << action_server->isActive()); 54 | if (has_last_odometry && action_server->isActive()) { 55 | const geometry_msgs::Point& current_position = msg->pose.pose.position; 56 | const geometry_msgs::Point& last_position = last_odometry.pose.pose.position; 57 | float delta_d = hypot(current_position.x - last_position.x, current_position.y - last_position.y); 58 | action_feedback.distance_traveled += delta_d; 59 | if (action_feedback.distance_traveled >= current_goal.distance) { action_server->setSucceeded(); started = false; } 60 | ROS_INFO_STREAM("delta: " << action_feedback.distance_traveled); 61 | } 62 | has_last_odometry = true; 63 | last_odometry = *msg; 64 | } 65 | 66 | void fd::PathFollower::on_image(const sensor_msgs::Image::ConstPtr& msg) 67 | { 68 | if (started) { 69 | cv_bridge::CvImageConstPtr bridge_ptr = cv_bridge::toCvShare(msg, "rgb8"); 70 | follow(bridge_ptr->image); 71 | } 72 | } 73 | 74 | void fd::PathFollower::on_event(const universal_teleop::Event::ConstPtr& msg) 75 | { 76 | if (!msg->state) return; 77 | if (!started && msg->event == "start") { 78 | ROS_INFO_STREAM("starting to follow path"); 79 | started = true; 80 | } 81 | else if (started && msg->event == "stop") { 82 | ROS_INFO_STREAM("stopping path following"); 83 | started = false; 84 | } 85 | } 86 | 87 | void fd::PathFollower::follow(const cv::Mat& input) 88 | { 89 | std::vector frontier; 90 | int horizon_level; 91 | cv::Rect roi; 92 | detector.detect(input, frontier, horizon_level, roi); 93 | compute_control(frontier, input, horizon_level, roi); 94 | } 95 | 96 | void fd::PathFollower::compute_control(std::vector& frontier, const cv::Mat& input, int horizon_level, const cv::Rect& roi) 97 | { 98 | cv::Mat output; 99 | cv::cvtColor(input, output, CV_RGB2BGR); 100 | 101 | /* rasterize contour */ 102 | cv::Size cropped_size(input.size().width, input.size().height - horizon_level); 103 | cv::Mat raster_contour(cropped_size, CV_8UC1); 104 | raster_contour = cv::Scalar(0); 105 | cv::drawContours(raster_contour, std::vector< std::vector >(1, frontier), 0, cv::Scalar(255)); 106 | 107 | std::vector points_by_row(cropped_size.height, cv::Point(-1,-1)); 108 | uchar* raster_ptr = raster_contour.ptr(0); 109 | for (int i = 0; i < cropped_size.height; i++) { 110 | bool was_set = false; 111 | 112 | for (int j = 0; j < cropped_size.width; j++, raster_ptr++) { 113 | if (*raster_ptr == 255) { 114 | if (!was_set) { points_by_row[i] = cv::Point(j, j); was_set = true; } 115 | else { 116 | if (j > points_by_row[i].y) points_by_row[i].y = j; 117 | } 118 | } 119 | } 120 | } 121 | 122 | float xspeed = 0, aspeed = 0; 123 | int n_points = 0; 124 | for (uint i = 0; i < cropped_size.height; i++) { 125 | if (points_by_row[i].x == -1) continue; 126 | cv::Point minmax = points_by_row[i]; 127 | uint row = i; 128 | float x = (minmax.y + minmax.x) / 2; 129 | aspeed += x - cropped_size.width * 0.5; 130 | n_points++; 131 | 132 | cv::circle(output, cv::Point(x, row + horizon_level) , 1, cv::Scalar(0, 255, 255), 1, CV_AA); 133 | } 134 | 135 | float alpha = 0.00005, beta = 0.005; 136 | aspeed *= alpha; 137 | xspeed = beta * n_points - fabs(aspeed); 138 | if (xspeed < 0) xspeed = 0; 139 | if (xspeed > 0.3) xspeed = 0.3; 140 | 141 | float delta = 0.3; 142 | accum_aspeed = delta * accum_aspeed + (1 - delta) * aspeed; 143 | accum_xspeed = delta * accum_xspeed + (1 - delta) * xspeed; 144 | ROS_INFO_STREAM("xspeed: " << accum_xspeed << " aspeed: " << accum_aspeed); 145 | robot.set_speeds(xspeed_scale * accum_xspeed, aspeed_scale * accum_aspeed); 146 | 147 | size_t xorigin = 32; 148 | size_t xsize = 20; 149 | cv::putText(output, "^", cv::Point(1, 7), cv::FONT_HERSHEY_PLAIN, 0.7, cv::Scalar(0,0,0), 1, CV_AA); 150 | cv::rectangle(output, cv::Point(xorigin - xsize, 7), cv::Point(xorigin + xsize, 1), cv::Scalar(255,255,255), -1, 4); 151 | cv::rectangle(output, cv::Point(xorigin - xsize, 7), cv::Point(xorigin + xsize, 1), cv::Scalar(0,0,0), 1, 4); 152 | cv::rectangle(output, cv::Point(xorigin - xsize, 7), cv::Point((xorigin - xsize) + xspeed * 2 * xsize, 1), cv::Scalar(0, 255, 0), -1, 4); 153 | cv::rectangle(output, cv::Point(xorigin - xsize, 7), cv::Point((xorigin - xsize) + xspeed * 2 * xsize, 1), cv::Scalar(0, 0, 0), 1, 4); 154 | 155 | cv::putText(output, (aspeed < 0 ? "< " : "> "), cv::Point(1, 16), cv::FONT_HERSHEY_PLAIN, 0.7, cv::Scalar(0,0,0), 1, CV_AA); 156 | cv::rectangle(output, cv::Point(xorigin - xsize, 16), cv::Point(xorigin + xsize, 10), cv::Scalar(255,255,255), -1, 4); 157 | cv::rectangle(output, cv::Point(xorigin - xsize, 16), cv::Point(xorigin + xsize, 10), cv::Scalar(0,0,0), 1, 4); 158 | cv::rectangle(output, cv::Point(xorigin, 16), cv::Point(xorigin + aspeed * xsize, 10), cv::Scalar(0, 255, 0), -1, 4); 159 | cv::rectangle(output, cv::Point(xorigin, 16), cv::Point(xorigin + aspeed * xsize, 10), cv::Scalar(0, 0, 0), 1, 4); 160 | 161 | cv::Mat output_lower = output(cv::Range(horizon_level, output.rows), cv::Range::all()); 162 | cv::drawContours(output_lower, std::vector >(1, frontier), 0, cv::Scalar(255, 0, 0), 3, CV_AA); 163 | cv::rectangle(output_lower, roi, cv::Scalar(0, 255, 0), 2, CV_AA); 164 | cv::circle(output_lower, cv::Point(roi.x + roi.width/2, roi.y + roi.height / 2), 1, cv::Scalar(0, 255, 0), 2, CV_AA); 165 | cv::line(output_lower, cv::Point(0, horizon_level), cv::Point(output.cols, horizon_level), cv::Scalar(0,255,255), 1); 166 | 167 | cv::imshow("output", output); 168 | } 169 | -------------------------------------------------------------------------------- /src/lib/detector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "common.h" 4 | #include "detector.h" 5 | namespace fd = floordetection; 6 | 7 | fd::Detector::Detector(ros::NodeHandle& _ros_node, bool _show_windows) : 8 | ros_node(_ros_node), horizon_finder(_ros_node), segmenter(_ros_node), classifier(_ros_node) 9 | { 10 | show_windows = _show_windows; 11 | if (show_windows) { 12 | cvStartWindowThread(); 13 | cv::namedWindow("labeled"); 14 | cv::namedWindow("avg_hsv"); 15 | cv::namedWindow("avg_rgb"); 16 | cv::namedWindow("contour"); 17 | cv::namedWindow("classified"); 18 | cv::namedWindow("models"); 19 | } 20 | 21 | ros_node.param("median_blur", median_blur, 7); 22 | ros_node.param("erode_size", erode_size, 10); 23 | } 24 | 25 | void fd::Detector::save_parameters(void) 26 | { 27 | ros_node.setParam("median_blur", median_blur); 28 | ros_node.setParam("erode_size", erode_size); 29 | } 30 | 31 | void fd::Detector::detect(const cv::Mat& input, std::vector& frontier, int& horizon_level, cv::Rect& roi) 32 | { 33 | cv::Mat input_original; 34 | input.copyTo(input_original); 35 | 36 | /* blur image */ 37 | cv::Mat blured; 38 | cv::medianBlur(input, blured, median_blur); 39 | 40 | /* detect horizon */ 41 | horizon_level = horizon_finder.find(blured); 42 | ROS_INFO_STREAM("horizon: " << horizon_level << "/" << input.rows); 43 | cv::Mat input_cropped = input(cv::Range(horizon_level, input_original.rows - 1), cv::Range::all()); 44 | blured = blured(cv::Range(horizon_level, input_original.rows - 1), cv::Range::all()); 45 | 46 | /* convert input to HSV */ 47 | cv::Mat tmp, input_hsv; 48 | input_cropped.convertTo(tmp, CV_32FC3); 49 | tmp *= (1 / 255.0f); 50 | cv::cvtColor(tmp, input_hsv, CV_RGB2HSV); 51 | 52 | /* convert blured to HSV also */ 53 | cv::Mat blured_hsv; 54 | blured.convertTo(tmp, CV_32FC3); 55 | tmp *= (1 / 255.0f); 56 | cv::cvtColor(tmp, blured_hsv, CV_RGB2HSV); 57 | 58 | /* perform segmentation */ 59 | cv::Mat labeled; 60 | segmenter.segment(blured, labeled); 61 | 62 | /* build segment list */ 63 | std::vector label2segment; 64 | extract_segments(input_cropped, input_hsv, labeled, label2segment); 65 | 66 | /* classify segments */ 67 | cv::Mat classified; 68 | classifier.classify(blured/*input*/, blured_hsv/*input_hsv*/, labeled, label2segment, classified, roi); // TODO: use blured to classify? (change also detect_segments) 69 | 70 | /* compute road contour */ 71 | extract_contour(classified, frontier, roi); 72 | ROS_INFO_STREAM("frontier size: " << frontier.size()); 73 | 74 | if (show_windows) { 75 | cv::imshow("classified", classified); 76 | cv::imshow("models", classifier.training_model_colors); 77 | 78 | cv::Mat contour; 79 | cv::cvtColor(input, contour, CV_RGB2BGR); 80 | cv::line(contour, cv::Point(0, horizon_level), cv::Point(contour.cols, horizon_level), cv::Scalar(0, 255, 255), 1, CV_AA); 81 | cv::drawContours(contour, std::vector< std::vector >(1, frontier), 0, cv::Scalar(255, 255, 0), 1, CV_AA, cv::Mat(), INT_MAX, cv::Point(0, horizon_level)); 82 | cv::imshow("contour", contour); 83 | } 84 | 85 | /* step 5: robot control law */ 86 | //cv::Mat final_lower = final(cv::Range(horizon_level, final.rows), cv::Range::all()); 87 | //robot_controller.apply_control(frontier, final_lower, final); 88 | } 89 | 90 | void fd::Detector::extract_segments(const cv::Mat& input, const cv::Mat& input_hsv, cv::Mat& labeled, std::vector& label2segment) 91 | { 92 | int* labeled_ptr; 93 | uint size = input.rows * input.cols; 94 | int iterations = 0; 95 | 96 | /* rewrite labels for conitnuos numbering */ 97 | std::map label_equiv; 98 | labeled_ptr = labeled.ptr(0); 99 | int last_label = 0; 100 | for (uint i = 0; i < size; i++, labeled_ptr++) { 101 | int& l = *labeled_ptr; 102 | std::map::iterator it = label_equiv.find(l); 103 | if (it == label_equiv.end()) { label_equiv.insert(std::pair(l, last_label)); l = last_label; last_label++; } 104 | else { l = it->second; } 105 | } 106 | label2segment.resize(last_label); 107 | 108 | /* generate random color for each segment */ 109 | cv::RNG rng = cv::theRNG(); 110 | std::map colors; 111 | labeled_ptr = labeled.ptr(0); 112 | const cv::Vec3b* input_ptr = input.ptr(0); 113 | for (uint i = 0; i < size; i++, labeled_ptr++, input_ptr++) { 114 | int label = *labeled_ptr; 115 | label2segment[label].mu += cv::Vec3f((*input_ptr)(0) / 255.0f, (*input_ptr)(1) / 255.0f, (*input_ptr)(2) / 255.0f); 116 | label2segment[label].mass++; 117 | 118 | if (show_windows) { 119 | if (colors.find(label) == colors.end()) { 120 | colors[label] = cv::Vec3b(((int)rng % 16) * 16, ((int)rng % 16) * 16, ((int)rng % 16)*16); 121 | } 122 | } 123 | } 124 | //cout << "detected segments: " << label2segment.size() << " (in " << iterations << " iterations)" << endl; 125 | 126 | /* finish avg calculation */ 127 | for (int i = 0; i < label2segment.size(); i++) { 128 | Segment& s = label2segment[i]; 129 | int n = s.mass; 130 | s.mu *= 1 / (float)n; 131 | rgb2hsv(s.mu, s.mu_hsv); 132 | } 133 | 134 | /* compute covariance */ 135 | input_ptr = input.ptr(0); 136 | const cv::Vec3f* input_hsv_ptr = input_hsv.ptr(0); 137 | labeled_ptr = labeled.ptr(0); 138 | for (uint i = 0; i < size; i++, labeled_ptr++, input_ptr++, input_hsv_ptr++) { 139 | int label = *labeled_ptr; 140 | Segment& segment = label2segment[label]; 141 | 142 | const cv::Vec3b& input3b = *input_ptr; 143 | cv::Vec3f sample(input3b(0) / 255.0f, input3b(1) / 255.0f, input3b(2) / 255.0f); 144 | cv::Vec3f diff = (sample - segment.mu); 145 | segment.sigma += diff * diff.t(); 146 | 147 | cv::Vec3f diff_hsv = hsv_diff(*input_hsv_ptr, segment.mu_hsv); 148 | segment.sigma_hsv += diff_hsv * diff_hsv.t(); 149 | } 150 | 151 | /* finish cov calculation */ 152 | for (int i = 0; i < label2segment.size(); i++) { 153 | Segment& s = label2segment[i]; 154 | int n = s.mass; 155 | //cout << (1 / (float)(n - 1)) << endl; 156 | s.sigma *= 1 / (float)(n - 1); 157 | s.sigma_hsv *= 1 / (float)(n - 1); 158 | } 159 | 160 | // convert to hsv 161 | /*cv::cvtColor(cv::Mat(mu), cv::Mat(mu), CV_RGB2HSV); 162 | mu(0) /= 360.0f;*/ 163 | 164 | if (show_windows) { 165 | /* create colored images */ 166 | cv::Mat colored_labels(labeled.size(), CV_8UC3); 167 | cv::Mat avg_rgb(labeled.size(), CV_8UC3); 168 | cv::Mat avg_hsv(labeled.size(), CV_8UC3); 169 | cv::Vec3b* colored_labels_ptr = colored_labels.ptr(0); 170 | cv::Vec3b* avg_rgb_ptr = avg_rgb.ptr(0); 171 | cv::Vec3b* avg_hsv_ptr = avg_hsv.ptr(0); 172 | labeled_ptr = labeled.ptr(0); 173 | for (uint i = 0; i < size; i++, colored_labels_ptr++, avg_rgb_ptr++, avg_hsv_ptr++, labeled_ptr++) { 174 | int label = *labeled_ptr; 175 | *colored_labels_ptr = colors[label]; 176 | cv::Vec3f avg = label2segment[label].mu * 255.0f; 177 | *avg_rgb_ptr = cv::Vec3b((uchar)roundf(avg(0)), (uchar)roundf(avg(1)), (uchar)roundf(avg(2))); 178 | avg = label2segment[label].mu_hsv; avg(0) /= 360.0; avg *= 180.0f; 179 | *avg_hsv_ptr = cv::Vec3b((uchar)roundf(avg(0)), (uchar)roundf(avg(1)), (uchar)roundf(avg(2))); 180 | } 181 | cv::Mat debug1,debug2, debug3; 182 | cv::cvtColor(colored_labels, debug1, CV_RGB2BGR); 183 | cv::imshow("labeled", debug1); 184 | cv::cvtColor(avg_rgb, debug2, CV_RGB2BGR); 185 | cv::imshow("avg_rgb", debug2); 186 | /*cv::cvtColor(avg_hsv, debug3, CV_RGB2BGR); 187 | cv::imshow("avg_hsv", avg_hsv);*/ 188 | } 189 | } 190 | 191 | void fd::Detector::extract_contour(const cv::Mat& classified, std::vector& frontier, const cv::Rect& roi) 192 | { 193 | /* find best contour */ 194 | cv::Mat tmp; 195 | classified.copyTo(tmp); // need to copy, findContour overwrites input 196 | std::vector< std::vector > contours; 197 | cv::findContours(tmp, contours, CV_RETR_TREE, CV_CHAIN_APPROX_NONE); 198 | 199 | // find contour intersecting square's middle point 200 | size_t winner = contours.size(); 201 | size_t max_size = 0; 202 | cv::Point center_point(roi.x + roi.width/2, roi.y + roi.height / 2); 203 | for (uint i = 0; i < contours.size(); i++) { 204 | size_t this_size = contours[i].size(); 205 | if (this_size > max_size) { max_size = this_size; winner = i; } 206 | } 207 | //cout << "winner: " << winner << endl; 208 | 209 | cv::Mat eroded(classified.size(), CV_8UC1); 210 | if (winner != contours.size()) { 211 | if (erode_size > 0) { 212 | // re-create winner contour by drawing a mask 213 | cv::Mat classified_best(classified.size(), CV_8UC1); classified_best = cv::Scalar(0); 214 | cv::drawContours(classified_best, contours, winner, cv::Scalar(255), -1); 215 | 216 | // erode (grow black area) winner contour 217 | cv::Mat kernel(erode_size, erode_size, CV_8UC1); 218 | cv::circle(kernel, cv::Point((float)erode_size/2, (float)erode_size/2), (float)erode_size/2, cv::Scalar(255), -1); 219 | cv::morphologyEx(classified_best, eroded, cv::MORPH_OPEN, kernel); 220 | eroded.copyTo(tmp); 221 | 222 | // obtain new contours after eroding 223 | cv::findContours(tmp, contours, CV_RETR_TREE, CV_CHAIN_APPROX_NONE); 224 | 225 | max_size = 0; 226 | winner = contours.size(); 227 | 228 | for (uint i = 0; i < contours.size(); i++) { 229 | size_t this_size = contours[i].size(); 230 | if (this_size > max_size) { max_size = this_size; winner = i; } 231 | } 232 | //cout << "winner: " << winner << endl; 233 | } 234 | if (winner != contours.size()) frontier = contours[winner]; 235 | } 236 | 237 | //final.create(input_original.size(), CV_8UC3); 238 | } 239 | 240 | cv::Vec3f fd::hsv_diff(const cv::Vec3f& v1, const cv::Vec3f& v2) { 241 | cv::Vec3f res = (v1 - v2); 242 | if (res(0) > 180.0f) res(0) -= 360.0f; 243 | else if (res(0) < -180.0f) res(0) += 360.0f; 244 | return res; 245 | } 246 | 247 | void fd::rgb2hsv(const cv::Vec3f& rgb, cv::Vec3f& hsv) { 248 | cv::Vec3f tmp = rgb; 249 | cv::Mat_ rgb_mat(1, 1, &tmp); 250 | cv::Mat_ hsv_mat(1, 1, &hsv); 251 | cv::cvtColor(rgb_mat, hsv_mat, CV_RGB2HSV); 252 | } 253 | -------------------------------------------------------------------------------- /src/lib/classifier.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "classifier.h" 5 | #include "common.h" 6 | namespace fd = floordetection; 7 | 8 | fd::Classifier::Classifier(ros::NodeHandle& _ros_node) : 9 | ros_node(_ros_node) 10 | { 11 | ros_node.param("rectangle_width", rectangle_width, 0.5); 12 | ros_node.param("rectangle_height", rectangle_height, 0.2); 13 | ros_node.param("classifier_models", models, 1); 14 | 15 | std::vector t; 16 | ros_node.param("classifier_threshold_merge", t, std::vector(3, 0.015)); 17 | for (int i = 0; i < 3; i++) threshold_merge(i) = t[i]; 18 | ros_node.param("classifier_threshold", t, std::vector(3, 0.015)); 19 | for (int i = 0; i < 3; i++) threshold(i) = t[i]; 20 | } 21 | 22 | void fd::Classifier::save_parameters(void) 23 | { 24 | ros_node.setParam("rectangle_width", rectangle_width); 25 | ros_node.setParam("rectangle_height", rectangle_height); 26 | ros_node.setParam("classifier_models", models); 27 | 28 | std::vector t(3); 29 | for (int i = 0; i < 3; i++) t[i] = threshold_merge(i); 30 | ros_node.setParam("classifier_threshold_merge", t); 31 | for (int i = 0; i < 3; i++) t[i] = threshold(i); 32 | ros_node.setParam("classifier_threshold", t); 33 | } 34 | 35 | void fd::Classifier::classify(const cv::Mat& input, const cv::Mat& input_hsv, const cv::Mat& labeled, 36 | std::vector& label2segment, cv::Mat& output, cv::Rect& roi) 37 | { 38 | /* determine size of rectangle */ 39 | int rectangle_width_px = input.size().width * rectangle_width; 40 | int rectangle_height_px = input.size().height * rectangle_height; 41 | int rectangle_x = (input.size().width / 2) - (rectangle_width_px / 2); 42 | int rectangle_y = input.size().height - rectangle_height_px; 43 | //cout << "w/h: " << rectangle_width_px << " " << rectangle_height_px << endl; 44 | roi = cv::Rect(rectangle_x, rectangle_y, rectangle_width_px, rectangle_height_px); 45 | cv::Mat input_subregion = input(roi); 46 | //cout << "window size: " << input_subregion.rows * input_subregion.cols << endl; 47 | 48 | /* extract floor segments */ 49 | std::list training_models; 50 | extract_training_models(roi, labeled, label2segment, training_models); 51 | 52 | /* update learnt models */ 53 | update_learnt_models(training_models); 54 | 55 | /* classify all segments */ 56 | classify_segments(label2segment, input_subregion.rows * input_subregion.cols); 57 | 58 | /* create binary mask */ 59 | const int* labeled_ptr; 60 | output.create(input.size(), CV_8UC1); 61 | uchar* output_ptr = output.ptr(0); 62 | labeled_ptr = labeled.ptr(0); 63 | for (int k = 0; k < output.rows * output.cols; k++, labeled_ptr++, output_ptr++) { 64 | Segment& s = label2segment[*labeled_ptr]; 65 | if (s.floor_certainty == 1) *output_ptr = 255; 66 | else *output_ptr = 0; 67 | } 68 | } 69 | 70 | void fd::Classifier::extract_training_models(const cv::Rect& roi, const cv::Mat& labeled, 71 | const std::vector& label2segment, std::list& training_models) 72 | { 73 | training_models.clear(); 74 | 75 | // 1. compute ammount of overlapping of each segment over the floor region 76 | std::map overlaps; 77 | cv::Mat floor_labeled = labeled(roi); 78 | int floor_size = floor_labeled.rows * floor_labeled.cols; 79 | for (int i = 0; i < floor_labeled.rows; i++) { 80 | int* label_ptr = floor_labeled.ptr(i); 81 | for (int j = 0; j < floor_labeled.cols; j++, label_ptr++) { 82 | int l = *label_ptr; 83 | std::map::iterator it = overlaps.find(l); 84 | if (it == overlaps.end()) overlaps.insert(std::pair(l, 1)); 85 | else it->second++; 86 | } 87 | } 88 | 89 | // 2. load overlapping segments into trainig_models set 90 | int mass_sum = 0; 91 | for (std::map::iterator it = overlaps.begin(); it != overlaps.end(); ++it) { 92 | training_models.push_back(label2segment[it->first]); 93 | training_models.back().mass = it->second; 94 | mass_sum += it->second; 95 | //cout << "over: " << training_models.back().mass << endl; 96 | } 97 | //cout << "#floor segments:" << overlaps.size() << endl; 98 | //cout << "mass sum: " << mass_sum << " " << floor_size << endl; 99 | 100 | // 3. combine similar segments to form minimal group 101 | bool changed; 102 | do { 103 | changed = false; 104 | 105 | // 3.1. compute nearest neighbors 106 | size_t model_count = training_models.size(); 107 | std::vector nearest(model_count, -1); 108 | std::vector::iterator> nearest_it(model_count, training_models.end()); 109 | int i = 0; 110 | for (std::list::iterator it = training_models.begin(); it != training_models.end(); ++it, ++i) { 111 | float min_dist = FLT_MAX; 112 | int j = 0; 113 | for (std::list::iterator it2 = training_models.begin(); it2 != training_models.end(); ++it2, ++j) { 114 | if (i == j) continue; 115 | float d = it->distance(*it2, threshold_merge); 116 | if (d <= 1 && d < min_dist) { nearest[i] = j; min_dist = d; nearest_it[i] = it2; } 117 | } 118 | } 119 | 120 | // 3.2. join corresponding pairs 121 | std::list new_list; 122 | i = 0; 123 | for (std::list::iterator it = training_models.begin(); it != training_models.end(); ++it, ++i) { 124 | int nearest_i = nearest[i]; 125 | if (nearest_i == -1) new_list.push_back(*it); 126 | else if (i == nearest[nearest_i]) { 127 | if (nearest_i > i) { 128 | it->mix(*nearest_it[i]); 129 | new_list.push_back(*it); 130 | changed = true; 131 | } 132 | } 133 | else new_list.push_back(*it); 134 | } 135 | training_models = new_list; 136 | } while (changed); 137 | 138 | //#if SAVE_GRAPHICS 139 | int rect_size = 32, i = 0; 140 | training_model_colors.create(50, 32 * training_models.size(), CV_32FC3); 141 | training_model_colors = cv::Scalar(1,1,1); 142 | for (std::list::iterator it = training_models.begin(); it != training_models.end(); ++it, ++i) { 143 | int coverage = (int)((it->mass / (float)floor_size) * 100); 144 | std::ostringstream ostr; ostr << coverage; 145 | cv::putText(training_model_colors, ostr.str(), cv::Point(i * rect_size + 5, 47), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(1,0,0), 1, CV_AA); 146 | cv::rectangle(training_model_colors, cv::Rect(i * rect_size, 0, rect_size, rect_size), cv::Scalar(it->mu), -1); 147 | 148 | } 149 | cv::cvtColor(training_model_colors, training_model_colors, CV_RGB2BGR); 150 | //#endif 151 | } 152 | 153 | void fd::Classifier::update_learnt_models(const std::list& training_models) 154 | { 155 | #if 0 156 | list new_models; 157 | for (std::list::const_iterator it = training_models.begin(); it != training_models.end(); ++it) { 158 | bool matched = false; 159 | for (std::list::iterator it2 = learnt_models.begin(); it2 != learnt_models.end(); ++it2) { 160 | if (it->distance(*it2, threshold_merge) <= 1) { matched = true; it2->mix(*it); break; } 161 | } 162 | if (!matched) new_models.push_back(*it); 163 | } 164 | 165 | learnt_models.insert(learnt_models.end(), new_models.begin(), new_models.end()); 166 | if (learnt_models.size() > models) learnt_models.resize(models); // TODO: size() is O(n) on some implementations*/ 167 | #else 168 | learnt_models = training_models; 169 | #endif 170 | } 171 | 172 | void fd::Classifier::classify_pixels(const cv::Mat& input_hsv, int floor_mass) 173 | { 174 | cv::Mat classified_pixels(input_hsv.size(), CV_8UC1); 175 | uchar* mask_ptr = classified_pixels.ptr(0); 176 | 177 | size_t pixels = input_hsv.rows * input_hsv.cols; 178 | const cv::Vec3f* pixel_ptr = input_hsv.ptr(0); 179 | for (int i = 0; i < pixels; i++, pixel_ptr++, mask_ptr++) { 180 | *mask_ptr = 0; 181 | for (std::list::iterator it = learnt_models.begin(); it != learnt_models.end(); ++it) { 182 | float coverage = it->mass / (float)floor_mass; 183 | //cout << "model coverage: " << coverage << endl; 184 | if (coverage < 0.3) continue; // small clusters are ignored 185 | float dist = it->distance(*pixel_ptr, threshold); 186 | //cout << "dist: " << dist << endl; 187 | if (dist <= 1) { *mask_ptr = 255; break; } 188 | } 189 | } 190 | //classified_pixels.copyTo(debug); 191 | int erode_size = erode_size; 192 | cv::Mat kernel(erode_size, erode_size, CV_8UC1); 193 | cv::circle(kernel, cv::Point((float)erode_size/2, (float)erode_size/2), (float)erode_size/2, cv::Scalar(255), -1); 194 | cv::morphologyEx(classified_pixels, debug, cv::MORPH_OPEN, kernel); 195 | } 196 | 197 | void fd::Classifier::classify_segments(std::vector& label2segment, int floor_mass) 198 | { 199 | for (int i = 0; i < label2segment.size(); i++) { 200 | Segment& s = label2segment[i]; 201 | for (std::list::const_iterator it = learnt_models.begin(); it != learnt_models.end(); ++it) { 202 | float coverage = it->mass / (float)floor_mass; 203 | //cout << "model coverage: " << coverage << endl; 204 | if (coverage < 0.3) continue; // small clusters are ignored 205 | float dist = s.distance(*it, threshold); 206 | //cout << "dist: " << dist << endl; 207 | if (dist <= 1) { s.floor_certainty = 1; break; } 208 | } 209 | } 210 | } 211 | 212 | 213 | 214 | void fd::Classifier::set_minimum_cov(cv::Matx33f& cov, const cv::Vec3f& minimums) 215 | { 216 | cv::Vec3f eigenvalues; 217 | cv::Matx33f eigenvectors; 218 | cv::eigen(cov, eigenvalues, eigenvectors); 219 | for (int i = 0; i < 3; i++) { 220 | if (eigenvalues(i) < (minimums(i) * (i == 0 ? 360 * 50 : 0.25))) eigenvalues(i) = minimums(i) * (i == 0 ? 360.0 * 50 : 0.25); 221 | } 222 | 223 | cov = eigenvectors * cv::Matx33f::diag(eigenvalues) * eigenvectors.t(); 224 | } 225 | 226 | 227 | /********** Segment **************/ 228 | fd::Segment::Segment(void) { 229 | mass = 0; 230 | mu = mu_hsv = cv::Vec3f(0,0,0); 231 | sigma = sigma_hsv = cv::Matx33f::zeros(); 232 | floor_certainty = 0; 233 | floor_pixels = 0; 234 | } 235 | 236 | float fd::Segment::distance(const cv::Vec3f& elem, const cv::Vec3f& minimums) const 237 | { 238 | cv::Matx33f new_sigma = sigma_hsv; 239 | Classifier::set_minimum_cov(new_sigma, minimums); 240 | 241 | cv::Vec3f diff = hsv_diff(mu_hsv, elem); 242 | cv::Vec result = (diff.t() * new_sigma.inv() * diff); 243 | return result(0); 244 | } 245 | 246 | float fd::Segment::distance(const Segment& other, const cv::Vec3f& minimums) const 247 | { 248 | cv::Matx33f sigma_sum = (sigma_hsv + other.sigma_hsv); 249 | Classifier::set_minimum_cov(sigma_sum, minimums); 250 | 251 | cv::Vec3f diff = hsv_diff(mu_hsv, other.mu_hsv); 252 | cv::Vec result = (diff.t() * sigma_sum.inv() * diff); 253 | 254 | /*cout << "diff: " << diff(0) << " " << diff(1) << " " << diff(2) << endl; 255 | cout << "sigma_sum2" << endl; 256 | cout << sigma_sum2(0,0) << "," << sigma_sum2(0,1) << "," << sigma_sum2(0,2) << endl; 257 | cout << sigma_sum2(1,0) << "," << sigma_sum2(1,1) << "," << sigma_sum2(1,2) << endl; 258 | cout << sigma_sum2(2,0) << "," << sigma_sum2(2,1) << "," << sigma_sum2(2,2) << endl;*/ 259 | /*cout << "floor mu/sample mu: " << mu_hsv(0) << "," << mu_hsv(1) << "," << mu_hsv(2) << " " << other.mu_hsv(0) << "," << other.mu_hsv(1) << "," << other.mu_hsv(2) << endl; 260 | cout << "floor sigma: " << sigma_hsv(0,0) << "," << sigma_hsv(0,1) << "," << sigma_hsv(0,2) << endl 261 | << sigma_hsv(1,0) << "," << sigma_hsv(1,1) << "," << sigma_hsv(1,2) << endl 262 | << sigma_hsv(2,0) << "," << sigma_hsv(2,1) << "," << sigma_hsv(2,2) << endl; 263 | cout << "sample sigma: " << other.sigma_hsv(0,0) << "," << other.sigma_hsv(0,1) << "," << other.sigma_hsv(0,2) << endl 264 | << other.sigma_hsv(1,0) << "," << other.sigma_hsv(1,1) << "," << other.sigma_hsv(1,2) << endl 265 | << other.sigma_hsv(2,0) << "," << other.sigma_hsv(2,1) << "," << other.sigma_hsv(2,2) << endl; 266 | cout << "inv sigma: " << sigma_sum(0,0) << "," << sigma_sum(0,1) << "," << sigma_sum(0,2) << endl 267 | << sigma_sum(1,0) << "," << sigma_sum(1,1) << "," << sigma_sum(1,2) << endl 268 | << sigma_sum(2,0) << "," << sigma_sum(2,1) << "," << sigma_sum(2,2) << endl; 269 | sigma_sum = sigma_sum.inv(); 270 | cout << "sigma: " << sigma_sum(0,0) << "," << sigma_sum(0,1) << "," << sigma_sum(0,2) << endl 271 | << sigma_sum(1,0) << "," << sigma_sum(1,1) << "," << sigma_sum(1,2) << endl 272 | << sigma_sum(2,0) << "," << sigma_sum(2,1) << "," << sigma_sum(2,2) << endl; 273 | cout << "similarity: " << result(0) << endl;*/ 274 | 275 | return result(0); 276 | 277 | /**** TODO: los umbrales / las varianzas son interdependientes (mas que nada: hue depende del resto). pensar tambien en thresholds con signo (porque puedo aceptar cosas mas claras pero no mas oscuras) ****/ 278 | } 279 | 280 | void fd::Segment::mix(const Segment& other, float alpha) 281 | { 282 | float alpha1, alpha2; 283 | if (alpha == -1) { 284 | alpha1 = (float)mass / (float)(mass + other.mass); 285 | alpha2 = (float)other.mass / (float)(mass + other.mass); 286 | mass += other.mass; 287 | } 288 | else { 289 | alpha1 = alpha; 290 | alpha2 = 1 - alpha; 291 | // mass not updated, this version is used for long-tearm learning 292 | } 293 | 294 | mu = (mu * alpha1 + other.mu * alpha2); 295 | rgb2hsv(mu, mu_hsv); 296 | sigma = (sigma * alpha1 + other.sigma * alpha2); 297 | sigma_hsv = (sigma_hsv * alpha1 + other.sigma_hsv * alpha2); 298 | } 299 | 300 | std::ostream& operator<<(std::ostream& out, const fd::Segment& s) { 301 | out << "mu " << s.mu(0) << " " << s.mu(1) << " " << s.mu(2) << std::endl; 302 | out << "mu hsv " << s.mu_hsv(0) << " " << s.mu_hsv(1) << " " << s.mu_hsv(2) << std::endl; 303 | out << "mass " << s.mass << std::endl; 304 | cv::Vec3f eigenvalues; 305 | cv::Matx33f eigenvectors; 306 | cv::eigen(s.sigma_hsv, eigenvalues, eigenvectors); 307 | out << "std2 " << eigenvalues(0) << " " << eigenvalues(1) << " " << eigenvalues(2) << std::endl; 308 | out << s.sigma_hsv(0,0) << " " << s.sigma_hsv(1,1) << " " << s.sigma_hsv(2,2); 309 | return out; 310 | } 311 | 312 | --------------------------------------------------------------------------------