From c547270a82c22fafc4e830e9b464acb253033c7e Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Thu, 15 Jun 2017 09:34:24 +0200 Subject: [PATCH 01/94] Initial commit --- CMakeLists.txt | 42 +- config/new_road_detection.yaml | 13 + .../new_road_detection.h | 143 ++++++ launch/new_road_detection.launch | 10 + msg/RoadLane.msg | 10 + package.xml | 8 +- src/new_road_detection.cpp | 449 ++++++++++++++++++ src/new_road_detection_node.cpp | 22 + 8 files changed, 694 insertions(+), 3 deletions(-) create mode 100644 config/new_road_detection.yaml create mode 100644 include/drive_ros_image_recognition/new_road_detection.h create mode 100644 launch/new_road_detection.launch create mode 100644 msg/RoadLane.msg create mode 100644 src/new_road_detection.cpp create mode 100644 src/new_road_detection_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3a0e46d..5c8888d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,11 +12,25 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs +#temp include to expedite transtition of new_road_detection -> todo: move to common message storage, not created yet + message_generation + geometry_msgs +) + +add_message_files( + FILES + RoadLane.msg + ) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs ) catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs + CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs message_runtime ) find_package(OpenCV REQUIRED) @@ -28,7 +42,7 @@ include_directories( ) ################################################################################ -# Line recognition node +# Line recognition node -> will leave this in for now, might be useful ################################################################################ add_executable(line_recognition_node src/line_recognition_node.cpp @@ -50,6 +64,30 @@ install(TARGETS line_recognition_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +################################################################################ +# New road detection (LMS port) +################################################################################ +# enable this to get debug publishing from the node +#add_definitions(-DDRAW_DEBUG=True) + +add_executable(new_road_detection + src/new_road_detection.cpp + src/new_road_detection_node.cpp + ) + +add_dependencies(new_road_detection ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(new_road_detection + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ) + +install(TARGETS new_road_detection + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + ################################################################################ # Warp image node ################################################################################ diff --git a/config/new_road_detection.yaml b/config/new_road_detection.yaml new file mode 100644 index 0000000..137bf33 --- /dev/null +++ b/config/new_road_detection.yaml @@ -0,0 +1,13 @@ +new_road_detection: + searchOffset: 0.15 + distanceBetweenSearchlines: 0.1 + minLineWidthMul: 0.5 + maxLineWidthMul: 2 + findBySobel: true + threshold: 50 + sobelThreshold: 50 + laneWidthOffsetInMeter: 0.1 + translateEnvironment: false + useWeights: false + renderDebugImage: false + threads: 4 diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h new file mode 100644 index 0000000..f643a74 --- /dev/null +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -0,0 +1,143 @@ +#ifndef NEW_ROAD_DETECTION_H +#define NEW_ROAD_DETECTION_H + +#include +#include +#include +#include +#include +//#include +//#include +//#include +//#include +//#include + +#include + +// for multithreading +#include +#include +#include + +typedef std::shared_ptr CvImagePtr; + +inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { + CvImagePtr cv_ptr; + try + { + // todo: check if this if we have 8 or 16 greyscale images + // hardcopies for now, might be possible to process on pointer if fast enough + cv_ptr = cv_bridge::toCvCopy(msg, "mono16"); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + if( !cv_ptr->data) + { + ROS_WARN("Empty image received, skipping!"); + return; + } + return cv_ptr; +} + +/** + * @brief Port of LMS module new_road_detection to ROS + **/ +class NewRoadDetection { + // configs + float searchOffset_; + float distanceBetweenSearchlines_; + bool findPointsBySobel_; + bool renderDebugImage_; + float minLineWidthMul_; + float maxLineWidthMul_; + int threshold_; + float laneWidthOffsetInMeter_; + bool translate_environment_; + bool useWeights_; + int sobelThreshold_; + int numThreads_; // 0 means single threaded + + // todo: port this +// lms::imaging::Homography homo; + //Datachannels + // todo: we have not determined all interfaces yet, so will leave this in for now until we have figured it out +// lms::ReadDataChannel image; +// lms::WriteDataChannel road; +// lms::WriteDataChannel output; +// lms::WriteDataChannel debugImage; +// lms::WriteDataChannel debugAllPoints; +// lms::WriteDataChannel debugValidPoints; +// lms::WriteDataChannel debugTranslatedPoints; +// lms::ReadDataChannel car; + + struct SearchLine{ + cv::Point2f w_start; + cv::Point2f w_end; + cv::Point i_start; + cv::Point i_end; + + + cv::Point2f w_left; + cv::Point2f w_mid; + cv::Point2f w_right; + }; + + std::list lines; + + std::mutex mutex; + std::mutex debugAllPointsMutex; + std::mutex debugValidPointsMutex; + std::vector threads; + std::condition_variable conditionNewLine; + std::condition_variable conditionLineProcessed; + bool threadsRunning; + int linesToProcess; + + ros::Subscriber image_sub_; + // road inputs and outputs + ros::Subscriber road_; + ros::Publisher output_; +#ifdef DRAW_DEBUG + ros::Publisher debug_img_pub_; +#endif + ros::NodeHandle nh_; + std::string my_namespace_; + +public: + bool init() override; + void destroy() override; + void imageCallback(const sensor_msgs::ImageConstPtr img_in); + void configsChanged() override; + + bool find(); + + std::vector + findBySobel(const bool renderDebugImage, + const std::vector &xv, + const std::vector &yv, + const float minLineWidthMul, + const float maxLineWidthMul, + const float iDist, + const float wDist, + const int threshold); + + std::vector + findByBrightness(const bool renderDebugImage, + const std::vector &xv, + const std::vector &yv, + const float minLineWidthMul, + const float maxLineWidthMul, + const float iDist, + const float wDist, + const int threshold); + + void processSearchLine(const SearchLine &line); + + void threadFunction(); +}; + +#endif // NEW_ROAD_DETECTION_H diff --git a/launch/new_road_detection.launch b/launch/new_road_detection.launch new file mode 100644 index 0000000..921f282 --- /dev/null +++ b/launch/new_road_detection.launch @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/msg/RoadLane.msg b/msg/RoadLane.msg new file mode 100644 index 0000000..9d20589 --- /dev/null +++ b/msg/RoadLane.msg @@ -0,0 +1,10 @@ +#Port of RoadLane from LMS: +Header header +# lane points +geometry_msgs/Point32[] points +# type of lane +uint8 UNKNOWN=0 +uint8 STRAIGHT=1 +uint8 STRAIGHT_CURVE=2 +uint8 CURVE = 3 +uint8 RoadStateType diff --git a/package.xml b/package.xml index 7417e5d..ede332c 100644 --- a/package.xml +++ b/package.xml @@ -45,12 +45,18 @@ roscpp sensor_msgs std_msgs + + message_generation + geometry_msgs + cv_bridge image_transport roscpp sensor_msgs std_msgs - + + message_runtime + geometry_msgs diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp new file mode 100644 index 0000000..c22d1fc --- /dev/null +++ b/src/new_road_detection.cpp @@ -0,0 +1,449 @@ +#include "drive_ros_image_recognition/new_road_detection.h" +//#include +//#include +//#include +//#include +//#include +//#include +//#include + +bool NewRoadDetection::NewRoadDetection(ros::NodeHandle nh): + nh_(nh), + image_sub_(), + my_namespace_("new_road_detection"), + searchOffset_(0.15), + distanceBetweenSearchlines_(0.1), + minLineWidthMul_(0.5), + maxLineWidthMul_(2), + findPointsBySobel_(true), + threshold_(50), + sobelThreshold_(50), + laneWidthOffsetInMeter_(0.1), + translateEnvironment_(false), + useWeights_(false), + renderDebugImage_(false), + numThreads_(4) +{ +} + +bool NewRoadDetection::init() { + // load parameters + if (!pnh_.getParam(my_namespace_+"searchOffset", searchOffset_)) { + ROS_WARN_STREAM("Unable to load 'searchOffset' parameter, using default: "<("ROAD"); + //output = writeChannel("ROAD_OUTPUT"); + +#ifdef DRAW_DEBUG + debug_img_pub_ = nh_.advertise("/debug_image_out", 10); +#endif + + // todo: we have not decided on an interface for these debug channels yet +// debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); +// debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); +// debugTranslatedPoints = writeChannel("DEBUG_VALID_TRANSLATED_POINTS"); + + // todo: we have not defined the interface for this yet +// car = readChannel("CAR"); //TODO create ego-estimation service + + return true; +} + +void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { + CvImagePtr img_conv = convertImageMessage(img_in); + + //if we have a road(?), try to find the line + // todo: no interface for this yet + if(road->points().size() > 1){ + find(); //TODO use bool from find + } + + float dx = 0; + float dy = 0; + float dPhi = 0; + + //update the course + // todo: no access to car command interface yet +// if(!translate_environment_){ +// logger.info("translation")<deltaPhi(); +// dPhi = car->deltaPhi(); +// } + + // todo: no interface for this yet -> ask Phibedy what this does +// lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); +// if(localCourse.isValid()){ +// logger.time("localCourse"); +// localCourse->update(dx,dy,dPhi); +// *road = localCourse->getCourse(); +// logger.timeEnd("localCourse"); +#ifdef DRAW_DEBUG + // todo: no interface for this yet +// if(renderDebugImage){ +// debugTranslatedPoints->points() = localCourse->getPointsAdded(); +// } +#endif + +// }else{ +// ROS_ERROR("LocalCourse invalid, aborting the callback!"); +// return; +// } + return; +} + +std::vector NewRoadDetection::findBySobel( + const bool renderDebugImage, + const std::vector &xv, + const std::vector &yv, + const float minLineWidthMul, + const float maxLineWidthMul, + const float iDist, + const float wDist, + const int threshold) { + return ::findBySobel(image.get(),debugImage.get(),renderDebugImage,xv,yv,minLineWidthMul,maxLineWidthMul,0.02,iDist,wDist,threshold,homo); +} +std::vector NewRoadDetection::findByBrightness(const bool renderDebugImage, const std::vector &xv,const std::vector &yv, const float minLineWidthMul, const float maxLineWidthMul,const float iDist,const float wDist, const int threshold){ + lms::imaging::BGRAImageGraphics graphics(*debugImage); + std::vector foundPoints; + std::vector color; + //get the color from the points + if(renderDebugImage) + graphics.setColor(lms::imaging::blue); + for(int k = 0; k < (int)xv.size(); k++){ + const int x = xv[k]; + const int y = yv[k]; + if(!image->inside(x,y)){ + color.push_back(0); + continue; + } + color.push_back(*(image->data()+x+y*image->width())); + if(renderDebugImage) + graphics.drawCross(x,y); + } + //detect peaks + float pxlPeakWidth = iDist/wDist*0.02; //TODO to bad, calculate for each road line (how should we use them for searching? + int tCounter = 0; + if(renderDebugImage) + graphics.setColor(lms::imaging::red); + for(int k = 0; k < (int)color.size(); k++){ + if(color[k]>threshold){ + tCounter++; + }else{ + if(tCounter - k != 0 && tCounter > pxlPeakWidth*minLineWidthMul && tCounter < pxlPeakWidth*maxLineWidthMul){ + if(renderDebugImage){ + for(int j = 0; jresize(image->width(),image->height(),lms::imaging::Format::BGRA); + debugImage->fill(0); + debugAllPoints->points().clear(); + debugValidPoints->points().clear(); + debugTranslatedPoints->points().clear(); + } + + //create left/mid/right lane + lms::math::polyLine2f mid = road->getWithDistanceBetweenPoints(config().get("distanceBetweenSearchlines",0.2)); + lms::math::polyLine2f left = mid.moveOrthogonal(-0.4); + lms::math::polyLine2f right = mid.moveOrthogonal(0.4); + if(mid.points().size() != left.points().size() || mid.points().size() != right.points().size()){ + logger.error("invalid midlane given!"); + return false; + } + //get all lines + for(int i = 0; i< (int)mid.points().size(); i++){ + SearchLine l; + l.w_start = left.points()[i]; + l.w_end = right.points()[i]; + l.w_left = left.points()[i]; + l.w_mid = mid.points()[i]; + l.w_right = right.points()[i]; + //check if the part is valid (middle should be always visible) + if(!image->inside(l.w_start.x,l.w_start.y)){ + l.w_start = mid.points()[i]; + if(!image->inside(l.w_end.x,l.w_end.y)){ + continue; + } + }else if(!image->inside(l.w_end.x,l.w_end.y)){ + l.w_end = mid.points()[i]; + } + + //transform them in image-coordinates + homo.vti(l.i_start,l.w_start); + homo.vti(l.i_end,l.w_end); + { + std::unique_lock lock(mutex); + lines.push_back(l); + linesToProcess ++; + conditionNewLine.notify_one(); + } + } + logger.timeEnd("create lines"); + + logger.time("search"); + if(numThreads == 0) { + // single threaded + for(SearchLine &l:lines){ + processSearchLine(l); + } + } else { + // multi threaded + + // initialize and start threads if not yet there + if(threads.size() == 0) { + threadsRunning = true; + for(int i = 0; i < numThreads; i++) { + threads.emplace_back([this] () { + threadFunction(); + }); + } + } + + // wait till every search line was processed + { + std::unique_lock lock(mutex); + while(linesToProcess > 0) conditionLineProcessed.wait(lock); + } + } + logger.timeEnd("search"); + return true; +} + +void NewRoadDetection::threadFunction() { + while(threadsRunning) { + SearchLine line; + { + std::unique_lock lock(mutex); + while(threadsRunning && lines.empty()) conditionNewLine.wait(lock); + if(lines.size() > 0) { + line = lines.front(); + lines.pop_front(); + } + if(! threadsRunning) { + break; + } + } + processSearchLine(line); + } +} + +void NewRoadDetection::processSearchLine(const SearchLine &l) { + std::vector xv; + std::vector yv; + + //calculate the offset + float iDist = l.i_start.distance(l.i_end); + float wDist = l.w_start.distance(l.w_end); + float pxlPerDist = iDist/wDist*searchOffset; + lms::math::vertex2f iDiff = lms::math::vertex2f(l.i_start-l.i_end).normalize(); + lms::math::vertex2f startLine = lms::math::vertex2f(l.i_start)+iDiff*pxlPerDist; + lms::math::vertex2f endLine = lms::math::vertex2f(l.i_end)-iDiff*pxlPerDist; + //get all points in between + lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! + + //find points + std::vector foundPoints; + if(findPointsBySobel){ + foundPoints = findBySobel(renderDebugImage,xv,yv,minLineWidthMul,maxLineWidthMul,iDist,wDist,sobelThreshold); + }else{ + foundPoints = findByBrightness(renderDebugImage,xv,yv,minLineWidthMul,maxLineWidthMul,iDist,wDist,threshold); + } + + if(renderDebugImage) { + std::unique_lock lock(debugAllPointsMutex); + for(lms::math::vertex2f &v:foundPoints) + debugAllPoints->points().push_back(v); + } + + //filter + //TODO filter points that are in poluted regions (for example the car itself) + //remove points with bad distances + if(foundPoints.size() > 2){ + std::vector foundCounter; + foundCounter.resize(foundPoints.size(),0); + for(std::size_t fp = 0; fp < foundPoints.size(); fp++){ + const lms::math::vertex2f &s = foundPoints[fp]; + for(std::size_t test = fp+1; test 0.4-laneWidthOffsetInMeter && distance < 0.4 + laneWidthOffsetInMeter)|| (distance > 0.8-laneWidthOffsetInMeter && distance < 0.8+laneWidthOffsetInMeter)){ + foundCounter[test]++; + foundCounter[fp]++; + } + } + } + //filter, all valid points should have the same counter and have the highest number + int max = 0; + for(int c:foundCounter){ + if(c > max){ + max = c; + } + } + std::vector validPoints; + for(int i = 0; i < (int)foundPoints.size(); i++){ + if(foundCounter[i] >= max){ + validPoints.push_back(foundPoints[i]); + } + } + //TODO if we have more than 3 points we know that there is an error! + foundPoints = validPoints; + } + + if(renderDebugImage){ + std::unique_lock lock(debugValidPointsMutex); + for(lms::math::vertex2f &v:foundPoints) + debugValidPoints->points().push_back(v); + } + + //Handle found points + lms::math::vertex2f diff; + std::vector distances; + for(int i = 0; i < (int)foundPoints.size(); i++){ + lms::math::vertex2f &wMind = foundPoints[i]; + float distanceToLeft = wMind.distance(l.w_left); + float distanceToRight= wMind.distance(l.w_right); + float distanceToMid= wMind.distance(l.w_mid); + diff = (l.w_left-l.w_right).normalize(); + if(distanceToLeft < distanceToMid && distanceToLeft < distanceToRight){ + //left point + wMind-=diff*0.4; + distances.push_back(distanceToLeft); + }else if(distanceToRight < distanceToMid ){ + wMind+=diff*0.4; + distances.push_back(distanceToRight); + }else{ + distances.push_back(distanceToMid); + } + } + + std::vector weights; + for(const float &dist:distances){ + //as distance is in meter, we multiply it by 100 + if(useWeights) + weights.push_back(1/(dist*100+0.001)); //TODO hier etwas sinnvolles überlegen + else + weights.push_back(1); + } + /* + if(renderDebugImage){ + for(lms::math::vertex2f &v:foundPoints) + debugTranslatedPoints->points().push_back(v); + } + */ + lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); + if(localCourse.isValid()){ + localCourse->addPoints(foundPoints,weights); + }else{ + logger.error("localCourse invalid!"); + } + + { + std::unique_lock lock(mutex); + linesToProcess --; + conditionLineProcessed.notify_all(); + } +} + +void NewRoadDetection::configsChanged(){ + lms::ServiceHandle warp = getService("WARP_SERVICE"); + if(!warp.isValid()){ + logger.error("WARP SERVICE is invalid!")<<"shutting down!"; + exit(0); //TODO shut down runtime + }else{ + homo = warp->getHomography(); + } + // read config + searchOffset = config().get("searchOffset",0.1); + findPointsBySobel = config().get("findBySobel",true); + renderDebugImage = config().get("renderDebugImage",false); + minLineWidthMul = config().get("minLineWidthMul",0.5); + maxLineWidthMul = config().get("maxLineWidthMul",1.5); + threshold = config().get("threshold",200); + laneWidthOffsetInMeter = config().get("laneWidthOffsetInMeter",0.1); + useWeights = config().get("useWeights",false); + sobelThreshold = config().get("sobelThreshold",200); + numThreads = config().get("threads",0); +} + +void NewRoadDetection::destroy() { + { + std::unique_lock lock(mutex); + threadsRunning = false; + conditionNewLine.notify_all(); + } + for(auto &t : threads) { + if(t.joinable()) { + t.join(); + } + } +} diff --git a/src/new_road_detection_node.cpp b/src/new_road_detection_node.cpp new file mode 100644 index 0000000..3cb4b49 --- /dev/null +++ b/src/new_road_detection_node.cpp @@ -0,0 +1,22 @@ +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "new_road_detection"); + ros::NodeHandle nh; + + drive_ros_image_recognition::NewRoadDetection new_road_detection = drive_ros_image_recognition::NewRoadDetection(nh); + if (!new_road_detection.init()) { + return 1; + } + +#ifndef NDEBUG + // give GDB time to attach + ros::Duration(1.0).sleep(); +#endif + + while (ros::ok()) { + ros::spin(); + } + return 0; +} From 5bc36ab383ebcdbecadcbd8fa0271a7de53288c2 Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Fri, 16 Jun 2017 10:33:39 +0200 Subject: [PATCH 02/94] Continued porting new_road_detection to ROS, added dynamic reconfigure for node, added first message definitions for interfacing with road lane calculation --- CMakeLists.txt | 7 +- cfg/NewRoadDetection.cfg | 21 ++ .../new_road_detection.h | 61 ++-- package.xml | 2 + src/new_road_detection.cpp | 287 ++++++++++++------ src/new_road_detection_node.cpp | 3 +- 6 files changed, 269 insertions(+), 112 deletions(-) create mode 100644 cfg/NewRoadDetection.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c8888d..aadcded 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs + dynamic_reconfigure #temp include to expedite transtition of new_road_detection -> todo: move to common message storage, not created yet message_generation geometry_msgs @@ -28,6 +29,10 @@ generate_messages( geometry_msgs ) +generate_dynamic_reconfigure_options( + cfg/NewRoadDetection.cfg +) + catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs message_runtime @@ -75,7 +80,7 @@ add_executable(new_road_detection src/new_road_detection_node.cpp ) -add_dependencies(new_road_detection ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(new_road_detection ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(new_road_detection ${catkin_LIBRARIES} diff --git a/cfg/NewRoadDetection.cfg b/cfg/NewRoadDetection.cfg new file mode 100644 index 0000000..1ef0a1c --- /dev/null +++ b/cfg/NewRoadDetection.cfg @@ -0,0 +1,21 @@ +#!/usr/bin/env python +PACKAGE = "drive_ros_image_recognition" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("searchOffset", double_t, 0, "Search offset", 0.15, 0.0, 2.0) +gen.add("distanceBetweenSearchlines", double_t, 0, "Distance between search lines", 0.1, 0.0, 1.0) +gen.add("minLineWidthMul", double_t, 0, "minLineWidthMul", 0.5, 0.0, 1.0) +gen.add("maxLineWidthMul", double_t, 0, "maxLineWidthMul", 2, 0.0, 5.0) +gen.add("findBySobel", bool_t, 0, "Use sobel", True) +gen.add("threshold", int_t, 0, "Threshold", 50, 0, 200) +gen.add("sobelThreshold", int_t, 0, "Sobel threshold", 50, 0, 200) +gen.add("laneWidthOffsetInMeter", double_t, 0, "Lane width offset (m)", 0.1, 0.0, 2.0) +gen.add("translateEnvironment", bool_t, 0, "Translate environment", False) +gen.add("useWeights", bool_t, 0, "Use weights", False) +gen.add("renderDebugImage", bool_t, 0, "Render debug image", False) +gen.add("threads", int_t, 0, "Number of threads", 4, 0, 8) + +exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "new_road_detection")) diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index f643a74..bdb0a7a 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -4,8 +4,11 @@ #include #include #include +#include #include #include +#include +#include //#include //#include //#include @@ -19,7 +22,7 @@ #include #include -typedef std::shared_ptr CvImagePtr; +typedef boost::shared_ptr CvImagePtr; inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { CvImagePtr cv_ptr; @@ -27,18 +30,18 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { // todo: check if this if we have 8 or 16 greyscale images // hardcopies for now, might be possible to process on pointer if fast enough - cv_ptr = cv_bridge::toCvCopy(msg, "mono16"); + cv_ptr = cv_bridge::toCvCopy(img_in, "mono16"); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); - return; + return NULL; } if( !cv_ptr->data) { ROS_WARN("Empty image received, skipping!"); - return; + return NULL; } return cv_ptr; } @@ -56,7 +59,7 @@ class NewRoadDetection { float maxLineWidthMul_; int threshold_; float laneWidthOffsetInMeter_; - bool translate_environment_; + bool translateEnvironment_; bool useWeights_; int sobelThreshold_; int numThreads_; // 0 means single threaded @@ -77,8 +80,8 @@ class NewRoadDetection { struct SearchLine{ cv::Point2f w_start; cv::Point2f w_end; - cv::Point i_start; - cv::Point i_end; + cv::Point2i i_start; + cv::Point2i i_end; cv::Point2f w_left; @@ -86,36 +89,46 @@ class NewRoadDetection { cv::Point2f w_right; }; - std::list lines; + std::list lines_; std::mutex mutex; std::mutex debugAllPointsMutex; std::mutex debugValidPointsMutex; - std::vector threads; - std::condition_variable conditionNewLine; - std::condition_variable conditionLineProcessed; - bool threadsRunning; - int linesToProcess; + std::vector threads_; + std::condition_variable conditionNewLine_; + std::condition_variable conditionLineProcessed_; + bool threadsRunning_; + int linesToProcess_; - ros::Subscriber image_sub_; + cv::Mat debug_image_; + + image_transport::ImageTransport it_; + image_transport::Subscriber img_sub_; // road inputs and outputs - ros::Subscriber road_; - ros::Publisher output_; + ros::Subscriber road_sub_; + ros::Publisher line_output_pub_; #ifdef DRAW_DEBUG - ros::Publisher debug_img_pub_; + image_transport::Publisher debug_img_pub_; #endif ros::NodeHandle nh_; + ros::NodeHandle pnh_; std::string my_namespace_; + drive_ros_image_recognition::RoadLane road_buffer_; -public: - bool init() override; - void destroy() override; - void imageCallback(const sensor_msgs::ImageConstPtr img_in); - void configsChanged() override; + void roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_); + void imageCallback(const sensor_msgs::ImageConstPtr& img_in); - bool find(); + dynamic_reconfigure::Server dsrv_server_; + dynamic_reconfigure::Server::CallbackType dsrv_cb_; + void reconfigureCB(drive_ros_image_recognition::new_road_detectionConfig& config, uint32_t level); + bool find(CvImagePtr& incoming_image); - std::vector +public: + NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh); + ~NewRoadDetection(); + bool init(); + + std::vector findBySobel(const bool renderDebugImage, const std::vector &xv, const std::vector &yv, diff --git a/package.xml b/package.xml index ede332c..e16459e 100644 --- a/package.xml +++ b/package.xml @@ -45,6 +45,7 @@ roscpp sensor_msgs std_msgs + dynamic_reconfigure message_generation geometry_msgs @@ -54,6 +55,7 @@ roscpp sensor_msgs std_msgs + dynamic_reconfigure message_runtime geometry_msgs diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index c22d1fc..056b76f 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -7,9 +7,10 @@ //#include //#include -bool NewRoadDetection::NewRoadDetection(ros::NodeHandle nh): +NewRoadDetection::NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh): nh_(nh), - image_sub_(), + pnh_(pnh), + img_sub_(), my_namespace_("new_road_detection"), searchOffset_(0.15), distanceBetweenSearchlines_(0.1), @@ -22,7 +23,19 @@ bool NewRoadDetection::NewRoadDetection(ros::NodeHandle nh): translateEnvironment_(false), useWeights_(false), renderDebugImage_(false), - numThreads_(4) + numThreads_(4), + road_sub_(), + debug_image_(0,0,CV_16UC1), + line_output_pub_(), + it_(pnh), + dsrv_server_(), + dsrv_cb_(boost::bind(&NewRoadDetection::reconfigureCB, this, _1, _2)), + lines_(), + threadsRunning_(false), + linesToProcess_(0), + threads_(), + conditionNewLine_(), + conditionLineProcessed_() { } @@ -76,10 +89,11 @@ bool NewRoadDetection::init() { ROS_WARN_STREAM("Unable to load 'threads' parameter, using default: "<("ROAD"); - //output = writeChannel("ROAD_OUTPUT"); + dsrv_server_.setCallback(dsrv_cb_); + + img_sub_ = it_.subscribe("image", 10, &NewRoadDetection::imageCallback, this); + road_sub_ = nh_.subscribe("road_in", 100, &NewRoadDetection::roadCallback, this); + line_output_pub_ = nh_.advertise("line_out",10); #ifdef DRAW_DEBUG debug_img_pub_ = nh_.advertise("/debug_image_out", 10); @@ -96,13 +110,23 @@ bool NewRoadDetection::init() { return true; } -void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { +void NewRoadDetection::roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_) { + // use simple singular buffer + road_buffer_ = *road_in_; +} + +void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr& img_in) { CvImagePtr img_conv = convertImageMessage(img_in); + // todo: adjust this time value + if (img_in->header.stamp.toSec() - road_buffer_.header.stamp.toSec() > 0.1) { + ROS_WARN("Outdated road callback in buffer, skipping incoming image"); + return; + } + //if we have a road(?), try to find the line - // todo: no interface for this yet - if(road->points().size() > 1){ - find(); //TODO use bool from find + if(road_buffer_.points.size() > 1){ + find(img_conv); //TODO use bool from find } float dx = 0; @@ -116,7 +140,7 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { // dPhi = car->deltaPhi(); // } - // todo: no interface for this yet -> ask Phibedy what this does + // todo: no interface for this yet, create course verification service to verfiy and publish // lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); // if(localCourse.isValid()){ // logger.time("localCourse"); @@ -129,6 +153,8 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { // debugTranslatedPoints->points() = localCourse->getPointsAdded(); // } #endif + // should do something like this +// line_output_pub_.publish(road_translated); // }else{ // ROS_ERROR("LocalCourse invalid, aborting the callback!"); @@ -137,17 +163,113 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { return; } -std::vector NewRoadDetection::findBySobel( - const bool renderDebugImage, +std::vector NewRoadDetection::findBySobel(const bool renderDebugImage, const std::vector &xv, const std::vector &yv, const float minLineWidthMul, const float maxLineWidthMul, const float iDist, const float wDist, - const int threshold) { - return ::findBySobel(image.get(),debugImage.get(),renderDebugImage,xv,yv,minLineWidthMul,maxLineWidthMul,0.02,iDist,wDist,threshold,homo); + const int threshold) +{ + // return ::findBySobel(image.get(),debugImage.get(),renderDebugImage,xv,yv,minLineWidthMul,maxLineWidthMul,0.02,iDist,wDist,threshold,homo); + + // todo: port this here + // inline std::vector findBySobel( +// const lms::imaging::Image *image, +// lms::imaging::Image *debugImage, +// const bool renderDebugImage, +// const std::vector &xv, +// const std::vector &yv, +// const float minLineWidthMul, +// const float maxLineWidthMul, +// const float lineWidth, +// const float iDist, +// const float wDist, +// const int threshold, +// lms::imaging::Transformation &trans) { +// //lms::logging::Logger logger("findBySobel"); + +// lms::imaging::BGRAImageGraphics graphics(*debugImage); +// std::vector foundPoints; +// bool foundLowHigh = false; +// int pxlCounter = 0; +// for(int k = 1; k < (int)xv.size()-1; k++){ +// const int xs = xv[k-1]; +// const int ys = yv[k-1]; +// const int x = xv[k]; +// const int y = yv[k]; +// const int xS = xv[k+1]; +// const int yS = yv[k+1]; +// //check if point is inside the image +// if(!image->inside(xs,ys) || !image->inside(x,y) || !image->inside(xS,yS)){ +// continue; +// } +// if(renderDebugImage){ +// graphics.setColor(lms::imaging::blue); +// graphics.drawCross(x,y); +// } + +// int colors = *(image->data()+xs+ys*image->width()); +// int colorS = *(image->data()+xS+yS*image->width()); +// int sobel = colorS-colors; +// //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang +// if(sobel > threshold){ +// if(!foundLowHigh){ +// foundLowHigh = true; +// //logger.debug("")<<"crossing found lowHigh"< pxlPeakWidth*minLineWidthMul && pxlCounter < pxlPeakWidth*maxLineWidthMul){ +// //we found a valid poit, mark it +// if(renderDebugImage){ +// graphics.setColor(lms::imaging::red); +// for(int j = 0; j 0){ +// graphics.setColor(lms::imaging::green); +// graphics.drawCross(x,y); +// } +// pxlCounter = 0; +// foundLowHigh = false; +// //if not, we dont have to do anything +// } +// if(foundLowHigh){ +// pxlCounter++; +// } +// } +// return foundPoints; +// } } + std::vector NewRoadDetection::findByBrightness(const bool renderDebugImage, const std::vector &xv,const std::vector &yv, const float minLineWidthMul, const float maxLineWidthMul,const float iDist,const float wDist, const int threshold){ lms::imaging::BGRAImageGraphics graphics(*debugImage); std::vector foundPoints; @@ -194,76 +316,78 @@ std::vector NewRoadDetection::findByBrightness(const bool r return foundPoints; } -bool NewRoadDetection::find(){ +bool NewRoadDetection::find(CvImagePtr& incoming_image){ //clear old lines - logger.time("create lines"); - lines.clear(); - linesToProcess = 0; + ROS_INFO("Creating new lines"); + lines_.clear(); + linesToProcess_ = 0; //TODO rectangle for neglecting areas //(TODO calculate threshold for each line) if(renderDebugImage){ //Clear debug image - debugImage->resize(image->width(),image->height(),lms::imaging::Format::BGRA); - debugImage->fill(0); - debugAllPoints->points().clear(); - debugValidPoints->points().clear(); - debugTranslatedPoints->points().clear(); + debugImage->resize(incoming_image->rows,incoming_image->cols); } //create left/mid/right lane - lms::math::polyLine2f mid = road->getWithDistanceBetweenPoints(config().get("distanceBetweenSearchlines",0.2)); - lms::math::polyLine2f left = mid.moveOrthogonal(-0.4); - lms::math::polyLine2f right = mid.moveOrthogonal(0.4); - if(mid.points().size() != left.points().size() || mid.points().size() != right.points().size()){ - logger.error("invalid midlane given!"); - return false; + // todo: port this function to ROS + std::vector mid = road->getWithDistanceBetweenPoints(config().get("distanceBetweenSearchlines",0.2)); + std::vector left = mid.moveOrthogonal(-0.4); + std::vector right = mid.moveOrthogonal(0.4); + if(mid.size() != left.size() || mid.size() != right.size()){ + ROS_ERROR("Generated lane sizes do not match! Aborting search!"); + return false; } //get all lines - for(int i = 0; i< (int)mid.points().size(); i++){ + for(int i = 0; i< mid.size(); i++){ SearchLine l; - l.w_start = left.points()[i]; - l.w_end = right.points()[i]; - l.w_left = left.points()[i]; - l.w_mid = mid.points()[i]; - l.w_right = right.points()[i]; + l.w_start = left[i]; + l.w_end = right[i]; + l.w_left = left[i]; + l.w_mid = mid[i]; + l.w_right = right[i]; //check if the part is valid (middle should be always visible) - if(!image->inside(l.w_start.x,l.w_start.y)){ - l.w_start = mid.points()[i]; - if(!image->inside(l.w_end.x,l.w_end.y)){ + + // todo: implement transformation functions from world to map and reversed + // maybe this would be a good use for services (from the preprocessing node, has access to homography stuff anyway) + int l_w_startx, l_w_starty, l_w_endx, l_w_endy; + worldToImage(l.w_start.x, l.w_start.y, l_w_startx, l_w_starty); + worldToImage(l.w_end.x, l.w_end.y, l_w_endx, l_w_endy); + cv::Rect img_rect(cv::Point(),cv::Point(incoming_image->cols,incoming_image->rows)); + if(!img_rect.contains(cv::Point(l_w_startx,l_w_starty))){ + // try middle lane -> should always be in image + l.w_start = mid[i]; + worldToImage(l.w_start.x, l.w_start.y, l_w_startx, l_w_starty); + if(img_rect.contains(cv::Point(l_w_startx,l_w_starty))){ continue; } - }else if(!image->inside(l.w_end.x,l.w_end.y)){ - l.w_end = mid.points()[i]; + }else if(!img_rect.contains(cv::Point(l_w_endx,l_w_endy))){ + l.w_end = mid[i]; } //transform them in image-coordinates - homo.vti(l.i_start,l.w_start); - homo.vti(l.i_end,l.w_end); - { - std::unique_lock lock(mutex); - lines.push_back(l); - linesToProcess ++; - conditionNewLine.notify_one(); - } + std::unique_lock lock(mutex); + lines_.push_back(l); + linesToProcess_++; + + // todo: figure out what this does (no internet) +// conditionNewLine.notify_one(); } - logger.timeEnd("create lines"); - logger.time("search"); - if(numThreads == 0) { + if(numThreads_ == 0) { // single threaded - for(SearchLine &l:lines){ + for(SearchLine &l:lines_){ processSearchLine(l); } } else { // multi threaded // initialize and start threads if not yet there - if(threads.size() == 0) { - threadsRunning = true; - for(int i = 0; i < numThreads; i++) { - threads.emplace_back([this] () { + if(threads_.size() == 0) { + threadsRunning_ = true; + for(int i = 0; i < numThreads_; i++) { + threads_.emplace_back([this] () { threadFunction(); }); } @@ -272,24 +396,23 @@ bool NewRoadDetection::find(){ // wait till every search line was processed { std::unique_lock lock(mutex); - while(linesToProcess > 0) conditionLineProcessed.wait(lock); + while(linesToProcess_ > 0) conditionNewLine_.wait(lock); } } - logger.timeEnd("search"); return true; } void NewRoadDetection::threadFunction() { - while(threadsRunning) { + while(threadsRunning_) { SearchLine line; { std::unique_lock lock(mutex); - while(threadsRunning && lines.empty()) conditionNewLine.wait(lock); - if(lines.size() > 0) { - line = lines.front(); - lines.pop_front(); + while(threadsRunning_ && lines_.empty()) conditionNewLine_.wait(lock); + if(lines_.size() > 0) { + line = lines_.front(); + lines_.pop_front(); } - if(! threadsRunning) { + if(!threadsRunning_) { break; } } @@ -414,28 +537,20 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { } } -void NewRoadDetection::configsChanged(){ - lms::ServiceHandle warp = getService("WARP_SERVICE"); - if(!warp.isValid()){ - logger.error("WARP SERVICE is invalid!")<<"shutting down!"; - exit(0); //TODO shut down runtime - }else{ - homo = warp->getHomography(); - } - // read config - searchOffset = config().get("searchOffset",0.1); - findPointsBySobel = config().get("findBySobel",true); - renderDebugImage = config().get("renderDebugImage",false); - minLineWidthMul = config().get("minLineWidthMul",0.5); - maxLineWidthMul = config().get("maxLineWidthMul",1.5); - threshold = config().get("threshold",200); - laneWidthOffsetInMeter = config().get("laneWidthOffsetInMeter",0.1); - useWeights = config().get("useWeights",false); - sobelThreshold = config().get("sobelThreshold",200); - numThreads = config().get("threads",0); +void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::new_road_detectionConfig& config, uint32_t level){ + searchOffset_ = config.searchOffset; + findPointsBySobel_ = config.findBySobel; + renderDebugImage_ = config.renderDebugImage; + minLineWidthMul_ = config.minLineWidthMul; + maxLineWidthMul_ = config.maxLineWidthMul; + threshold_ = config.threshold; + laneWidthOffsetInMeter_ = config.laneWidthOffsetInMeter; + useWeights_ = config.useWeights; + sobelThreshold_ = config.sobelThreshold; + numThreads_ = config.threads; } -void NewRoadDetection::destroy() { +NewRoadDetection::~NewRoadDetection() { { std::unique_lock lock(mutex); threadsRunning = false; diff --git a/src/new_road_detection_node.cpp b/src/new_road_detection_node.cpp index 3cb4b49..78226b0 100644 --- a/src/new_road_detection_node.cpp +++ b/src/new_road_detection_node.cpp @@ -4,8 +4,9 @@ int main(int argc, char** argv) { ros::init(argc, argv, "new_road_detection"); ros::NodeHandle nh; + ros::NodeHandle pnh("~"); - drive_ros_image_recognition::NewRoadDetection new_road_detection = drive_ros_image_recognition::NewRoadDetection(nh); + NewRoadDetection new_road_detection(nh,pnh); if (!new_road_detection.init()) { return 1; } From 4ba460abafe8060a2d53a4de55646147a33a403a Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sun, 18 Jun 2017 12:13:45 +0200 Subject: [PATCH 03/94] Rewrite of new_road_detection complete, needs Eigen integration for world line processing and service for translation from/to image --- CMakeLists.txt | 7 +- cfg/NewRoadDetection.cfg | 21 + config/new_road_detection.yaml | 2 +- .../new_road_detection.h | 88 ++-- msg/RoadLane.msg | 2 +- package.xml | 2 + src/new_road_detection.cpp | 491 +++++++++++------- src/new_road_detection_node.cpp | 3 +- 8 files changed, 386 insertions(+), 230 deletions(-) create mode 100755 cfg/NewRoadDetection.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c8888d..aadcded 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs + dynamic_reconfigure #temp include to expedite transtition of new_road_detection -> todo: move to common message storage, not created yet message_generation geometry_msgs @@ -28,6 +29,10 @@ generate_messages( geometry_msgs ) +generate_dynamic_reconfigure_options( + cfg/NewRoadDetection.cfg +) + catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs message_runtime @@ -75,7 +80,7 @@ add_executable(new_road_detection src/new_road_detection_node.cpp ) -add_dependencies(new_road_detection ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(new_road_detection ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(new_road_detection ${catkin_LIBRARIES} diff --git a/cfg/NewRoadDetection.cfg b/cfg/NewRoadDetection.cfg new file mode 100755 index 0000000..e456566 --- /dev/null +++ b/cfg/NewRoadDetection.cfg @@ -0,0 +1,21 @@ +#!/usr/bin/env python +PACKAGE = "drive_ros_image_recognition" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("searchOffset", double_t, 0, "Search offset", 0.15, 0.0, 2.0) +gen.add("distanceBetweenSearchlines", double_t, 0, "Distance between search lines", 0.1, 0.0, 1.0) +gen.add("minLineWidthMul", double_t, 0, "minLineWidthMul", 0.5, 0.0, 1.0) +gen.add("maxLineWidthMul", double_t, 0, "maxLineWidthMul", 2, 0.0, 5.0) +gen.add("findBySobel", bool_t, 0, "Use sobel", True) +gen.add("brightness_threshold", int_t, 0, "Brightness threshold", 50, 0, 200) +gen.add("sobelThreshold", int_t, 0, "Sobel threshold", 50, 0, 200) +gen.add("laneWidthOffsetInMeter", double_t, 0, "Lane width offset (m)", 0.1, 0.0, 2.0) +gen.add("translateEnvironment", bool_t, 0, "Translate environment", False) +gen.add("useWeights", bool_t, 0, "Use weights", False) +gen.add("renderDebugImage", bool_t, 0, "Render debug image", False) +gen.add("threads", int_t, 0, "Number of threads", 4, 0, 8) + +exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "new_road_detection")) diff --git a/config/new_road_detection.yaml b/config/new_road_detection.yaml index 137bf33..84d7c29 100644 --- a/config/new_road_detection.yaml +++ b/config/new_road_detection.yaml @@ -4,7 +4,7 @@ new_road_detection: minLineWidthMul: 0.5 maxLineWidthMul: 2 findBySobel: true - threshold: 50 + brightness_threshold: 50 sobelThreshold: 50 laneWidthOffsetInMeter: 0.1 translateEnvironment: false diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index f643a74..f35333e 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -4,8 +4,11 @@ #include #include #include +#include #include #include +#include +#include //#include //#include //#include @@ -19,7 +22,7 @@ #include #include -typedef std::shared_ptr CvImagePtr; +typedef boost::shared_ptr CvImagePtr; inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { CvImagePtr cv_ptr; @@ -27,18 +30,18 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { // todo: check if this if we have 8 or 16 greyscale images // hardcopies for now, might be possible to process on pointer if fast enough - cv_ptr = cv_bridge::toCvCopy(msg, "mono16"); + cv_ptr = cv_bridge::toCvCopy(img_in, "mono16"); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); - return; + return NULL; } if( !cv_ptr->data) { ROS_WARN("Empty image received, skipping!"); - return; + return NULL; } return cv_ptr; } @@ -54,9 +57,9 @@ class NewRoadDetection { bool renderDebugImage_; float minLineWidthMul_; float maxLineWidthMul_; - int threshold_; + int brightness_threshold_; float laneWidthOffsetInMeter_; - bool translate_environment_; + bool translateEnvironment_; bool useWeights_; int sobelThreshold_; int numThreads_; // 0 means single threaded @@ -77,8 +80,8 @@ class NewRoadDetection { struct SearchLine{ cv::Point2f w_start; cv::Point2f w_end; - cv::Point i_start; - cv::Point i_end; + cv::Point2i i_start; + cv::Point2i i_end; cv::Point2f w_left; @@ -86,54 +89,55 @@ class NewRoadDetection { cv::Point2f w_right; }; - std::list lines; + std::list lines_; std::mutex mutex; std::mutex debugAllPointsMutex; std::mutex debugValidPointsMutex; - std::vector threads; - std::condition_variable conditionNewLine; - std::condition_variable conditionLineProcessed; - bool threadsRunning; - int linesToProcess; - - ros::Subscriber image_sub_; + std::vector threads_; + std::condition_variable conditionNewLine_; + std::condition_variable conditionLineProcessed_; + bool threadsRunning_; + int linesToProcess_; + CvImagePtr current_image_; + CvImagePtr current_image_sobel_; + + cv::Mat debug_image_; + + image_transport::ImageTransport it_; + image_transport::Subscriber img_sub_; // road inputs and outputs - ros::Subscriber road_; - ros::Publisher output_; + ros::Subscriber road_sub_; + ros::Publisher line_output_pub_; #ifdef DRAW_DEBUG - ros::Publisher debug_img_pub_; + image_transport::Publisher debug_img_pub_; #endif ros::NodeHandle nh_; + ros::NodeHandle pnh_; std::string my_namespace_; + drive_ros_image_recognition::RoadLane road_buffer_; -public: - bool init() override; - void destroy() override; - void imageCallback(const sensor_msgs::ImageConstPtr img_in); - void configsChanged() override; + void roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_); + void imageCallback(const sensor_msgs::ImageConstPtr& img_in); + dynamic_reconfigure::Server dsrv_server_; + dynamic_reconfigure::Server::CallbackType dsrv_cb_; + void reconfigureCB(drive_ros_image_recognition::new_road_detectionConfig& config, uint32_t level); bool find(); - std::vector - findBySobel(const bool renderDebugImage, - const std::vector &xv, - const std::vector &yv, - const float minLineWidthMul, - const float maxLineWidthMul, - const float iDist, - const float wDist, - const int threshold); - - std::vector - findByBrightness(const bool renderDebugImage, - const std::vector &xv, - const std::vector &yv, - const float minLineWidthMul, - const float maxLineWidthMul, + std::vector findBySobel(cv::LineIterator it, + const float lineWidth, + const float iDist, + const float wDist); + + std::vector findByBrightness(cv::LineIterator it, const float iDist, - const float wDist, - const int threshold); + const float wDist); + +public: + NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh); + ~NewRoadDetection(); + bool init(); void processSearchLine(const SearchLine &line); diff --git a/msg/RoadLane.msg b/msg/RoadLane.msg index 9d20589..c995c7c 100644 --- a/msg/RoadLane.msg +++ b/msg/RoadLane.msg @@ -7,4 +7,4 @@ uint8 UNKNOWN=0 uint8 STRAIGHT=1 uint8 STRAIGHT_CURVE=2 uint8 CURVE = 3 -uint8 RoadStateType +uint8 roadStateType diff --git a/package.xml b/package.xml index ede332c..e16459e 100644 --- a/package.xml +++ b/package.xml @@ -45,6 +45,7 @@ roscpp sensor_msgs std_msgs + dynamic_reconfigure message_generation geometry_msgs @@ -54,6 +55,7 @@ roscpp sensor_msgs std_msgs + dynamic_reconfigure message_runtime geometry_msgs diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index c22d1fc..8d33d5f 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -7,22 +7,37 @@ //#include //#include -bool NewRoadDetection::NewRoadDetection(ros::NodeHandle nh): +NewRoadDetection::NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh): nh_(nh), - image_sub_(), + pnh_(pnh), + img_sub_(), my_namespace_("new_road_detection"), searchOffset_(0.15), distanceBetweenSearchlines_(0.1), minLineWidthMul_(0.5), maxLineWidthMul_(2), findPointsBySobel_(true), - threshold_(50), + brightness_threshold_(50), sobelThreshold_(50), laneWidthOffsetInMeter_(0.1), translateEnvironment_(false), useWeights_(false), renderDebugImage_(false), - numThreads_(4) + numThreads_(4), + road_sub_(), + debug_image_(0,0,CV_16UC1), + line_output_pub_(), + it_(pnh), + dsrv_server_(), + dsrv_cb_(boost::bind(&NewRoadDetection::reconfigureCB, this, _1, _2)), + lines_(), + threadsRunning_(false), + linesToProcess_(0), + threads_(), + conditionNewLine_(), + conditionLineProcessed_(), + current_image_(), + current_image_sobel_() { } @@ -48,8 +63,8 @@ bool NewRoadDetection::init() { ROS_WARN_STREAM("Unable to load 'findBySobel' parameter, using default: "<("ROAD"); - //output = writeChannel("ROAD_OUTPUT"); + dsrv_server_.setCallback(dsrv_cb_); + + img_sub_ = it_.subscribe("image", 10, &NewRoadDetection::imageCallback, this); + road_sub_ = nh_.subscribe("road_in", 100, &NewRoadDetection::roadCallback, this); + line_output_pub_ = nh_.advertise("line_out",10); #ifdef DRAW_DEBUG debug_img_pub_ = nh_.advertise("/debug_image_out", 10); @@ -96,12 +112,25 @@ bool NewRoadDetection::init() { return true; } -void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { - CvImagePtr img_conv = convertImageMessage(img_in); +void NewRoadDetection::roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_) { + // use simple singular buffer + road_buffer_ = *road_in_; +} + +void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr& img_in) { + current_image_ = convertImageMessage(img_in); + if (renderDebugImage_) { + debug_image_ = current_image_->clone(); + } + + // todo: adjust this time value + if (img_in->header.stamp.toSec() - road_buffer_.header.stamp.toSec() > 0.1) { + ROS_WARN("Outdated road callback in buffer, skipping incoming image"); + return; + } //if we have a road(?), try to find the line - // todo: no interface for this yet - if(road->points().size() > 1){ + if(road_buffer_.points.size() > 1){ find(); //TODO use bool from find } @@ -116,7 +145,7 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { // dPhi = car->deltaPhi(); // } - // todo: no interface for this yet -> ask Phibedy what this does + // todo: no interface for this yet, create course verification service to verfiy and publish // lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); // if(localCourse.isValid()){ // logger.time("localCourse"); @@ -129,6 +158,8 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { // debugTranslatedPoints->points() = localCourse->getPointsAdded(); // } #endif + // should do something like this +// line_output_pub_.publish(road_translated); // }else{ // ROS_ERROR("LocalCourse invalid, aborting the callback!"); @@ -137,133 +168,199 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr img_in) { return; } -std::vector NewRoadDetection::findBySobel( - const bool renderDebugImage, - const std::vector &xv, - const std::vector &yv, - const float minLineWidthMul, - const float maxLineWidthMul, - const float iDist, - const float wDist, - const int threshold) { - return ::findBySobel(image.get(),debugImage.get(),renderDebugImage,xv,yv,minLineWidthMul,maxLineWidthMul,0.02,iDist,wDist,threshold,homo); +std::vector NewRoadDetection::findBySobel(cv::LineIterator it, + const float lineWidth, + const float iDist, + const float wDist) +{ + std::vector found_points(); + bool foundLowHigh = false; + int pxlCounter = 0; + + cv::Rect rect(cv::Point(),cv::Point(current_image_->rows, current_image_->cols)); + + for(int i = 0; i < it.count; i++, ++it) + { + // safety check : is the point inside the image + if(!rect.contains(it.pos())){ + ROS_WARN("Received an invalid point outside the image to check for Sobel"); + return found_points(); + } + + //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang + int sobel = (int)current_image_sobel_->at(it.pos().x,it.pos().y); + if(sobel > sobelThreshold_){ + if(!foundLowHigh){ + foundLowHigh = true; + } + }else if(sobel < -sobelThreshold_){ //hell-dunkel übergang + //check if we found a lowHigh + highLow border + if(foundLowHigh){ + //check if the points have the right distance + float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? + + //logger.debug("")<<"crossing found highLow: "< pxlPeakWidth*minLineWidthMul_ && pxlCounter < pxlPeakWidth*maxLineWidthMul_){ + //we found a valid point + //get the middle + cv::Point2f wMid; + imageToWorld(xv[k-pxlCounter/2],yv[k-pxlCounter/2],wMid.x,wMid.y); + foundPoints.push_back(wMid); + //logger.debug("")<<"crossing FOUND VALID CROSSING"; + } + } + pxlCounter = 0; + foundLowHigh = false; + //if not, we dont have to do anything + } + } + return found_points(); } -std::vector NewRoadDetection::findByBrightness(const bool renderDebugImage, const std::vector &xv,const std::vector &yv, const float minLineWidthMul, const float maxLineWidthMul,const float iDist,const float wDist, const int threshold){ - lms::imaging::BGRAImageGraphics graphics(*debugImage); - std::vector foundPoints; + +std::vector NewRoadDetection::findByBrightness( cv::LineIterator it, + const float iDist, + const float wDist ) +{ + std::vector foundPoints; std::vector color; - //get the color from the points - if(renderDebugImage) - graphics.setColor(lms::imaging::blue); - for(int k = 0; k < (int)xv.size(); k++){ - const int x = xv[k]; - const int y = yv[k]; - if(!image->inside(x,y)){ - color.push_back(0); - continue; - } - color.push_back(*(image->data()+x+y*image->width())); - if(renderDebugImage) - graphics.drawCross(x,y); + cv::Rect rect(cv::Point(),cv::Point(current_image_->rows, current_image_->cols)); + cv::LineIterator it_debug = it; + + for(int i = 0; i < it.count; i++, ++it) { + // safety check : is the point inside the image + if(!rect.contains(it.pos())){ + ROS_WARN("Received an invalid point outside the image to check for brightness"); + color.push_back(0); + continue; + } + color.push_back(current_image_->at(it.pos().x,it.pos().y)); + + if (renderDebugImage_) { + cv::namedWindow("Unfiltered points processed by brightness search", CV_WINDOW_NORMAL); + cv::circle(debug_image_, it.pos(), 2, cv::Scalar(255)); + cv::imshow("Unfiltered points processed by brightness search", debug_image_); + } } + //detect peaks + // todo: fix hardcoded line width here as well float pxlPeakWidth = iDist/wDist*0.02; //TODO to bad, calculate for each road line (how should we use them for searching? int tCounter = 0; - if(renderDebugImage) - graphics.setColor(lms::imaging::red); + // todo: make this more efficient for(int k = 0; k < (int)color.size(); k++){ - if(color[k]>threshold){ - tCounter++; - }else{ - if(tCounter - k != 0 && tCounter > pxlPeakWidth*minLineWidthMul && tCounter < pxlPeakWidth*maxLineWidthMul){ - if(renderDebugImage){ - for(int j = 0; jbrightness_threshold_){ + tCounter++; + }else{ + if(tCounter - k != 0 && tCounter > pxlPeakWidth*minLineWidthMul_ && tCounter < pxlPeakWidth*maxLineWidthMul_){ + for (int i = 0; i < tCounter; i++, ++it_debug) { + } + // get points for debug drawing on the way + cv::Point point_first = it_debug.pos(); + for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { + } + cv::Point point_mid = it_debug.pos(); + for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { + } + cv::Point point_end = it_debug.pos(); + + if (renderDebugImage_) { + cv::namedWindow("Line detected by brightness", CV_WINDOW_NORMAL); + debug_image_ = current_image_->clone(); + + cv::line(debug_image_, point_first, point_end, cv::Scalar(255)); + cv::imshow("Line detected by brightness", debug_image_); + } + + //we found a valid point + //get the middle + cv::Point2i mid = point_mid; + cv::Point2f wMid; + imageToWorld(mid,wMid); + foundPoints.push_back(wMid); } + tCounter = 0; + } } return foundPoints; } bool NewRoadDetection::find(){ //clear old lines - logger.time("create lines"); - lines.clear(); - linesToProcess = 0; + ROS_INFO("Creating new lines"); + lines_.clear(); + linesToProcess_ = 0; - //TODO rectangle for neglecting areas + // already there in LMS +// //TODO rectangle for neglecting areas - //(TODO calculate threshold for each line) - if(renderDebugImage){ +// //(TODO calculate threshold for each line) + if(renderDebugImage_){ //Clear debug image - debugImage->resize(image->width(),image->height(),lms::imaging::Format::BGRA); - debugImage->fill(0); - debugAllPoints->points().clear(); - debugValidPoints->points().clear(); - debugTranslatedPoints->points().clear(); + debug_image_.resize(current_image_->rows,current_image_->cols); } //create left/mid/right lane - lms::math::polyLine2f mid = road->getWithDistanceBetweenPoints(config().get("distanceBetweenSearchlines",0.2)); - lms::math::polyLine2f left = mid.moveOrthogonal(-0.4); - lms::math::polyLine2f right = mid.moveOrthogonal(0.4); - if(mid.points().size() != left.points().size() || mid.points().size() != right.points().size()){ - logger.error("invalid midlane given!"); - return false; + // todo: move to eigen and port this + std::vector mid = road->getWithDistanceBetweenPoints(config().get("distanceBetweenSearchlines",0.2)); + std::vector left = mid.moveOrthogonal(-0.4); + std::vector right = mid.moveOrthogonal(0.4); + if(mid.size() != left.size() || mid.size() != right.size()){ + ROS_ERROR("Generated lane sizes do not match! Aborting search!"); + return false; } //get all lines - for(int i = 0; i< (int)mid.points().size(); i++){ + for(int i = 0; i< mid.size(); i++){ SearchLine l; - l.w_start = left.points()[i]; - l.w_end = right.points()[i]; - l.w_left = left.points()[i]; - l.w_mid = mid.points()[i]; - l.w_right = right.points()[i]; + l.w_start = left[i]; + l.w_end = right[i]; + l.w_left = left[i]; + l.w_mid = mid[i]; + l.w_right = right[i]; //check if the part is valid (middle should be always visible) - if(!image->inside(l.w_start.x,l.w_start.y)){ - l.w_start = mid.points()[i]; - if(!image->inside(l.w_end.x,l.w_end.y)){ + + // todo: implement transformation functions from world to map and reversed + // maybe this would be a good use for services (from the preprocessing node, has access to homography stuff anyway) + int l_w_startx, l_w_starty, l_w_endx, l_w_endy; + worldToImage(l.w_start.x, l.w_start.y, l_w_startx, l_w_starty); + worldToImage(l.w_end.x, l.w_end.y, l_w_endx, l_w_endy); + cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); + if(!img_rect.contains(cv::Point(l_w_startx,l_w_starty))){ + // try middle lane -> should always be in image + l.w_start = mid[i]; + worldToImage(l.w_start.x, l.w_start.y, l_w_startx, l_w_starty); + if(img_rect.contains(cv::Point(l_w_startx,l_w_starty))){ continue; } - }else if(!image->inside(l.w_end.x,l.w_end.y)){ - l.w_end = mid.points()[i]; + }else if(!img_rect.contains(cv::Point(l_w_endx,l_w_endy))){ + l.w_end = mid[i]; } //transform them in image-coordinates - homo.vti(l.i_start,l.w_start); - homo.vti(l.i_end,l.w_end); - { - std::unique_lock lock(mutex); - lines.push_back(l); - linesToProcess ++; - conditionNewLine.notify_one(); - } + std::unique_lock lock(mutex); + lines_.push_back(l); + linesToProcess_++; + + // todo: figure out what this does (no internet) +// conditionNewLine.notify_one(); } - logger.timeEnd("create lines"); - logger.time("search"); - if(numThreads == 0) { + if(numThreads_ == 0) { // single threaded - for(SearchLine &l:lines){ + for(SearchLine &l:lines_){ processSearchLine(l); } } else { // multi threaded // initialize and start threads if not yet there - if(threads.size() == 0) { - threadsRunning = true; - for(int i = 0; i < numThreads; i++) { - threads.emplace_back([this] () { + if(threads_.size() == 0) { + threadsRunning_ = true; + for(int i = 0; i < numThreads_; i++) { + threads_.emplace_back([this] () { threadFunction(); }); } @@ -272,24 +369,23 @@ bool NewRoadDetection::find(){ // wait till every search line was processed { std::unique_lock lock(mutex); - while(linesToProcess > 0) conditionLineProcessed.wait(lock); + while(linesToProcess_ > 0) conditionNewLine_.wait(lock); } } - logger.timeEnd("search"); return true; } void NewRoadDetection::threadFunction() { - while(threadsRunning) { + while(threadsRunning_) { SearchLine line; { std::unique_lock lock(mutex); - while(threadsRunning && lines.empty()) conditionNewLine.wait(lock); - if(lines.size() > 0) { - line = lines.front(); - lines.pop_front(); + while(threadsRunning_ && lines_.empty()) conditionNewLine_.wait(lock); + if(lines_.size() > 0) { + line = lines_.front(); + lines_.pop_front(); } - if(! threadsRunning) { + if(!threadsRunning_) { break; } } @@ -302,41 +398,57 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { std::vector yv; //calculate the offset - float iDist = l.i_start.distance(l.i_end); - float wDist = l.w_start.distance(l.w_end); - float pxlPerDist = iDist/wDist*searchOffset; - lms::math::vertex2f iDiff = lms::math::vertex2f(l.i_start-l.i_end).normalize(); - lms::math::vertex2f startLine = lms::math::vertex2f(l.i_start)+iDiff*pxlPerDist; - lms::math::vertex2f endLine = lms::math::vertex2f(l.i_end)-iDiff*pxlPerDist; - //get all points in between - lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! - - //find points - std::vector foundPoints; - if(findPointsBySobel){ - foundPoints = findBySobel(renderDebugImage,xv,yv,minLineWidthMul,maxLineWidthMul,iDist,wDist,sobelThreshold); - }else{ - foundPoints = findByBrightness(renderDebugImage,xv,yv,minLineWidthMul,maxLineWidthMul,iDist,wDist,threshold); - } - - if(renderDebugImage) { + float iDist = cv::norm(l.i_end - l.i_start); + float wDist = cv::norm(l.w_end - l.w_start); +// float pxlPerDist = iDist/wDist*searchOffset_; +// lms::math::vertex2f iDiff = lms::math::vertex2f(l.i_start-l.i_end).normalize(); +// lms::math::vertex2f startLine = lms::math::vertex2f(l.i_start)+iDiff*pxlPerDist; +// lms::math::vertex2f endLine = lms::math::vertex2f(l.i_end)-iDiff*pxlPerDist; +// //get all points in between +// lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! + + // generate Bresenham search line + cv::LineIterator it(*current_image_,l.i_start,l.i_end); + + std::vector foundPoints; + //find points + if(findPointsBySobel_){ + // directly apply sobel operations on the image -> we only use the grad_x here as we only search horizontally + // we perform order of 1 operations only + // todo: check if CV_8UC1 is valid + cv::Sobel( *current_image_, *current_image_sobel_, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); + // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter + foundPoints = findBySobel(it,0.02,iDist,wDist); + }else{ + foundPoints = findByBrightness(it,iDist,wDist); + } + + // draw unfiltered image points + if(renderDebugImage_) { std::unique_lock lock(debugAllPointsMutex); - for(lms::math::vertex2f &v:foundPoints) - debugAllPoints->points().push_back(v); + cv::namedWindow("Unfiltered points", CV_WINDOW_NORMAL); + cv::Mat found_points_mat = current_image_->clone(); + for (auto point : foundPoints) { + cv::circle(found_points_mat, point, 2, cv::Scalar(255)); + } + cv::imshow("Unfiltered points", found_points_mat); } //filter //TODO filter points that are in poluted regions (for example the car itself) //remove points with bad distances + std::vector validPoints; if(foundPoints.size() > 2){ std::vector foundCounter; foundCounter.resize(foundPoints.size(),0); for(std::size_t fp = 0; fp < foundPoints.size(); fp++){ - const lms::math::vertex2f &s = foundPoints[fp]; + // todo: insert service here + const cv::Point2f &s = foundPoints[fp]; for(std::size_t test = fp+1; test 0.4-laneWidthOffsetInMeter && distance < 0.4 + laneWidthOffsetInMeter)|| (distance > 0.8-laneWidthOffsetInMeter && distance < 0.8+laneWidthOffsetInMeter)){ + // todo: insert service here + const cv::Point2f &toTest = foundPoints[test]; + float distance = cv::norm(s-toTest); + if((distance > 0.4-laneWidthOffsetInMeter_ && distance < 0.4 + laneWidthOffsetInMeter_)|| (distance > 0.8-laneWidthOffsetInMeter_ && distance < 0.8+laneWidthOffsetInMeter_)){ foundCounter[test]++; foundCounter[fp]++; } @@ -349,37 +461,46 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { max = c; } } - std::vector validPoints; for(int i = 0; i < (int)foundPoints.size(); i++){ if(foundCounter[i] >= max){ validPoints.push_back(foundPoints[i]); } } //TODO if we have more than 3 points we know that there is an error! - foundPoints = validPoints; +// foundPoints = validPoints; + } + else { + // just use the 2 points we found otherwise + validPoints = foundPoints; } - if(renderDebugImage){ + // draw filtered points + if(renderDebugImage_) { + // todo: make some more efficient storage method here, will translate back and forth here for now std::unique_lock lock(debugValidPointsMutex); - for(lms::math::vertex2f &v:foundPoints) - debugValidPoints->points().push_back(v); + cv::namedWindow("Filtered Points", CV_WINDOW_NORMAL); + cv::Mat filtered_points_mat = current_image_->clone(); + for (auto point : validPoints) { + cv::circle(filtered_points_mat, worldToImage(point), 2, cv::Scalar(255)); + } + cv::imshow("Filtered Points", filtered_points_mat); } //Handle found points - lms::math::vertex2f diff; + cv::Point2f diff; std::vector distances; - for(int i = 0; i < (int)foundPoints.size(); i++){ - lms::math::vertex2f &wMind = foundPoints[i]; - float distanceToLeft = wMind.distance(l.w_left); - float distanceToRight= wMind.distance(l.w_right); - float distanceToMid= wMind.distance(l.w_mid); - diff = (l.w_left-l.w_right).normalize(); + for(int i = 0; i < (int)validPoints.size(); i++){ + float distanceToLeft = cv::norm(validPoints[i]-l.w_left); + float distanceToRight= cv::norm(validPoints[i]-l.w_right); + float distanceToMid= cv::norm(validPoints[i]-l.w_mid); + // normalize distance + diff = (l.w_left - l.w_right)/cv::norm(l.w_left-l.w_right); if(distanceToLeft < distanceToMid && distanceToLeft < distanceToRight){ //left point - wMind-=diff*0.4; + validPoints[i]-=diff*0.4; distances.push_back(distanceToLeft); }else if(distanceToRight < distanceToMid ){ - wMind+=diff*0.4; + validPoints[i]+=diff*0.4; distances.push_back(distanceToRight); }else{ distances.push_back(distanceToMid); @@ -389,7 +510,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { std::vector weights; for(const float &dist:distances){ //as distance is in meter, we multiply it by 100 - if(useWeights) + if(useWeights_) weights.push_back(1/(dist*100+0.001)); //TODO hier etwas sinnvolles überlegen else weights.push_back(1); @@ -400,48 +521,50 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { debugTranslatedPoints->points().push_back(v); } */ - lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); - if(localCourse.isValid()){ - localCourse->addPoints(foundPoints,weights); - }else{ - logger.error("localCourse invalid!"); + // todo: check how this gets handled and adjust the interface + drive_ros_image_recognition::RoadLane lane_out; + lane_out.header.stamp = ros::Time::now(); + geometry_msgs::Point32 point_temp; + point_temp.z = 0.f; + for (auto point: validPoints) { + point_temp.x = point.x; + point_temp.y = point.y; + lane_out.points.push_back(point_temp); } + lane_out.roadStateType = drive_ros_image_recognition::RoadLane::UNKNOWN; + line_output_pub_.publish(lane_out); +// lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); +// if(localCourse.isValid()){ +// localCourse->addPoints(foundPoints,weights); +// }else{ +// logger.error("localCourse invalid!"); +// } - { - std::unique_lock lock(mutex); - linesToProcess --; - conditionLineProcessed.notify_all(); - } + std::unique_lock lock(mutex); + linesToProcess_ --; + conditionLineProcessed_.notify_all(); } -void NewRoadDetection::configsChanged(){ - lms::ServiceHandle warp = getService("WARP_SERVICE"); - if(!warp.isValid()){ - logger.error("WARP SERVICE is invalid!")<<"shutting down!"; - exit(0); //TODO shut down runtime - }else{ - homo = warp->getHomography(); - } - // read config - searchOffset = config().get("searchOffset",0.1); - findPointsBySobel = config().get("findBySobel",true); - renderDebugImage = config().get("renderDebugImage",false); - minLineWidthMul = config().get("minLineWidthMul",0.5); - maxLineWidthMul = config().get("maxLineWidthMul",1.5); - threshold = config().get("threshold",200); - laneWidthOffsetInMeter = config().get("laneWidthOffsetInMeter",0.1); - useWeights = config().get("useWeights",false); - sobelThreshold = config().get("sobelThreshold",200); - numThreads = config().get("threads",0); +void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::new_road_detectionConfig& config, uint32_t level){ + searchOffset_ = config.searchOffset; + findPointsBySobel_ = config.findBySobel; + renderDebugImage_ = config.renderDebugImage; + minLineWidthMul_ = config.minLineWidthMul; + maxLineWidthMul_ = config.maxLineWidthMul; + brightness_threshold_ = config.brightness_threshold; + laneWidthOffsetInMeter_ = config.laneWidthOffsetInMeter; + useWeights_ = config.useWeights; + sobelThreshold_ = config.sobelThreshold; + numThreads_ = config.threads; } -void NewRoadDetection::destroy() { +NewRoadDetection::~NewRoadDetection() { { std::unique_lock lock(mutex); - threadsRunning = false; - conditionNewLine.notify_all(); + threadsRunning_ = false; + conditionNewLine_.notify_all(); } - for(auto &t : threads) { + for(auto &t : threads_) { if(t.joinable()) { t.join(); } diff --git a/src/new_road_detection_node.cpp b/src/new_road_detection_node.cpp index 3cb4b49..78226b0 100644 --- a/src/new_road_detection_node.cpp +++ b/src/new_road_detection_node.cpp @@ -4,8 +4,9 @@ int main(int argc, char** argv) { ros::init(argc, argv, "new_road_detection"); ros::NodeHandle nh; + ros::NodeHandle pnh("~"); - drive_ros_image_recognition::NewRoadDetection new_road_detection = drive_ros_image_recognition::NewRoadDetection(nh); + NewRoadDetection new_road_detection(nh,pnh); if (!new_road_detection.init()) { return 1; } From e33a2aaf6067f5bbfc7a319944faaf15906cd820 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sun, 18 Jun 2017 12:42:01 +0200 Subject: [PATCH 04/94] Fixed bresenham line search logic, changed function visibility --- .../new_road_detection.h | 12 ++++----- src/new_road_detection.cpp | 26 ++++++++++++++----- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index f35333e..13b6d41 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -131,17 +131,17 @@ class NewRoadDetection { const float wDist); std::vector findByBrightness(cv::LineIterator it, - const float iDist, - const float wDist); + const float lineWidth, + const float iDist, + const float wDist); + + void processSearchLine(const SearchLine &line); + void threadFunction(); public: NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh); ~NewRoadDetection(); bool init(); - - void processSearchLine(const SearchLine &line); - - void threadFunction(); }; #endif // NEW_ROAD_DETECTION_H diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 8d33d5f..87e8be3 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -173,9 +173,10 @@ std::vector NewRoadDetection::findBySobel(cv::LineIterator it, const float iDist, const float wDist) { - std::vector found_points(); + std::vector foundPoints; bool foundLowHigh = false; int pxlCounter = 0; + cv::LineIterator it_backup = it; cv::Rect rect(cv::Point(),cv::Point(current_image_->rows, current_image_->cols)); @@ -184,14 +185,16 @@ std::vector NewRoadDetection::findBySobel(cv::LineIterator it, // safety check : is the point inside the image if(!rect.contains(it.pos())){ ROS_WARN("Received an invalid point outside the image to check for Sobel"); - return found_points(); + return foundPoints; } //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang int sobel = (int)current_image_sobel_->at(it.pos().x,it.pos().y); if(sobel > sobelThreshold_){ + // found low-high pass (on the left side of the line) if(!foundLowHigh){ foundLowHigh = true; + pxlCounter = 0; } }else if(sobel < -sobelThreshold_){ //hell-dunkel übergang //check if we found a lowHigh + highLow border @@ -207,8 +210,11 @@ std::vector NewRoadDetection::findBySobel(cv::LineIterator it, if(pxlCounter > pxlPeakWidth*minLineWidthMul_ && pxlCounter < pxlPeakWidth*maxLineWidthMul_){ //we found a valid point //get the middle + // todo: make this more elegant in both sobel and brightness (no std::advance for lineIterator) + for (int iter = 0; iter<(i-pxlCounter/2); iter++, ++it_backup) { + } cv::Point2f wMid; - imageToWorld(xv[k-pxlCounter/2],yv[k-pxlCounter/2],wMid.x,wMid.y); + imageToWorld(it_backup.pos(),wMid); foundPoints.push_back(wMid); //logger.debug("")<<"crossing FOUND VALID CROSSING"; } @@ -217,11 +223,17 @@ std::vector NewRoadDetection::findBySobel(cv::LineIterator it, foundLowHigh = false; //if not, we dont have to do anything } + + // for calculation of line width + if(foundLowHigh){ + pxlCounter++; + } } - return found_points(); + return foundPoints; } std::vector NewRoadDetection::findByBrightness( cv::LineIterator it, + const float lineWidth, const float iDist, const float wDist ) { @@ -247,8 +259,7 @@ std::vector NewRoadDetection::findByBrightness( cv::LineIterator it } //detect peaks - // todo: fix hardcoded line width here as well - float pxlPeakWidth = iDist/wDist*0.02; //TODO to bad, calculate for each road line (how should we use them for searching? + float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? int tCounter = 0; // todo: make this more efficient for(int k = 0; k < (int)color.size(); k++){ @@ -420,7 +431,8 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter foundPoints = findBySobel(it,0.02,iDist,wDist); }else{ - foundPoints = findByBrightness(it,iDist,wDist); + // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter + foundPoints = findByBrightness(it,0.02,iDist,wDist); } // draw unfiltered image points From 84d150b0e12fe8a831d6c9139510c2efa9c60ce7 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sun, 18 Jun 2017 14:58:50 +0200 Subject: [PATCH 05/94] Added world->image, image->world services, last remaining step: eigen translation to complete port --- CMakeLists.txt | 9 ++- .../new_road_detection.h | 6 ++ .../drive_ros_image_recognition/warp_image.h | 10 +++ src/new_road_detection.cpp | 66 +++++++++++++--- src/warp_image.cpp | 76 +++++++++++++------ srv/ImageToWorld.srv | 5 ++ srv/WorldToImage.srv | 5 ++ 7 files changed, 143 insertions(+), 34 deletions(-) create mode 100644 srv/ImageToWorld.srv create mode 100644 srv/WorldToImage.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index aadcded..6e8309c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,9 +13,8 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs dynamic_reconfigure -#temp include to expedite transtition of new_road_detection -> todo: move to common message storage, not created yet - message_generation geometry_msgs + message_generation ) add_message_files( @@ -23,6 +22,12 @@ add_message_files( RoadLane.msg ) +add_service_files( + FILES + WorldToImage.srv + ImageToWorld.srv +) + generate_messages( DEPENDENCIES std_msgs diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index 13b6d41..f4b7aa1 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -125,6 +125,12 @@ class NewRoadDetection { void reconfigureCB(drive_ros_image_recognition::new_road_detectionConfig& config, uint32_t level); bool find(); + ros::ServiceClient worldToImageClient_; + ros::ServiceClient imageToWorldClient_; + + bool imageToWorld(const cv::Point &image_point, cv::Point2f &world_point); + bool worldToImage(const cv::Point2f &world_point, cv::Point &image_point); + std::vector findBySobel(cv::LineIterator it, const float lineWidth, const float iDist, diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 2264f1b..7f2f357 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -6,6 +6,8 @@ #include #include #include +#include +#include namespace drive_ros_image_recognition { @@ -16,6 +18,10 @@ class WarpContent { bool init(); private: void world_image_callback(const sensor_msgs::ImageConstPtr& msg); + bool worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, + drive_ros_image_recognition::WorldToImage::Response &res); + bool imageToWorld(drive_ros_image_recognition::ImageToWorld::Request &req, + drive_ros_image_recognition::ImageToWorld::Response &res); ros::NodeHandle pnh_; cv::Mat current_image_; // needs two components: camera model and homography for transformation @@ -28,6 +34,10 @@ class WarpContent { image_transport::ImageTransport it_; image_transport::Subscriber img_sub_; image_transport::Publisher img_pub_; + ros::ServiceServer worldToImageServer_; + ros::ServiceServer imageToWorldServer_; + // todo: move to using this in the future +// sensor_msgs::CameraInfo cam_info_; }; } // namespace drive_ros_image_recognition diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 87e8be3..6b6fcd6 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -1,4 +1,6 @@ #include "drive_ros_image_recognition/new_road_detection.h" +#include +#include //#include //#include //#include @@ -37,7 +39,9 @@ NewRoadDetection::NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh): conditionNewLine_(), conditionLineProcessed_(), current_image_(), - current_image_sobel_() + current_image_sobel_(), + worldToImageClient_(), + imageToWorldClient_() { } @@ -101,6 +105,9 @@ bool NewRoadDetection::init() { debug_img_pub_ = nh_.advertise("/debug_image_out", 10); #endif + imageToWorldClient_ = pnh_.serviceClient("ImageToWorld"); + worldToImageClient_ = pnh_.serviceClient("WorldToImage"); + // todo: we have not decided on an interface for these debug channels yet // debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); // debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); @@ -112,6 +119,44 @@ bool NewRoadDetection::init() { return true; } +bool NewRoadDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &world_point) { + drive_ros_image_recognition::ImageToWorld srv; + geometry_msgs::Point image_point_send; + image_point_send.x = image_point.x; + image_point_send.y = image_point.y; + image_point_send.z = 0.0; + srv.request.image_point = image_point_send; + if (imageToWorldClient_.call(srv)) + { + world_point.x = srv.response.world_point.x; + world_point.y = srv.response.world_point.y; + } + else + { + ROS_ERROR("Failed to call service add_two_ints"); + return false; + } +} + +bool NewRoadDetection::worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { + drive_ros_image_recognition::WorldToImage srv; + geometry_msgs::Point world_point_send; + world_point_send.x = world_point.x; + world_point_send.y = world_point.y; + world_point_send.z = 0.0; + srv.request.world_point = world_point_send; + if (worldToImageClient_.call(srv)) + { + image_point.x = (int)srv.response.image_point.x; + image_point.y = (int)srv.response.image_point.y; + } + else + { + ROS_ERROR("Failed to call service add_two_ints"); + return false; + } +} + void NewRoadDetection::roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_) { // use simple singular buffer road_buffer_ = *road_in_; @@ -288,9 +333,8 @@ std::vector NewRoadDetection::findByBrightness( cv::LineIterator it //we found a valid point //get the middle - cv::Point2i mid = point_mid; cv::Point2f wMid; - imageToWorld(mid,wMid); + imageToWorld(point_mid,wMid); foundPoints.push_back(wMid); } tCounter = 0; @@ -336,14 +380,16 @@ bool NewRoadDetection::find(){ // todo: implement transformation functions from world to map and reversed // maybe this would be a good use for services (from the preprocessing node, has access to homography stuff anyway) int l_w_startx, l_w_starty, l_w_endx, l_w_endy; - worldToImage(l.w_start.x, l.w_start.y, l_w_startx, l_w_starty); - worldToImage(l.w_end.x, l.w_end.y, l_w_endx, l_w_endy); + cv::Point l_w_start; + cv::Point l_w_end; + worldToImage(l.w_start, l_w_start); + worldToImage(l.w_end, l_w_end); cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); - if(!img_rect.contains(cv::Point(l_w_startx,l_w_starty))){ + if(!img_rect.contains(l_w_start)){ // try middle lane -> should always be in image l.w_start = mid[i]; - worldToImage(l.w_start.x, l.w_start.y, l_w_startx, l_w_starty); - if(img_rect.contains(cv::Point(l_w_startx,l_w_starty))){ + worldToImage(l.w_start, l_w_start); + if(img_rect.contains(l_w_start)){ continue; } }else if(!img_rect.contains(cv::Point(l_w_endx,l_w_endy))){ @@ -492,8 +538,10 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { std::unique_lock lock(debugValidPointsMutex); cv::namedWindow("Filtered Points", CV_WINDOW_NORMAL); cv::Mat filtered_points_mat = current_image_->clone(); + cv::Point img_point; for (auto point : validPoints) { - cv::circle(filtered_points_mat, worldToImage(point), 2, cv::Scalar(255)); + worldToImage(point, img_point); + cv::circle(filtered_points_mat, img_point, 2, cv::Scalar(255)); } cv::imshow("Filtered Points", filtered_points_mat); } diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 285ddec..e9225f6 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -10,7 +10,9 @@ WarpContent::WarpContent(ros::NodeHandle& pnh): cam_mat_(3,3,CV_64F,cv::Scalar(0.0)), dist_coeffs_(7,1,CV_64F,cv::Scalar(0.0)), it_(pnh), - img_sub_() + img_sub_(), + worldToImageServer_(), + imageToWorldServer_() { img_pub_ = it_.advertise("warped_out", 1); } @@ -63,23 +65,7 @@ bool WarpContent::init() { cam_mat_.at(1,2) = temp_vals[3]; cam_mat_.at(2,2) = 1.0; - // retreive camera model matrix for undistortion - if (!pnh_.getParam("camera_matrix/cam_mat", temp_vals)) { - ROS_ERROR("Unable to load camera matrix parameters from configuration file!"); - return false; - } - if (temp_vals.size() != 4) { - ROS_ERROR("Retreived camera matrix does not have 4 values!"); - return false; - } - // (row,column) indexing - cam_mat_.at(0,0) = temp_vals[0]; - cam_mat_.at(1,1) = temp_vals[1]; - cam_mat_.at(0,2) = temp_vals[2]; - cam_mat_.at(1,2) = temp_vals[3]; - cam_mat_.at(2,2) = 1.0; - - // retreive camera parameter vector for undistortion + // retreive distortion parameter vector for undistortion temp_vals.clear(); if (!pnh_.getParam("camera_matrix/dist_coeffs", temp_vals)) { ROS_ERROR("Unable to load distortion coefficient vector parameters from configuration file!"); @@ -90,14 +76,16 @@ bool WarpContent::init() { return false; } // (row,column) indexing - cam_mat_.at(0,0) = temp_vals[0]; - cam_mat_.at(1,0) = temp_vals[1]; - cam_mat_.at(4,0) = temp_vals[2]; - cam_mat_.at(5,0) = temp_vals[3]; - cam_mat_.at(6,0) = temp_vals[4]; + dist_coeffs_.at(0,0) = temp_vals[0]; + dist_coeffs_.at(1,0) = temp_vals[1]; + dist_coeffs_.at(4,0) = temp_vals[2]; + dist_coeffs_.at(5,0) = temp_vals[3]; + dist_coeffs_.at(6,0) = temp_vals[4]; // initialize homography transformation subscriber img_sub_ = it_.subscribe("img_in", 10, &WarpContent::world_image_callback, this); + worldToImageServer_ = pnh_.advertiseService("WorldToImage", &WarpContent::worldToImage, this); + imageToWorldServer_ = pnh_.advertiseService("ImageToWorld", &WarpContent::imageToWorld, this); return true; } @@ -119,6 +107,48 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg) { img_pub_.publish(cv_bridge::CvImage(std_msgs::Header(), "mono16", current_image_).toImageMsg()); } +bool WarpContent::worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, + drive_ros_image_recognition::WorldToImage::Response &res) +{ + ROS_DEBUG("WorldToImage service: transforming incoming world coordinates to image coordinates"); + cv::Mat point_world = cv::Mat::zeros(3,1,CV_64F); + point_world.at(0,0) = req.world_point.x; + point_world.at(1,0) = req.world_point.y; + point_world.at(2,0) = req.world_point.z; + + cv::Mat point_image; + perspectiveTransform(point_world, point_image, world2cam_); + if (point_world.rows != 3 || point_world.cols != 1) { + ROS_WARN("Point transformed to image dimensions has invalid dimensions"); + return false; + } + res.image_point.x = point_image.at(0,0); + res.image_point.y = point_image.at(1,0); + res.image_point.z = 0.0; + // todo: move to sensor_msgs::CameraInfo for transformation, uses tf as base, could use as unified interface + return true; +} + +bool WarpContent::imageToWorld(drive_ros_image_recognition::ImageToWorld::Request &req, + drive_ros_image_recognition::ImageToWorld::Response &res) +{ + ROS_DEBUG("WorldToImage service: transforming incoming image coordinates to world coordinates"); + cv::Mat point_world = cv::Mat::zeros(3,1,CV_64F); + point_world.at(0,0) = req.image_point.x; + point_world.at(1,0) = req.image_point.y; + + cv::Mat point_image; + perspectiveTransform(point_world, point_image, cam2world_); + if (point_world.rows != 3 || point_world.cols != 1) { + ROS_WARN("Point transformed to world dimensions has invalid dimensions"); + return false; + } + res.world_point.x = point_image.at(0,0); + res.world_point.y = point_image.at(1,0); + res.world_point.z = point_image.at(2,0);; + // todo: move to sensor_msgs::CameraInfo for transformation, uses tf as base, could use as unified interface + return true; +} } // namespace drive_ros_image_recognition diff --git a/srv/ImageToWorld.srv b/srv/ImageToWorld.srv new file mode 100644 index 0000000..41837c5 --- /dev/null +++ b/srv/ImageToWorld.srv @@ -0,0 +1,5 @@ +#point in image coordinates +geometry_msgs/Point image_point +--- +#point in world coordinates +geometry_msgs/Point world_point diff --git a/srv/WorldToImage.srv b/srv/WorldToImage.srv new file mode 100644 index 0000000..47e3f6d --- /dev/null +++ b/srv/WorldToImage.srv @@ -0,0 +1,5 @@ +#point in world coordinates +geometry_msgs/Point world_point +--- +#point in image coordinates +geometry_msgs/Point image_point From 81fa3e01bb063d0e5afcd78ba66ef31b7959cb60 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Mon, 19 Jun 2017 01:57:47 +0200 Subject: [PATCH 06/94] Add common geometry operations header (based on boost::geometry), fix bugs in findbySobel(), succesfully compile --- CMakeLists.txt | 5 ++ cfg/NewRoadDetection.cfg | 2 +- .../geometry_common.h | 59 +++++++++++++++++++ .../new_road_detection.h | 6 +- src/new_road_detection.cpp | 45 +++++++------- 5 files changed, 94 insertions(+), 23 deletions(-) create mode 100644 include/drive_ros_image_recognition/geometry_common.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e8309c..858857c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,9 @@ find_package(catkin REQUIRED COMPONENTS message_generation ) +#include Boost for lane line processing +find_package(Boost REQUIRED) + add_message_files( FILES RoadLane.msg @@ -49,6 +52,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) ################################################################################ @@ -90,6 +94,7 @@ add_dependencies(new_road_detection ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXP target_link_libraries(new_road_detection ${catkin_LIBRARIES} ${OpenCV_LIBS} + ${Boost_LIBRARIES} ) install(TARGETS new_road_detection diff --git a/cfg/NewRoadDetection.cfg b/cfg/NewRoadDetection.cfg index e456566..8497f43 100755 --- a/cfg/NewRoadDetection.cfg +++ b/cfg/NewRoadDetection.cfg @@ -18,4 +18,4 @@ gen.add("useWeights", bool_t, 0, "Use weights", False) gen.add("renderDebugImage", bool_t, 0, "Render debug image", False) gen.add("threads", int_t, 0, "Number of threads", 4, 0, 8) -exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "new_road_detection")) +exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "NewRoadDetection")) diff --git a/include/drive_ros_image_recognition/geometry_common.h b/include/drive_ros_image_recognition/geometry_common.h new file mode 100644 index 0000000..782eb92 --- /dev/null +++ b/include/drive_ros_image_recognition/geometry_common.h @@ -0,0 +1,59 @@ +// todo: move this to common package, will be used for trajectory as well +#ifndef GEOMETRY_COMMON_H +#define GEOMETRY_COMMON_H +#include +#include + +namespace trans = boost::geometry::strategy::transform; +namespace bg = boost::geometry; + +typedef boost::geometry::model::d2::point_xy point_xy; +typedef boost::geometry::model::linestring linestring; +typedef boost::geometry::model::referring_segment segment; +typedef boost::geometry::model::segment segment_hc; + + +namespace drive_ros_geometry_common { + +const trans::rotate_transformer rotate(90.0); + +template +struct orthogonalHelper{ + orthogonalHelper(double distance) : distance_(distance){} + + inline void operator()(Segment &s){ + // offset 90 degrees + segment_hc rot; + boost::geometry::transform(s, rot, rotate); + + // normalize + double length = boost::geometry::distance(s.first, s.second); + trans::scale_transformer scale(distance_/length); + boost::geometry::transform(rot, rot, scale); + + // offset original segment + trans::translate_transformer translate(bg::get<0>(rot.second)-bg::get<0>(rot.first), bg::get<1>(rot.second)-bg::get<1>(rot.first)); + boost::geometry::transform(s,s,translate); + } + + double distance_; +}; + + +inline void moveOrthogonal (const linestring &lines_in, linestring &lines_out, double distance) { + lines_out = lines_in; + orthogonalHelper helper(distance); + bg::for_each_segment(lines_out, helper); + return ; +} + +// in case we want to change interface -> boost esoterics are annoying +inline void simplify (const linestring &lines_in, linestring &lines_out, double distance ) { + bg::simplify(lines_in, lines_out, distance); + return ; +} + +} // namespace drive_ros_geometry_common + +#endif // GEOMETRY_COMMON_H + diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index f4b7aa1..a71a0f0 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -9,6 +9,7 @@ #include #include #include +#include //#include //#include //#include @@ -28,9 +29,10 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) CvImagePtr cv_ptr; try { - // todo: check if this if we have 8 or 16 greyscale images // hardcopies for now, might be possible to process on pointer if fast enough - cv_ptr = cv_bridge::toCvCopy(img_in, "mono16"); + // todo: make prettier + cv_bridge::CvImagePtr temp_ptr = cv_bridge::toCvCopy(*img_in, ""); + cv_ptr.reset(new cv::Mat(temp_ptr->image) ); } catch (cv_bridge::Exception& e) { diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 6b6fcd6..e744b35 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -41,7 +41,8 @@ NewRoadDetection::NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh): current_image_(), current_image_sobel_(), worldToImageClient_(), - imageToWorldClient_() + imageToWorldClient_(), + road_buffer_() { } @@ -99,7 +100,7 @@ bool NewRoadDetection::init() { img_sub_ = it_.subscribe("image", 10, &NewRoadDetection::imageCallback, this); road_sub_ = nh_.subscribe("road_in", 100, &NewRoadDetection::roadCallback, this); - line_output_pub_ = nh_.advertise("line_out",10); + line_output_pub_ = nh_.advertise("line_out",10); #ifdef DRAW_DEBUG debug_img_pub_ = nh_.advertise("/debug_image_out", 10); @@ -359,41 +360,45 @@ bool NewRoadDetection::find(){ } //create left/mid/right lane - // todo: move to eigen and port this - std::vector mid = road->getWithDistanceBetweenPoints(config().get("distanceBetweenSearchlines",0.2)); - std::vector left = mid.moveOrthogonal(-0.4); - std::vector right = mid.moveOrthogonal(0.4); + linestring mid, left, right; + for (auto point : road_buffer_.points) { + boost::geometry::append(mid,point_xy(point.x, point.y)); + } + // simplify mid (previously done with distance-based algorithm + drive_ros_geometry_common::simplify(mid, mid, distanceBetweenSearchlines_); + drive_ros_geometry_common::moveOrthogonal(mid, left, -0.4); + drive_ros_geometry_common::moveOrthogonal(mid, right, 0.4); if(mid.size() != left.size() || mid.size() != right.size()){ ROS_ERROR("Generated lane sizes do not match! Aborting search!"); return false; } //get all lines - for(int i = 0; i< mid.size(); i++){ + auto it_mid = mid.begin(); + auto it_left = left.begin(); + auto it_right = right.begin(); + for(auto it = 0; it(*it_left),bg::get<1>(*it_left)); + l.w_end = cv::Point2f(bg::get<0>(*it_right),bg::get<1>(*it_right)); + l.w_left = cv::Point2f(bg::get<0>(*it_left),bg::get<1>(*it_left)); + l.w_mid = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); + l.w_right = cv::Point2f(bg::get<0>(*it_right),bg::get<1>(*it_right)); //check if the part is valid (middle should be always visible) - // todo: implement transformation functions from world to map and reversed - // maybe this would be a good use for services (from the preprocessing node, has access to homography stuff anyway) - int l_w_startx, l_w_starty, l_w_endx, l_w_endy; - cv::Point l_w_start; - cv::Point l_w_end; + cv::Point l_w_start, l_w_end; worldToImage(l.w_start, l_w_start); worldToImage(l.w_end, l_w_end); cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); if(!img_rect.contains(l_w_start)){ // try middle lane -> should always be in image - l.w_start = mid[i]; + l.w_start = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); worldToImage(l.w_start, l_w_start); if(img_rect.contains(l_w_start)){ continue; } - }else if(!img_rect.contains(cv::Point(l_w_endx,l_w_endy))){ - l.w_end = mid[i]; + }else if(!img_rect.contains(l_w_end)){ + l.w_end = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); } //transform them in image-coordinates From d32f0ba4b5ca43bb95c11cd40cc55fbeb3ce50dd Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Wed, 21 Jun 2017 20:23:29 +0200 Subject: [PATCH 07/94] Fixed config header naming --- cfg/NewRoadDetection.cfg | 1 + include/drive_ros_image_recognition/new_road_detection.h | 8 ++++---- src/new_road_detection.cpp | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/cfg/NewRoadDetection.cfg b/cfg/NewRoadDetection.cfg index 8497f43..424052a 100755 --- a/cfg/NewRoadDetection.cfg +++ b/cfg/NewRoadDetection.cfg @@ -19,3 +19,4 @@ gen.add("renderDebugImage", bool_t, 0, "Render debug image", False) gen.add("threads", int_t, 0, "Number of threads", 4, 0, 8) exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "NewRoadDetection")) + diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index a71a0f0..8cd877e 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include //#include //#include @@ -122,9 +122,9 @@ class NewRoadDetection { void roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_); void imageCallback(const sensor_msgs::ImageConstPtr& img_in); - dynamic_reconfigure::Server dsrv_server_; - dynamic_reconfigure::Server::CallbackType dsrv_cb_; - void reconfigureCB(drive_ros_image_recognition::new_road_detectionConfig& config, uint32_t level); + dynamic_reconfigure::Server dsrv_server_; + dynamic_reconfigure::Server::CallbackType dsrv_cb_; + void reconfigureCB(drive_ros_image_recognition::NewRoadDetectionConfig& config, uint32_t level); bool find(); ros::ServiceClient worldToImageClient_; diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index e744b35..e550254 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -610,7 +610,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { conditionLineProcessed_.notify_all(); } -void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::new_road_detectionConfig& config, uint32_t level){ +void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::NewRoadDetectionConfig& config, uint32_t level){ searchOffset_ = config.searchOffset; findPointsBySobel_ = config.findBySobel; renderDebugImage_ = config.renderDebugImage; From 11ce761b53ede27afc6a2c3f49621476afd6ad60 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Wed, 21 Jun 2017 23:40:18 +0200 Subject: [PATCH 08/94] Warp node functioning, but homography seems incorrect, added debug publisher for undistorted image --- config/camera_matrix.yaml | 4 +- .../drive_ros_image_recognition/warp_image.h | 2 + launch/warp_image.launch | 1 + src/warp_image.cpp | 71 +++++++++++++------ 4 files changed, 56 insertions(+), 22 deletions(-) diff --git a/config/camera_matrix.yaml b/config/camera_matrix.yaml index f4bf2ea..5b6c775 100644 --- a/config/camera_matrix.yaml +++ b/config/camera_matrix.yaml @@ -1,6 +1,6 @@ camera_matrix: # ordered as: Fx, Fy, Cx, Cy cam_mat: [4.558675e+02, 4.569925e+02, 3.633525e+02, 1.724759e+02] - # ordered as: K1, K2, K3, K4, K5 - dist_coeffs: [-2.994090e-01, 9.807324e-02, -2.577654e-03, -9.520364e-04, 0.0] + # ordered as: K1, K2, K3, K4, K5, K6 (assumed as 0, not in code) + dist_coeffs: [-2.994090e-01, 9.807324e-02, -2.577654e-03, -9.520364e-04, 0.0, 0.0] # assuming no tangential distortion (p1, p2 = 0) diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 7f2f357..9d756e8 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -34,6 +34,8 @@ class WarpContent { image_transport::ImageTransport it_; image_transport::Subscriber img_sub_; image_transport::Publisher img_pub_; + // debug undistort publisher + image_transport::Publisher undistort_pub_; ros::ServiceServer worldToImageServer_; ros::ServiceServer imageToWorldServer_; // todo: move to using this in the future diff --git a/launch/warp_image.launch b/launch/warp_image.launch index 976ac13..a4a2354 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -2,5 +2,6 @@ + diff --git a/src/warp_image.cpp b/src/warp_image.cpp index e9225f6..c98002c 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -8,13 +8,14 @@ WarpContent::WarpContent(ros::NodeHandle& pnh): world2cam_(3,3,CV_64F,cv::Scalar(0.0)), cam2world_(3,3,CV_64F,cv::Scalar(0.0)), cam_mat_(3,3,CV_64F,cv::Scalar(0.0)), - dist_coeffs_(7,1,CV_64F,cv::Scalar(0.0)), + dist_coeffs_(8,1,CV_64F,cv::Scalar(0.0)), it_(pnh), img_sub_(), worldToImageServer_(), imageToWorldServer_() { img_pub_ = it_.advertise("warped_out", 1); + undistort_pub_ = it_.advertise("undistort_out", 1); } WarpContent::~WarpContent() { @@ -32,8 +33,20 @@ bool WarpContent::init() { return false; } for(unsigned i=0; i < temp_vals.size(); i++) { - world2cam_.at(i) = temp_vals[i]; + // todo: check: according to warp_cpp.h, those two might actually be the other way round + world2cam_.at(i) = temp_vals[i]; } + // todo: if the index does not match opencv default +// world2cam_.at(0,0) = temp_vals[0]; +// world2cam_.at(1,0) = temp_vals[1]; +// world2cam_.at(2,0) = temp_vals[2]; +// world2cam_.at(0,1) = temp_vals[3]; +// world2cam_.at(1,1) = temp_vals[4]; +// world2cam_.at(2,1) = temp_vals[5]; +// world2cam_.at(0,2) = temp_vals[6]; +// world2cam_.at(1,2) = temp_vals[7]; +// world2cam_.at(2,2) = temp_vals[8]; + ROS_INFO_STREAM("World2cam loaded as: "<(i) = temp_vals[i]; + // todo: check: according to warp_cpp.h, those two might actually be the other way round + cam2world_.at(i) = temp_vals[i]; } + ROS_INFO_STREAM("Cam2World loaded as: "<(0,0) = temp_vals[0]; - cam_mat_.at(1,1) = temp_vals[1]; - cam_mat_.at(0,2) = temp_vals[2]; - cam_mat_.at(1,2) = temp_vals[3]; - cam_mat_.at(2,2) = 1.0; + // Fx: + cam_mat_.at(0,0) = temp_vals[0]; + // Fy: + cam_mat_.at(1,1) = temp_vals[1]; + // Cx: + cam_mat_.at(0,2) = temp_vals[2]; + // Cy: + cam_mat_.at(1,2) = temp_vals[3]; + cam_mat_.at(2,2) = 1.0; + ROS_INFO_STREAM("Camera matrix loaded as: "<(0,0) = temp_vals[0]; - dist_coeffs_.at(1,0) = temp_vals[1]; - dist_coeffs_.at(4,0) = temp_vals[2]; - dist_coeffs_.at(5,0) = temp_vals[3]; - dist_coeffs_.at(6,0) = temp_vals[4]; + // K1 + dist_coeffs_.at(0,0) = temp_vals[0]; + // K2 + dist_coeffs_.at(1,0) = temp_vals[1]; + // K3 + dist_coeffs_.at(4,0) = temp_vals[2]; + // K4 + dist_coeffs_.at(5,0) = temp_vals[3]; + // K5 + dist_coeffs_.at(6,0) = temp_vals[4]; + // K6 + dist_coeffs_.at(7,0) = temp_vals[5]; + ROS_INFO_STREAM("Distortion coefficients loaded as: "<image; + // copy + current_image_ = cv_bridge::toCvCopy(msg, "")->image; } catch (cv_bridge::Exception& e) { ROS_WARN("Could not convert incoming image from '%s' to 'CV_16U', skipping.", msg->encoding.c_str()); return; } + // todo: extract required region from image, defined in the config, should be done in the camera // undistort and apply homography transformation - cv::undistort(current_image_, current_image_, cam_mat_, dist_coeffs_); - cv::perspectiveTransform(current_image_, current_image_, world2cam_); - img_pub_.publish(cv_bridge::CvImage(std_msgs::Header(), "mono16", current_image_).toImageMsg()); + cv::Mat undistorted_mat; + cv::undistort(current_image_, undistorted_mat, cam_mat_, dist_coeffs_); + undistort_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); + cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size()); + img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } bool WarpContent::worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, From e3c9531ccc83d1bcdb9f3751d1ebb92dd8b353c0 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Thu, 22 Jun 2017 01:03:01 +0200 Subject: [PATCH 09/94] Homography scaling --- src/warp_image.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/warp_image.cpp b/src/warp_image.cpp index c98002c..1f32c2f 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -61,6 +61,15 @@ bool WarpContent::init() { // todo: check: according to warp_cpp.h, those two might actually be the other way round cam2world_.at(i) = temp_vals[i]; } +// world2cam_.at(0,0) = temp_vals[0]; +// world2cam_.at(1,0) = temp_vals[1]; +// world2cam_.at(2,0) = temp_vals[2]; +// world2cam_.at(0,1) = temp_vals[3]; +// world2cam_.at(1,1) = temp_vals[4]; +// world2cam_.at(2,1) = temp_vals[5]; +// world2cam_.at(0,2) = temp_vals[6]; +// world2cam_.at(1,2) = temp_vals[7]; +// world2cam_.at(2,2) = temp_vals[8]; ROS_INFO_STREAM("Cam2World loaded as: "<(0,0) = 410/current_image_.rows; + S.at(1,1) = 752/current_image_.cols; // undistort and apply homography transformation cv::Mat undistorted_mat; cv::undistort(current_image_, undistorted_mat, cam_mat_, dist_coeffs_); undistort_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); - cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size()); + cv::warpPerspective(undistorted_mat, undistorted_mat, S*cam2world_*S.inv(), current_image_.size()); img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } @@ -153,6 +169,7 @@ bool WarpContent::worldToImage(drive_ros_image_recognition::WorldToImage::Reques ROS_WARN("Point transformed to image dimensions has invalid dimensions"); return false; } + res.image_point.x = point_image.at(0,0); res.image_point.y = point_image.at(1,0); res.image_point.z = 0.0; From ebdcbcd23033ae216f3e763e2774932d580a2761 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Fri, 23 Jun 2017 00:45:38 +0200 Subject: [PATCH 10/94] Added image warp nodelet, fixed services for image transformation --- CMakeLists.txt | 25 ++++++ .../drive_ros_image_recognition/warp_image.h | 11 ++- launch/warp_image_nodelet.launch | 11 +++ package.xml | 7 +- src/warp_image.cpp | 88 +++++++++---------- warp_image_nodelet.xml | 7 ++ 6 files changed, 98 insertions(+), 51 deletions(-) create mode 100644 launch/warp_image_nodelet.launch create mode 100644 warp_image_nodelet.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 858857c..ef050c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure geometry_msgs message_generation + nodelet ) #include Boost for lane line processing @@ -124,6 +125,30 @@ install(TARGETS warp_image_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +################################################################################ +# Warp image nodelet +################################################################################ +add_library(warp_image_nodelet + src/warp_image.cpp + ) + +add_dependencies(warp_image_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(warp_image_nodelet + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ) + +install(TARGETS warp_image_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +install(FILES warp_image_nodelet.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + ################################################################################ # Image publisher node ################################################################################ diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 9d756e8..021e1f2 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -9,11 +9,13 @@ #include #include +#include + namespace drive_ros_image_recognition { class WarpContent { public: - WarpContent(ros::NodeHandle& pnh); + WarpContent(const ros::NodeHandle& pnh); ~WarpContent(); bool init(); private: @@ -42,6 +44,13 @@ class WarpContent { // sensor_msgs::CameraInfo cam_info_; }; +class WarpImageNodelet : public nodelet::Nodelet { +public: + virtual void onInit(); +private: + std::unique_ptr my_content_; +}; + } // namespace drive_ros_image_recognition #endif diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch new file mode 100644 index 0000000..51c0021 --- /dev/null +++ b/launch/warp_image_nodelet.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/package.xml b/package.xml index e16459e..3d111f1 100644 --- a/package.xml +++ b/package.xml @@ -49,6 +49,7 @@ message_generation geometry_msgs + nodelet cv_bridge image_transport @@ -59,10 +60,10 @@ message_runtime geometry_msgs + nodelet - - - + + diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 1f32c2f..1d0b633 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -1,8 +1,9 @@ #include +#include namespace drive_ros_image_recognition { -WarpContent::WarpContent(ros::NodeHandle& pnh): +WarpContent::WarpContent(const ros::NodeHandle& pnh): pnh_(pnh), current_image_(), world2cam_(3,3,CV_64F,cv::Scalar(0.0)), @@ -24,6 +25,7 @@ WarpContent::~WarpContent() { bool WarpContent::init() { // retrieve world2map and map2world homography matrices std::vector temp_vals; + if (!pnh_.getParam("homography_matrix/world2cam", temp_vals)) { ROS_ERROR("Unable to load world2cam homography matrix from configuration file!"); return false; @@ -36,17 +38,7 @@ bool WarpContent::init() { // todo: check: according to warp_cpp.h, those two might actually be the other way round world2cam_.at(i) = temp_vals[i]; } - // todo: if the index does not match opencv default -// world2cam_.at(0,0) = temp_vals[0]; -// world2cam_.at(1,0) = temp_vals[1]; -// world2cam_.at(2,0) = temp_vals[2]; -// world2cam_.at(0,1) = temp_vals[3]; -// world2cam_.at(1,1) = temp_vals[4]; -// world2cam_.at(2,1) = temp_vals[5]; -// world2cam_.at(0,2) = temp_vals[6]; -// world2cam_.at(1,2) = temp_vals[7]; -// world2cam_.at(2,2) = temp_vals[8]; - ROS_INFO_STREAM("World2cam loaded as: "<(i) = temp_vals[i]; } -// world2cam_.at(0,0) = temp_vals[0]; -// world2cam_.at(1,0) = temp_vals[1]; -// world2cam_.at(2,0) = temp_vals[2]; -// world2cam_.at(0,1) = temp_vals[3]; -// world2cam_.at(1,1) = temp_vals[4]; -// world2cam_.at(2,1) = temp_vals[5]; -// world2cam_.at(0,2) = temp_vals[6]; -// world2cam_.at(1,2) = temp_vals[7]; -// world2cam_.at(2,2) = temp_vals[8]; - ROS_INFO_STREAM("Cam2World loaded as: "<(1,2) = temp_vals[3]; cam_mat_.at(2,2) = 1.0; - ROS_INFO_STREAM("Camera matrix loaded as: "<(6,0) = temp_vals[4]; // K6 dist_coeffs_.at(7,0) = temp_vals[5]; - ROS_INFO_STREAM("Distortion coefficients loaded as: "<encoding.c_str()); return; } - // todo: extract required region from image, defined in the config, should be done in the camera + + // optionally: scale homography, as image is too small // cv::Rect roi(cv::Point(current_image_.cols/2-(410/2),0),cv::Point(current_image_.cols/2+(410/2),current_image_.rows)); - cv::Rect roi(cv::Point(current_image_.cols/2-(410/2),0),cv::Point(current_image_.cols/2+(410/2),current_image_.rows)); - current_image_ = current_image_(roi); - // scale homography? - cv::Mat S = cv::Mat::eye(3,3,CV_64F); - S.at(0,0) = 410/current_image_.rows; - S.at(1,1) = 752/current_image_.cols; +// current_image_ = current_image_(roi); +// cv::Mat S = cv::Mat::eye(3,3,CV_64F); +// S.at(0,0) = 410/current_image_.rows; +// S.at(1,1) = 752/current_image_.cols; // undistort and apply homography transformation cv::Mat undistorted_mat; cv::undistort(current_image_, undistorted_mat, cam_mat_, dist_coeffs_); undistort_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); - cv::warpPerspective(undistorted_mat, undistorted_mat, S*cam2world_*S.inv(), current_image_.size()); - img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); + // opionally: apply scaled homography + // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); + + // flag ensures that we directly use the matrix, as it is done in LMS + cv::warpPerspective(current_image_, current_image_, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); } bool WarpContent::worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, drive_ros_image_recognition::WorldToImage::Response &res) { ROS_DEBUG("WorldToImage service: transforming incoming world coordinates to image coordinates"); - cv::Mat point_world = cv::Mat::zeros(3,1,CV_64F); + cv::Mat point_world(1,1,CV_64FC2); point_world.at(0,0) = req.world_point.x; - point_world.at(1,0) = req.world_point.y; - point_world.at(2,0) = req.world_point.z; + point_world.at(0,1) = req.world_point.y; + point_world.at(0,2) = req.world_point.z; - cv::Mat point_image; - perspectiveTransform(point_world, point_image, world2cam_); - if (point_world.rows != 3 || point_world.cols != 1) { + cv::Mat point_image(1,1,CV_64FC2); + cv::perspectiveTransform(point_world, point_image, world2cam_); + if (point_world.rows != 1 || point_world.cols != 1) { ROS_WARN("Point transformed to image dimensions has invalid dimensions"); return false; } res.image_point.x = point_image.at(0,0); - res.image_point.y = point_image.at(1,0); + res.image_point.y = point_image.at(0,1); res.image_point.z = 0.0; // todo: move to sensor_msgs::CameraInfo for transformation, uses tf as base, could use as unified interface return true; @@ -181,22 +166,31 @@ bool WarpContent::imageToWorld(drive_ros_image_recognition::ImageToWorld::Reques drive_ros_image_recognition::ImageToWorld::Response &res) { ROS_DEBUG("WorldToImage service: transforming incoming image coordinates to world coordinates"); - cv::Mat point_world = cv::Mat::zeros(3,1,CV_64F); + cv::Mat point_world(1,1,CV_64FC2); point_world.at(0,0) = req.image_point.x; - point_world.at(1,0) = req.image_point.y; + point_world.at(0,1) = req.image_point.y; - cv::Mat point_image; - perspectiveTransform(point_world, point_image, cam2world_); - if (point_world.rows != 3 || point_world.cols != 1) { + cv::Mat point_image(1,1,CV_64FC2); + cv::perspectiveTransform(point_world, point_image, cam2world_); + if (point_world.rows != 1 || point_world.cols != 1) { ROS_WARN("Point transformed to world dimensions has invalid dimensions"); return false; } res.world_point.x = point_image.at(0,0); - res.world_point.y = point_image.at(1,0); - res.world_point.z = point_image.at(2,0);; + res.world_point.y = point_image.at(0,1); + res.world_point.z = point_image.at(0,2); // todo: move to sensor_msgs::CameraInfo for transformation, uses tf as base, could use as unified interface return true; } +void WarpImageNodelet::onInit() +{ + my_content_.reset(new WarpContent(ros::NodeHandle("~"))); + if (!my_content_->init()) { + ROS_ERROR("WarpImageNodelet failed to initialize!"); + } +} + } // namespace drive_ros_image_recognition +PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::WarpImageNodelet, nodelet::Nodelet) diff --git a/warp_image_nodelet.xml b/warp_image_nodelet.xml new file mode 100644 index 0000000..b89baf7 --- /dev/null +++ b/warp_image_nodelet.xml @@ -0,0 +1,7 @@ + + + + Image warper nodelet. + + + From 0e7f387815e48aca835702c43d88f5fb4c63e36d Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Fri, 23 Jun 2017 23:49:33 +0200 Subject: [PATCH 11/94] Added line recognition nodelet, added nodelet and node launch files, fixed bugs in warp and line search, debugging line search --- CMakeLists.txt | 48 +- .../new_road_detection.h | 21 +- launch/new_road_detection.launch | 10 +- launch/new_road_detection_nodelet.launch | 31 + launch/vision.launch | 20 + launch/vision_nodelet.launch | 45 + launch/warp_image.launch | 9 +- launch/warp_image_nodelet.launch | 40 +- nodelets.xml | 17 + package.xml | 2 +- scripts/publish_road.sh | 15 + src/new_road_detection.cpp | 805 +++++++++--------- src/new_road_detection_node.cpp | 5 +- src/warp_image.cpp | 10 +- src/warp_image_node.cpp | 3 + warp_image_nodelet.xml | 7 - 16 files changed, 655 insertions(+), 433 deletions(-) create mode 100644 launch/new_road_detection_nodelet.launch create mode 100644 launch/vision.launch create mode 100644 launch/vision_nodelet.launch create mode 100644 nodelets.xml create mode 100755 scripts/publish_road.sh delete mode 100644 warp_image_nodelet.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index ef050c1..eeef236 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,11 @@ project(drive_ros_image_recognition) #SET(CMAKE_BUILD_TYPE Release) SET(CMAKE_BUILD_TYPE Debug) +# enable to compile with OpenCV window displays +add_definitions(-DDRAW_DEBUG) +# enable to compile with publishers of internally processed images +add_definitions(-DPUBLISH_DEBUG) + add_definitions(-std=c++11) find_package(catkin REQUIRED COMPONENTS @@ -59,6 +64,7 @@ include_directories( ################################################################################ # Line recognition node -> will leave this in for now, might be useful ################################################################################ + add_executable(line_recognition_node src/line_recognition_node.cpp src/line_recognition.cpp @@ -80,25 +86,45 @@ install(TARGETS line_recognition_node ) ################################################################################ -# New road detection (LMS port) +# New road detection node (LMS port) ################################################################################ -# enable this to get debug publishing from the node -#add_definitions(-DDRAW_DEBUG=True) -add_executable(new_road_detection +add_executable(new_road_detection_node src/new_road_detection.cpp src/new_road_detection_node.cpp ) -add_dependencies(new_road_detection ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(new_road_detection_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(new_road_detection +target_link_libraries(new_road_detection_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ) -install(TARGETS new_road_detection +install(TARGETS new_road_detection_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +################################################################################ +# New road detection nodelet (LMS port) +################################################################################ + +add_library(new_road_detection_nodelet + src/new_road_detection.cpp + ) + +add_dependencies(new_road_detection_nodelet ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(new_road_detection_nodelet + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + ) + +install(TARGETS new_road_detection_nodelet ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -145,10 +171,6 @@ install(TARGETS warp_image_nodelet RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(FILES warp_image_nodelet.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - ) - ################################################################################ # Image publisher node ################################################################################ @@ -172,6 +194,10 @@ install(TARGETS image_publisher_node ################################################################################ # Install other files ################################################################################ +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index 8cd877e..a4bb738 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -10,6 +10,8 @@ #include #include #include +#include + //#include //#include //#include @@ -48,6 +50,8 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) return cv_ptr; } +namespace drive_ros_image_recognition { + /** * @brief Port of LMS module new_road_detection to ROS **/ @@ -66,7 +70,7 @@ class NewRoadDetection { int sobelThreshold_; int numThreads_; // 0 means single threaded - // todo: port this + // ported in the WarpImage class, ports the entire image already // lms::imaging::Homography homo; //Datachannels // todo: we have not determined all interfaces yet, so will leave this in for now until we have figured it out @@ -104,19 +108,20 @@ class NewRoadDetection { CvImagePtr current_image_; CvImagePtr current_image_sobel_; +#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) cv::Mat debug_image_; +#endif image_transport::ImageTransport it_; image_transport::Subscriber img_sub_; // road inputs and outputs ros::Subscriber road_sub_; ros::Publisher line_output_pub_; -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG image_transport::Publisher debug_img_pub_; #endif ros::NodeHandle nh_; ros::NodeHandle pnh_; - std::string my_namespace_; drive_ros_image_recognition::RoadLane road_buffer_; void roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_); @@ -147,9 +152,17 @@ class NewRoadDetection { void threadFunction(); public: - NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh); + NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); ~NewRoadDetection(); bool init(); }; +class NewRoadDetectionNodelet: public nodelet::Nodelet { +public: + virtual void onInit(); +private: + std::unique_ptr new_road_detection_; +}; + +} #endif // NEW_ROAD_DETECTION_H diff --git a/launch/new_road_detection.launch b/launch/new_road_detection.launch index 921f282..d74b004 100644 --- a/launch/new_road_detection.launch +++ b/launch/new_road_detection.launch @@ -1,8 +1,10 @@ - - - + + + + + + diff --git a/launch/new_road_detection_nodelet.launch b/launch/new_road_detection_nodelet.launch new file mode 100644 index 0000000..7cfdcf9 --- /dev/null +++ b/launch/new_road_detection_nodelet.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/vision.launch b/launch/vision.launch new file mode 100644 index 0000000..23e55af --- /dev/null +++ b/launch/vision.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/launch/vision_nodelet.launch b/launch/vision_nodelet.launch new file mode 100644 index 0000000..ccdf5bf --- /dev/null +++ b/launch/vision_nodelet.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/warp_image.launch b/launch/warp_image.launch index a4a2354..48f9e90 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -1,7 +1,14 @@ + + + + - + + + + diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch index 51c0021..acde656 100644 --- a/launch/warp_image_nodelet.launch +++ b/launch/warp_image_nodelet.launch @@ -1,11 +1,33 @@ - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodelets.xml b/nodelets.xml new file mode 100644 index 0000000..9450a56 --- /dev/null +++ b/nodelets.xml @@ -0,0 +1,17 @@ + + + + + Image warper nodelet. + + + + + + + + + Image warper nodelet. + + + diff --git a/package.xml b/package.xml index 3d111f1..f1f997c 100644 --- a/package.xml +++ b/package.xml @@ -63,7 +63,7 @@ nodelet - + diff --git a/scripts/publish_road.sh b/scripts/publish_road.sh new file mode 100755 index 0000000..6b9c03e --- /dev/null +++ b/scripts/publish_road.sh @@ -0,0 +1,15 @@ +# simple publisher for road messages to be able to view what happens during processing +rostopic pub /road_trajectory drive_ros_image_recognition/RoadLane "header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' +points: +- x: 0.0 + y: 0.0 + z: 0.0 +- x: 1.0 + y: 0.0 + z: 0.0 +roadStateType: 0" -r 10 diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index e550254..03b19ce 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -1,6 +1,7 @@ #include "drive_ros_image_recognition/new_road_detection.h" #include #include +#include //#include //#include //#include @@ -9,11 +10,12 @@ //#include //#include -NewRoadDetection::NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh): +namespace drive_ros_image_recognition { + +NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh): nh_(nh), pnh_(pnh), img_sub_(), - my_namespace_("new_road_detection"), searchOffset_(0.15), distanceBetweenSearchlines_(0.1), minLineWidthMul_(0.5), @@ -27,9 +29,14 @@ NewRoadDetection::NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh): renderDebugImage_(false), numThreads_(4), road_sub_(), +#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) debug_image_(0,0,CV_16UC1), +#endif line_output_pub_(), it_(pnh), +#ifdef PUBLISH_DEBUG + debug_img_pub_(), +#endif dsrv_server_(), dsrv_cb_(boost::bind(&NewRoadDetection::reconfigureCB, this, _1, _2)), lines_(), @@ -48,95 +55,94 @@ NewRoadDetection::NewRoadDetection(ros::NodeHandle nh, ros::NodeHandle pnh): bool NewRoadDetection::init() { // load parameters - if (!pnh_.getParam(my_namespace_+"searchOffset", searchOffset_)) { + if (!pnh_.getParam("new_road_detection/searchOffset", searchOffset_)) { ROS_WARN_STREAM("Unable to load 'searchOffset' parameter, using default: "<("line_out",10); + img_sub_ = it_.subscribe("img_in", 10, &NewRoadDetection::imageCallback, this); + road_sub_ = pnh_.subscribe("road_in", 100, &NewRoadDetection::roadCallback, this); + line_output_pub_ = pnh_.advertise("line_out",10); #ifdef DRAW_DEBUG - debug_img_pub_ = nh_.advertise("/debug_image_out", 10); + debug_img_pub_ = it_.advertise("debug_image_out", 10); #endif - imageToWorldClient_ = pnh_.serviceClient("ImageToWorld"); - worldToImageClient_ = pnh_.serviceClient("WorldToImage"); - - // todo: we have not decided on an interface for these debug channels yet -// debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); -// debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); -// debugTranslatedPoints = writeChannel("DEBUG_VALID_TRANSLATED_POINTS"); + imageToWorldClient_ = pnh_.serviceClient("ImageToWorld"); + worldToImageClient_ = pnh_.serviceClient("WorldToImage"); - // todo: we have not defined the interface for this yet -// car = readChannel("CAR"); //TODO create ego-estimation service + // todo: we have not decided on an interface for these debug channels yet + // debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); + // debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); + // debugTranslatedPoints = writeChannel("DEBUG_VALID_TRANSLATED_POINTS"); - return true; + // todo: we have not defined the interface for this yet + // car = readChannel("CAR"); //TODO create ego-estimation service + return true; } bool NewRoadDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &world_point) { - drive_ros_image_recognition::ImageToWorld srv; - geometry_msgs::Point image_point_send; - image_point_send.x = image_point.x; - image_point_send.y = image_point.y; - image_point_send.z = 0.0; - srv.request.image_point = image_point_send; - if (imageToWorldClient_.call(srv)) - { + drive_ros_image_recognition::ImageToWorld srv; + geometry_msgs::Point image_point_send; + image_point_send.x = image_point.x; + image_point_send.y = image_point.y; + image_point_send.z = 0.0; + srv.request.image_point = image_point_send; + if (imageToWorldClient_.call(srv)) + { world_point.x = srv.response.world_point.x; world_point.y = srv.response.world_point.y; - } - else - { - ROS_ERROR("Failed to call service add_two_ints"); - return false; - } + } + else + { + ROS_ERROR("Failed to call service add_two_ints"); + return false; + } } bool NewRoadDetection::worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { @@ -148,8 +154,8 @@ bool NewRoadDetection::worldToImage(const cv::Point2f &world_point, cv::Point &i srv.request.world_point = world_point_send; if (worldToImageClient_.call(srv)) { - image_point.x = (int)srv.response.image_point.x; - image_point.y = (int)srv.response.image_point.y; + image_point.x = (int)srv.response.image_point.x; + image_point.y = (int)srv.response.image_point.y; } else { @@ -164,60 +170,65 @@ void NewRoadDetection::roadCallback(const drive_ros_image_recognition::RoadLaneC } void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr& img_in) { - current_image_ = convertImageMessage(img_in); - if (renderDebugImage_) { - debug_image_ = current_image_->clone(); - } - - // todo: adjust this time value - if (img_in->header.stamp.toSec() - road_buffer_.header.stamp.toSec() > 0.1) { - ROS_WARN("Outdated road callback in buffer, skipping incoming image"); - return; - } + current_image_ = convertImageMessage(img_in); +#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) + if (renderDebugImage_) { + debug_image_ = current_image_->clone(); + } +#endif - //if we have a road(?), try to find the line - if(road_buffer_.points.size() > 1){ - find(); //TODO use bool from find - } + // todo: adjust this time value +// if (img_in->header.stamp.toSec() - road_buffer_.header.stamp.toSec() > 0.1) { +// // todo: move to warn level, too spammy right now +// ROS_DEBUG("Outdated road callback in buffer, skipping incoming image"); +// return; +// } + + //if we have a road(?), try to find the line + if(road_buffer_.points.size() > 1){ + find(); //TODO use bool from find + } else { + ROS_WARN("Road buffer has no points stored"); + } - float dx = 0; - float dy = 0; - float dPhi = 0; - - //update the course - // todo: no access to car command interface yet -// if(!translate_environment_){ -// logger.info("translation")<deltaPhi(); -// dPhi = car->deltaPhi(); -// } - - // todo: no interface for this yet, create course verification service to verfiy and publish -// lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); -// if(localCourse.isValid()){ -// logger.time("localCourse"); -// localCourse->update(dx,dy,dPhi); -// *road = localCourse->getCourse(); -// logger.timeEnd("localCourse"); +// float dx = 0; +// float dy = 0; +// float dPhi = 0; + + //update the course + // todo: no access to car command interface yet + // if(!translate_environment_){ + // logger.info("translation")<deltaPhi(); + // dPhi = car->deltaPhi(); + // } + + // todo: no interface for this yet, create course verification service to verfiy and publish + // lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); + // if(localCourse.isValid()){ + // logger.time("localCourse"); + // localCourse->update(dx,dy,dPhi); + // *road = localCourse->getCourse(); + // logger.timeEnd("localCourse"); #ifdef DRAW_DEBUG - // todo: no interface for this yet -// if(renderDebugImage){ -// debugTranslatedPoints->points() = localCourse->getPointsAdded(); -// } + // todo: no interface for this yet + // if(renderDebugImage){ + // debugTranslatedPoints->points() = localCourse->getPointsAdded(); + // } #endif - // should do something like this -// line_output_pub_.publish(road_translated); - -// }else{ -// ROS_ERROR("LocalCourse invalid, aborting the callback!"); -// return; -// } - return; + // should do something like this + // line_output_pub_.publish(road_translated); + + // }else{ + // ROS_ERROR("LocalCourse invalid, aborting the callback!"); + // return; + // } + return; } std::vector NewRoadDetection::findBySobel(cv::LineIterator it, - const float lineWidth, - const float iDist, - const float wDist) + const float lineWidth, + const float iDist, + const float wDist) { std::vector foundPoints; bool foundLowHigh = false; @@ -283,355 +294,363 @@ std::vector NewRoadDetection::findByBrightness( cv::LineIterator it const float iDist, const float wDist ) { - std::vector foundPoints; - std::vector color; - cv::Rect rect(cv::Point(),cv::Point(current_image_->rows, current_image_->cols)); - cv::LineIterator it_debug = it; - - for(int i = 0; i < it.count; i++, ++it) { - // safety check : is the point inside the image - if(!rect.contains(it.pos())){ - ROS_WARN("Received an invalid point outside the image to check for brightness"); - color.push_back(0); - continue; - } - color.push_back(current_image_->at(it.pos().x,it.pos().y)); + std::vector foundPoints; + std::vector color; + cv::Rect rect(cv::Point(),cv::Point(current_image_->rows, current_image_->cols)); + cv::LineIterator it_debug = it; - if (renderDebugImage_) { - cv::namedWindow("Unfiltered points processed by brightness search", CV_WINDOW_NORMAL); - cv::circle(debug_image_, it.pos(), 2, cv::Scalar(255)); - cv::imshow("Unfiltered points processed by brightness search", debug_image_); - } + for(int i = 0; i < it.count; i++, ++it) { + // safety check : is the point inside the image + if(!rect.contains(it.pos())){ + ROS_WARN("Received an invalid point outside the image to check for brightness"); + color.push_back(0); + continue; } + color.push_back(current_image_->at(it.pos().x,it.pos().y)); - //detect peaks - float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? - int tCounter = 0; - // todo: make this more efficient - for(int k = 0; k < (int)color.size(); k++){ - if(color[k]>brightness_threshold_){ - tCounter++; - }else{ - if(tCounter - k != 0 && tCounter > pxlPeakWidth*minLineWidthMul_ && tCounter < pxlPeakWidth*maxLineWidthMul_){ - for (int i = 0; i < tCounter; i++, ++it_debug) { - } - // get points for debug drawing on the way - cv::Point point_first = it_debug.pos(); - for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { - } - cv::Point point_mid = it_debug.pos(); - for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { - } - cv::Point point_end = it_debug.pos(); + if (renderDebugImage_) { + cv::namedWindow("Unfiltered points processed by brightness search", CV_WINDOW_NORMAL); + cv::circle(debug_image_, it.pos(), 2, cv::Scalar(255)); + cv::imshow("Unfiltered points processed by brightness search", debug_image_); + } + } - if (renderDebugImage_) { - cv::namedWindow("Line detected by brightness", CV_WINDOW_NORMAL); - debug_image_ = current_image_->clone(); + //detect peaks + float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? + int tCounter = 0; + // todo: make this more efficient + for(int k = 0; k < (int)color.size(); k++){ + if(color[k]>brightness_threshold_){ + tCounter++; + }else{ + if(tCounter - k != 0 && tCounter > pxlPeakWidth*minLineWidthMul_ && tCounter < pxlPeakWidth*maxLineWidthMul_){ + for (int i = 0; i < tCounter; i++, ++it_debug) { + } + // get points for debug drawing on the way + cv::Point point_first = it_debug.pos(); + for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { + } + cv::Point point_mid = it_debug.pos(); + for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { + } + cv::Point point_end = it_debug.pos(); - cv::line(debug_image_, point_first, point_end, cv::Scalar(255)); - cv::imshow("Line detected by brightness", debug_image_); - } + if (renderDebugImage_) { + cv::namedWindow("Line detected by brightness", CV_WINDOW_NORMAL); + debug_image_ = current_image_->clone(); - //we found a valid point - //get the middle - cv::Point2f wMid; - imageToWorld(point_mid,wMid); - foundPoints.push_back(wMid); + cv::line(debug_image_, point_first, point_end, cv::Scalar(255)); + cv::imshow("Line detected by brightness", debug_image_); } - tCounter = 0; + + //we found a valid point + //get the middle + cv::Point2f wMid; + imageToWorld(point_mid,wMid); + foundPoints.push_back(wMid); } + tCounter = 0; } - return foundPoints; + } + return foundPoints; } bool NewRoadDetection::find(){ - //clear old lines - ROS_INFO("Creating new lines"); - lines_.clear(); - linesToProcess_ = 0; - - // already there in LMS -// //TODO rectangle for neglecting areas - -// //(TODO calculate threshold for each line) - if(renderDebugImage_){ - //Clear debug image - debug_image_.resize(current_image_->rows,current_image_->cols); + //clear old lines + ROS_INFO("Creating new lines"); + lines_.clear(); + linesToProcess_ = 0; + + //create left/mid/right lane + linestring mid, left, right; + for (auto point : road_buffer_.points) { + boost::geometry::append(mid,point_xy(point.x, point.y)); + } + // simplify mid (previously done with distance-based algorithm + // skip this for now, can completely clear the line +// drive_ros_geometry_common::simplify(mid, mid, distanceBetweenSearchlines_); + drive_ros_geometry_common::moveOrthogonal(mid, left, -0.4); + drive_ros_geometry_common::moveOrthogonal(mid, right, 0.4); + if(mid.size() != left.size() || mid.size() != right.size()){ + ROS_ERROR("Generated lane sizes do not match! Aborting search!"); + return false; + } + //get all lines + auto it_mid = mid.begin(); + auto it_left = left.begin(); + auto it_right = right.begin(); + for(int it = 0; itx(),it_left->y()); + l.w_end = cv::Point2f(it_right->x(),it_right->y()); + l.w_left = cv::Point2f(it_left->x(),it_left->y()); + l.w_mid = cv::Point2f(it_mid->x(),it_mid->y()); + l.w_right = cv::Point2f(it_right->x(),it_right->y()); + //check if the part is valid (middle should be always visible) + + cv::Point l_w_start, l_w_end; + worldToImage(l.w_start, l_w_start); + worldToImage(l.w_end, l_w_end); + cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); + if(!img_rect.contains(l_w_start)){ + // try middle lane -> should always be in image + l.w_start = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); + worldToImage(l.w_start, l_w_start); + if(img_rect.contains(l_w_start)){ + continue; + } + }else if(!img_rect.contains(l_w_end)){ + l.w_end = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); } - //create left/mid/right lane - linestring mid, left, right; - for (auto point : road_buffer_.points) { - boost::geometry::append(mid,point_xy(point.x, point.y)); - } - // simplify mid (previously done with distance-based algorithm - drive_ros_geometry_common::simplify(mid, mid, distanceBetweenSearchlines_); - drive_ros_geometry_common::moveOrthogonal(mid, left, -0.4); - drive_ros_geometry_common::moveOrthogonal(mid, right, 0.4); - if(mid.size() != left.size() || mid.size() != right.size()){ - ROS_ERROR("Generated lane sizes do not match! Aborting search!"); - return false; - } - //get all lines - auto it_mid = mid.begin(); - auto it_left = left.begin(); - auto it_right = right.begin(); - for(auto it = 0; it(*it_left),bg::get<1>(*it_left)); - l.w_end = cv::Point2f(bg::get<0>(*it_right),bg::get<1>(*it_right)); - l.w_left = cv::Point2f(bg::get<0>(*it_left),bg::get<1>(*it_left)); - l.w_mid = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); - l.w_right = cv::Point2f(bg::get<0>(*it_right),bg::get<1>(*it_right)); - //check if the part is valid (middle should be always visible) - - cv::Point l_w_start, l_w_end; - worldToImage(l.w_start, l_w_start); - worldToImage(l.w_end, l_w_end); - cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); - if(!img_rect.contains(l_w_start)){ - // try middle lane -> should always be in image - l.w_start = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); - worldToImage(l.w_start, l_w_start); - if(img_rect.contains(l_w_start)){ - continue; - } - }else if(!img_rect.contains(l_w_end)){ - l.w_end = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); - } + //transform them in image-coordinates + std::unique_lock lock(mutex); + lines_.push_back(l); + linesToProcess_++; - //transform them in image-coordinates - std::unique_lock lock(mutex); - lines_.push_back(l); - linesToProcess_++; + // todo: figure out what this does (no internet) + // conditionNewLine.notify_one(); + } - // todo: figure out what this does (no internet) -// conditionNewLine.notify_one(); + if(numThreads_ == 0) { + // single threaded + for(SearchLine &l:lines_){ + processSearchLine(l); + } + } else { + // multi threaded + + // initialize and start threads if not yet there + if(threads_.size() == 0) { + threadsRunning_ = true; + for(int i = 0; i < numThreads_; i++) { + threads_.emplace_back([this] () { + threadFunction(); + }); + } } - if(numThreads_ == 0) { - // single threaded - for(SearchLine &l:lines_){ - processSearchLine(l); - } - } else { - // multi threaded - - // initialize and start threads if not yet there - if(threads_.size() == 0) { - threadsRunning_ = true; - for(int i = 0; i < numThreads_; i++) { - threads_.emplace_back([this] () { - threadFunction(); - }); - } - } - - // wait till every search line was processed - { - std::unique_lock lock(mutex); - while(linesToProcess_ > 0) conditionNewLine_.wait(lock); - } + // wait till every search line was processed + { + std::unique_lock lock(mutex); + while(linesToProcess_ > 0) conditionNewLine_.wait(lock); } - return true; + } + return true; } void NewRoadDetection::threadFunction() { - while(threadsRunning_) { - SearchLine line; - { - std::unique_lock lock(mutex); - while(threadsRunning_ && lines_.empty()) conditionNewLine_.wait(lock); - if(lines_.size() > 0) { - line = lines_.front(); - lines_.pop_front(); - } - if(!threadsRunning_) { - break; - } - } - processSearchLine(line); + while(threadsRunning_) { + SearchLine line; + { + std::unique_lock lock(mutex); + while(threadsRunning_ && lines_.empty()) conditionNewLine_.wait(lock); + if(lines_.size() > 0) { + line = lines_.front(); + lines_.pop_front(); + } + if(!threadsRunning_) { + break; + } } + processSearchLine(line); + } } void NewRoadDetection::processSearchLine(const SearchLine &l) { - std::vector xv; - std::vector yv; - - //calculate the offset - float iDist = cv::norm(l.i_end - l.i_start); - float wDist = cv::norm(l.w_end - l.w_start); -// float pxlPerDist = iDist/wDist*searchOffset_; -// lms::math::vertex2f iDiff = lms::math::vertex2f(l.i_start-l.i_end).normalize(); -// lms::math::vertex2f startLine = lms::math::vertex2f(l.i_start)+iDiff*pxlPerDist; -// lms::math::vertex2f endLine = lms::math::vertex2f(l.i_end)-iDiff*pxlPerDist; -// //get all points in between -// lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! - - // generate Bresenham search line - cv::LineIterator it(*current_image_,l.i_start,l.i_end); - - std::vector foundPoints; - //find points - if(findPointsBySobel_){ - // directly apply sobel operations on the image -> we only use the grad_x here as we only search horizontally - // we perform order of 1 operations only - // todo: check if CV_8UC1 is valid - cv::Sobel( *current_image_, *current_image_sobel_, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); - // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter - foundPoints = findBySobel(it,0.02,iDist,wDist); - }else{ - // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter - foundPoints = findByBrightness(it,0.02,iDist,wDist); - } + std::vector xv; + std::vector yv; + + //calculate the offset + float iDist = cv::norm(l.i_end - l.i_start); + float wDist = cv::norm(l.w_end - l.w_start); + // float pxlPerDist = iDist/wDist*searchOffset_; + // lms::math::vertex2f iDiff = lms::math::vertex2f(l.i_start-l.i_end).normalize(); + // lms::math::vertex2f startLine = lms::math::vertex2f(l.i_start)+iDiff*pxlPerDist; + // lms::math::vertex2f endLine = lms::math::vertex2f(l.i_end)-iDiff*pxlPerDist; + // //get all points in between + // lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! + + // generate Bresenham search line + cv::LineIterator it(*current_image_,l.i_start,l.i_end); - // draw unfiltered image points - if(renderDebugImage_) { - std::unique_lock lock(debugAllPointsMutex); - cv::namedWindow("Unfiltered points", CV_WINDOW_NORMAL); - cv::Mat found_points_mat = current_image_->clone(); - for (auto point : foundPoints) { - cv::circle(found_points_mat, point, 2, cv::Scalar(255)); - } - cv::imshow("Unfiltered points", found_points_mat); + std::vector foundPoints; + //find points + if(findPointsBySobel_){ + // directly apply sobel operations on the image -> we only use the grad_x here as we only search horizontally + // we perform order of 1 operations only + // todo: check if CV_8UC1 is valid + cv::Sobel( *current_image_, *current_image_sobel_, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); + // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter + foundPoints = findBySobel(it,0.02,iDist,wDist); + }else{ + // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter + foundPoints = findByBrightness(it,0.02,iDist,wDist); + } + + // draw unfiltered image points + if(renderDebugImage_) { + std::unique_lock lock(debugAllPointsMutex); + cv::namedWindow("Unfiltered points", CV_WINDOW_NORMAL); + cv::Mat found_points_mat = current_image_->clone(); + for (auto point : foundPoints) { + cv::circle(found_points_mat, point, 2, cv::Scalar(255)); } + cv::imshow("Unfiltered points", found_points_mat); + } - //filter - //TODO filter points that are in poluted regions (for example the car itself) - //remove points with bad distances - std::vector validPoints; - if(foundPoints.size() > 2){ - std::vector foundCounter; - foundCounter.resize(foundPoints.size(),0); - for(std::size_t fp = 0; fp < foundPoints.size(); fp++){ - // todo: insert service here - const cv::Point2f &s = foundPoints[fp]; - for(std::size_t test = fp+1; test 0.4-laneWidthOffsetInMeter_ && distance < 0.4 + laneWidthOffsetInMeter_)|| (distance > 0.8-laneWidthOffsetInMeter_ && distance < 0.8+laneWidthOffsetInMeter_)){ - foundCounter[test]++; - foundCounter[fp]++; - } - } + //filter + //TODO filter points that are in poluted regions (for example the car itself) + //remove points with bad distances + std::vector validPoints; + if(foundPoints.size() > 2){ + std::vector foundCounter; + foundCounter.resize(foundPoints.size(),0); + for(std::size_t fp = 0; fp < foundPoints.size(); fp++){ + // todo: insert service here + const cv::Point2f &s = foundPoints[fp]; + for(std::size_t test = fp+1; test 0.4-laneWidthOffsetInMeter_ && distance < 0.4 + laneWidthOffsetInMeter_)|| (distance > 0.8-laneWidthOffsetInMeter_ && distance < 0.8+laneWidthOffsetInMeter_)){ + foundCounter[test]++; + foundCounter[fp]++; } - //filter, all valid points should have the same counter and have the highest number - int max = 0; - for(int c:foundCounter){ - if(c > max){ - max = c; - } - } - for(int i = 0; i < (int)foundPoints.size(); i++){ - if(foundCounter[i] >= max){ - validPoints.push_back(foundPoints[i]); - } - } - //TODO if we have more than 3 points we know that there is an error! -// foundPoints = validPoints; + } } - else { - // just use the 2 points we found otherwise - validPoints = foundPoints; + //filter, all valid points should have the same counter and have the highest number + int max = 0; + for(int c:foundCounter){ + if(c > max){ + max = c; + } } - - // draw filtered points - if(renderDebugImage_) { - // todo: make some more efficient storage method here, will translate back and forth here for now - std::unique_lock lock(debugValidPointsMutex); - cv::namedWindow("Filtered Points", CV_WINDOW_NORMAL); - cv::Mat filtered_points_mat = current_image_->clone(); - cv::Point img_point; - for (auto point : validPoints) { - worldToImage(point, img_point); - cv::circle(filtered_points_mat, img_point, 2, cv::Scalar(255)); - } - cv::imshow("Filtered Points", filtered_points_mat); + for(int i = 0; i < (int)foundPoints.size(); i++){ + if(foundCounter[i] >= max){ + validPoints.push_back(foundPoints[i]); + } } + //TODO if we have more than 3 points we know that there is an error! + // foundPoints = validPoints; + } + else { + // just use the 2 points we found otherwise + validPoints = foundPoints; + } - //Handle found points - cv::Point2f diff; - std::vector distances; - for(int i = 0; i < (int)validPoints.size(); i++){ - float distanceToLeft = cv::norm(validPoints[i]-l.w_left); - float distanceToRight= cv::norm(validPoints[i]-l.w_right); - float distanceToMid= cv::norm(validPoints[i]-l.w_mid); - // normalize distance - diff = (l.w_left - l.w_right)/cv::norm(l.w_left-l.w_right); - if(distanceToLeft < distanceToMid && distanceToLeft < distanceToRight){ - //left point - validPoints[i]-=diff*0.4; - distances.push_back(distanceToLeft); - }else if(distanceToRight < distanceToMid ){ - validPoints[i]+=diff*0.4; - distances.push_back(distanceToRight); - }else{ - distances.push_back(distanceToMid); - } + // draw filtered points + if(renderDebugImage_) { + // todo: make some more efficient storage method here, will translate back and forth here for now + std::unique_lock lock(debugValidPointsMutex); + cv::namedWindow("Filtered Points", CV_WINDOW_NORMAL); + cv::Mat filtered_points_mat = current_image_->clone(); + cv::Point img_point; + for (auto point : validPoints) { + worldToImage(point, img_point); + cv::circle(filtered_points_mat, img_point, 2, cv::Scalar(255)); } + cv::imshow("Filtered Points", filtered_points_mat); + } - std::vector weights; - for(const float &dist:distances){ - //as distance is in meter, we multiply it by 100 - if(useWeights_) - weights.push_back(1/(dist*100+0.001)); //TODO hier etwas sinnvolles überlegen - else - weights.push_back(1); + //Handle found points + cv::Point2f diff; + std::vector distances; + for(int i = 0; i < (int)validPoints.size(); i++){ + float distanceToLeft = cv::norm(validPoints[i]-l.w_left); + float distanceToRight= cv::norm(validPoints[i]-l.w_right); + float distanceToMid= cv::norm(validPoints[i]-l.w_mid); + // normalize distance + diff = (l.w_left - l.w_right)/cv::norm(l.w_left-l.w_right); + if(distanceToLeft < distanceToMid && distanceToLeft < distanceToRight){ + //left point + validPoints[i]-=diff*0.4; + distances.push_back(distanceToLeft); + }else if(distanceToRight < distanceToMid ){ + validPoints[i]+=diff*0.4; + distances.push_back(distanceToRight); + }else{ + distances.push_back(distanceToMid); } - /* + } + + std::vector weights; + for(const float &dist:distances){ + //as distance is in meter, we multiply it by 100 + if(useWeights_) + weights.push_back(1/(dist*100+0.001)); //TODO hier etwas sinnvolles überlegen + else + weights.push_back(1); + } + /* if(renderDebugImage){ for(lms::math::vertex2f &v:foundPoints) debugTranslatedPoints->points().push_back(v); } */ - // todo: check how this gets handled and adjust the interface - drive_ros_image_recognition::RoadLane lane_out; - lane_out.header.stamp = ros::Time::now(); - geometry_msgs::Point32 point_temp; - point_temp.z = 0.f; - for (auto point: validPoints) { - point_temp.x = point.x; - point_temp.y = point.y; - lane_out.points.push_back(point_temp); - } - lane_out.roadStateType = drive_ros_image_recognition::RoadLane::UNKNOWN; - line_output_pub_.publish(lane_out); -// lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); -// if(localCourse.isValid()){ -// localCourse->addPoints(foundPoints,weights); -// }else{ -// logger.error("localCourse invalid!"); -// } - - std::unique_lock lock(mutex); - linesToProcess_ --; - conditionLineProcessed_.notify_all(); + // todo: check how this gets handled and adjust the interface + drive_ros_image_recognition::RoadLane lane_out; + lane_out.header.stamp = ros::Time::now(); + geometry_msgs::Point32 point_temp; + point_temp.z = 0.f; + for (auto point: validPoints) { + point_temp.x = point.x; + point_temp.y = point.y; + lane_out.points.push_back(point_temp); + } + lane_out.roadStateType = drive_ros_image_recognition::RoadLane::UNKNOWN; + line_output_pub_.publish(lane_out); + // lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); + // if(localCourse.isValid()){ + // localCourse->addPoints(foundPoints,weights); + // }else{ + // logger.error("localCourse invalid!"); + // } + + std::unique_lock lock(mutex); + linesToProcess_ --; + conditionLineProcessed_.notify_all(); } void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::NewRoadDetectionConfig& config, uint32_t level){ - searchOffset_ = config.searchOffset; - findPointsBySobel_ = config.findBySobel; - renderDebugImage_ = config.renderDebugImage; - minLineWidthMul_ = config.minLineWidthMul; - maxLineWidthMul_ = config.maxLineWidthMul; - brightness_threshold_ = config.brightness_threshold; - laneWidthOffsetInMeter_ = config.laneWidthOffsetInMeter; - useWeights_ = config.useWeights; - sobelThreshold_ = config.sobelThreshold; - numThreads_ = config.threads; + searchOffset_ = config.searchOffset; + findPointsBySobel_ = config.findBySobel; + renderDebugImage_ = config.renderDebugImage; + minLineWidthMul_ = config.minLineWidthMul; + maxLineWidthMul_ = config.maxLineWidthMul; + brightness_threshold_ = config.brightness_threshold; + laneWidthOffsetInMeter_ = config.laneWidthOffsetInMeter; + useWeights_ = config.useWeights; + sobelThreshold_ = config.sobelThreshold; + numThreads_ = config.threads; } NewRoadDetection::~NewRoadDetection() { - { - std::unique_lock lock(mutex); - threadsRunning_ = false; - conditionNewLine_.notify_all(); - } - for(auto &t : threads_) { - if(t.joinable()) { - t.join(); - } + { + std::unique_lock lock(mutex); + threadsRunning_ = false; + conditionNewLine_.notify_all(); + } + for(auto &t : threads_) { + if(t.joinable()) { + t.join(); } + } } + +void NewRoadDetectionNodelet::onInit() { + new_road_detection_.reset(new NewRoadDetection(getNodeHandle(),getPrivateNodeHandle())); + if (!new_road_detection_->init()) { + ROS_ERROR("New_road_detection nodelet failed to initialize"); + // nodelet failing will kill the entire loader anyway + ros::shutdown(); + } + else { + ROS_INFO("New road detection nodelet succesfully initialized"); + } +} + +} // namespace drive_ros_image_recognition + +PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::NewRoadDetectionNodelet, nodelet::Nodelet) diff --git a/src/new_road_detection_node.cpp b/src/new_road_detection_node.cpp index 78226b0..c647277 100644 --- a/src/new_road_detection_node.cpp +++ b/src/new_road_detection_node.cpp @@ -6,10 +6,13 @@ int main(int argc, char** argv) ros::NodeHandle nh; ros::NodeHandle pnh("~"); - NewRoadDetection new_road_detection(nh,pnh); + drive_ros_image_recognition::NewRoadDetection new_road_detection(nh,pnh); if (!new_road_detection.init()) { return 1; } + else { + ROS_INFO("New road detection node succesfully initialized"); + } #ifndef NDEBUG // give GDB time to attach diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 1d0b633..0a10a1f 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -136,7 +136,8 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg) { // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); // flag ensures that we directly use the matrix, as it is done in LMS - cv::warpPerspective(current_image_, current_image_, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); + cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); + img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } bool WarpContent::worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, @@ -185,9 +186,14 @@ bool WarpContent::imageToWorld(drive_ros_image_recognition::ImageToWorld::Reques void WarpImageNodelet::onInit() { - my_content_.reset(new WarpContent(ros::NodeHandle("~"))); + my_content_.reset(new WarpContent(ros::NodeHandle(getPrivateNodeHandle()))); if (!my_content_->init()) { ROS_ERROR("WarpImageNodelet failed to initialize!"); + // nodelet failing will kill the entire loader anyway + ros::shutdown(); + } + else { + ROS_INFO("Warp_image nodelet succesfully initialized"); } } diff --git a/src/warp_image_node.cpp b/src/warp_image_node.cpp index 679a03e..3303f94 100644 --- a/src/warp_image_node.cpp +++ b/src/warp_image_node.cpp @@ -10,6 +10,9 @@ int main(int argc, char** argv) if (!warp.init()) { return 1; } + else { + ROS_INFO("Warp_image node succesfully initialized"); + } #ifndef NDEBUG // give GDB time to attach diff --git a/warp_image_nodelet.xml b/warp_image_nodelet.xml deleted file mode 100644 index b89baf7..0000000 --- a/warp_image_nodelet.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Image warper nodelet. - - - From 80c69eb680bcdf6c2768c02e4ed6efbef4206a33 Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Sat, 24 Jun 2017 16:29:06 +0200 Subject: [PATCH 12/94] Remove perspective transformation, add image transformation of search lines --- src/new_road_detection.cpp | 2 ++ src/warp_image.cpp | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 03b19ce..c60a678 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -406,6 +406,8 @@ bool NewRoadDetection::find(){ //transform them in image-coordinates std::unique_lock lock(mutex); + worldToImage(l.w_start,l.i_start); + worldToImage(l.w_end,l.i_end); lines_.push_back(l); linesToProcess_++; diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 0a10a1f..6d26aa3 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -136,8 +136,8 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg) { // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); // flag ensures that we directly use the matrix, as it is done in LMS - cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); - img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); +// cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); +// img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } bool WarpContent::worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, From f5fe3634255f4eeba97caa054d0cef709dd3187e Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Sun, 25 Jun 2017 00:45:37 +0200 Subject: [PATCH 13/94] Use CameraInfo for image transformations, adjust service requests --- CMakeLists.txt | 2 + .../drive_ros_image_recognition/warp_image.h | 13 +- package.xml | 12 +- src/new_road_detection.cpp | 14 +- src/warp_image.cpp | 126 +++++++++++++----- src/warp_image_node.cpp | 2 +- srv/ImageToWorld.srv | 2 +- srv/WorldToImage.srv | 2 +- 8 files changed, 118 insertions(+), 55 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eeef236..b2cb86b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ add_definitions(-std=c++11) find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport + image_geometry roscpp sensor_msgs std_msgs @@ -21,6 +22,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation nodelet + tf ) #include Boost for lane line processing diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 021e1f2..6ca7176 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -1,4 +1,4 @@ -#ifndef WARP_IMAGE_H +#ifndef WARP_IMAGE_Hag #define WARP_IMAGE_H #include @@ -6,8 +6,10 @@ #include #include #include +#include #include #include +#include #include @@ -19,7 +21,7 @@ class WarpContent { ~WarpContent(); bool init(); private: - void world_image_callback(const sensor_msgs::ImageConstPtr& msg); + void world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg); bool worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, drive_ros_image_recognition::WorldToImage::Response &res); bool imageToWorld(drive_ros_image_recognition::ImageToWorld::Request &req, @@ -34,14 +36,15 @@ class WarpContent { cv::Mat cam_mat_; cv::Mat dist_coeffs_; image_transport::ImageTransport it_; - image_transport::Subscriber img_sub_; + image_transport::CameraSubscriber cam_sub_; image_transport::Publisher img_pub_; // debug undistort publisher image_transport::Publisher undistort_pub_; ros::ServiceServer worldToImageServer_; ros::ServiceServer imageToWorldServer_; - // todo: move to using this in the future -// sensor_msgs::CameraInfo cam_info_; + // todo: moving to camera model subscription + image_geometry::PinholeCameraModel cam_model_; + tf::TransformListener tf_listener_; }; class WarpImageNodelet : public nodelet::Nodelet { diff --git a/package.xml b/package.xml index f1f997c..8effea1 100644 --- a/package.xml +++ b/package.xml @@ -42,25 +42,29 @@ catkin cv_bridge image_transport + image_geometry roscpp sensor_msgs std_msgs dynamic_reconfigure + nodelet + geometry_msgs + tf message_generation - geometry_msgs - nodelet cv_bridge image_transport + image_geometry roscpp sensor_msgs std_msgs dynamic_reconfigure + nodelet + geometry_msgs + tf message_runtime - geometry_msgs - nodelet diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index c60a678..1983925 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -135,8 +135,8 @@ bool NewRoadDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &w srv.request.image_point = image_point_send; if (imageToWorldClient_.call(srv)) { - world_point.x = srv.response.world_point.x; - world_point.y = srv.response.world_point.y; + world_point.x = srv.response.world_point.point.x; + world_point.y = srv.response.world_point.point.y; } else { @@ -147,10 +147,12 @@ bool NewRoadDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &w bool NewRoadDetection::worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { drive_ros_image_recognition::WorldToImage srv; - geometry_msgs::Point world_point_send; - world_point_send.x = world_point.x; - world_point_send.y = world_point.y; - world_point_send.z = 0.0; + geometry_msgs::PointStamped world_point_send; + world_point_send.point.x = world_point.x; + world_point_send.point.y = world_point.y; + world_point_send.point.z = 0.0; + // todo: check if this is valid, we assume the trajectory is defined in the axis reference frame + world_point_send.header.frame_id = "tf_front_axis_middle"; srv.request.world_point = world_point_send; if (worldToImageClient_.call(srv)) { diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 6d26aa3..a2363b4 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -11,9 +11,11 @@ WarpContent::WarpContent(const ros::NodeHandle& pnh): cam_mat_(3,3,CV_64F,cv::Scalar(0.0)), dist_coeffs_(8,1,CV_64F,cv::Scalar(0.0)), it_(pnh), - img_sub_(), + cam_sub_(), worldToImageServer_(), - imageToWorldServer_() + imageToWorldServer_(), + cam_model_(), + tf_listener_() { img_pub_ = it_.advertise("warped_out", 1); undistort_pub_ = it_.advertise("undistort_out", 1); @@ -103,13 +105,14 @@ bool WarpContent::init() { ROS_DEBUG_STREAM("Distortion coefficients loaded as: "<header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); // opionally: apply scaled homography // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); @@ -143,45 +148,92 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg) { bool WarpContent::worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, drive_ros_image_recognition::WorldToImage::Response &res) { - ROS_DEBUG("WorldToImage service: transforming incoming world coordinates to image coordinates"); - cv::Mat point_world(1,1,CV_64FC2); - point_world.at(0,0) = req.world_point.x; - point_world.at(0,1) = req.world_point.y; - point_world.at(0,2) = req.world_point.z; - - cv::Mat point_image(1,1,CV_64FC2); - cv::perspectiveTransform(point_world, point_image, world2cam_); - if (point_world.rows != 1 || point_world.cols != 1) { - ROS_WARN("Point transformed to image dimensions has invalid dimensions"); - return false; - } - - res.image_point.x = point_image.at(0,0); - res.image_point.y = point_image.at(0,1); +// ROS_DEBUG("WorldToImage service: transforming incoming world coordinates to image coordinates"); +// cv::Mat point_world(1,1,CV_64FC2); +// point_world.at(0,0) = req.world_point.x; +// point_world.at(0,1) = req.world_point.y; +// point_world.at(0,2) = req.world_point.z; + + cv::Point3d world_point(req.world_point.point.x, req.world_point.point.y, req.world_point.point.z); + cv::Point2d world_point_cam = cam_model_.project3dToPixel(world_point); + +// cv::Mat point_image(1,1,CV_64FC2); +// cv::perspectiveTransform(point_world, point_image, world2cam_); +// if (point_world.rows != 1 || point_world.cols != 1) { +// ROS_WARN("Point transformed to image dimensions has invalid dimensions"); +// return false; +// } + +// res.image_point.x = point_image.at(0,0); +// res.image_point.y = point_image.at(0,1); +// res.image_point.z = 0.0; + + res.image_point.x = world_point_cam.x; + res.image_point.y = world_point_cam.y; res.image_point.z = 0.0; - // todo: move to sensor_msgs::CameraInfo for transformation, uses tf as base, could use as unified interface + return true; } bool WarpContent::imageToWorld(drive_ros_image_recognition::ImageToWorld::Request &req, drive_ros_image_recognition::ImageToWorld::Response &res) { - ROS_DEBUG("WorldToImage service: transforming incoming image coordinates to world coordinates"); - cv::Mat point_world(1,1,CV_64FC2); - point_world.at(0,0) = req.image_point.x; - point_world.at(0,1) = req.image_point.y; - - cv::Mat point_image(1,1,CV_64FC2); - cv::perspectiveTransform(point_world, point_image, cam2world_); - if (point_world.rows != 1 || point_world.cols != 1) { - ROS_WARN("Point transformed to world dimensions has invalid dimensions"); - return false; - } - res.world_point.x = point_image.at(0,0); - res.world_point.y = point_image.at(0,1); - res.world_point.z = point_image.at(0,2); - // todo: move to sensor_msgs::CameraInfo for transformation, uses tf as base, could use as unified interface - return true; +// ROS_DEBUG("WorldToImage service: transforming incoming image coordinates to world coordinates"); +// cv::Mat point_world(1,1,CV_64FC2); +// point_world.at(0,0) = req.image_point.x; +// point_world.at(0,1) = req.image_point.y; + +// cv::Mat point_image(1,1,CV_64FC2); +// cv::perspectiveTransform(point_world, point_image, cam2world_); +// if (point_world.rows != 1 || point_world.cols != 1) { +// ROS_WARN("Point transformed to world dimensions has invalid dimensions"); +// return false; +// } +// res.world_point.x = point_image.at(0,0); +// res.world_point.y = point_image.at(0,1); +// res.world_point.z = point_image.at(0,2); +// // todo: move to sensor_msgs::CameraInfo for transformation, uses tf as base, could use as unified interface +// return true; + + // assuming we are defining our road points in the middle from axis frame id +// geometry_msgs::PointStamped point_out; +// cv::Point2d image_point_; +// cv::Point3d world_point_; +// image_point_.x = req.image_point.x; +// image_point_.y = req.image_point.y; +// world_point_ = cam_model_.projectPixelTo3dRay(image_point_); +// cam_model_. + cv::Mat point_world(1,1,CV_64FC2); + point_world.at(0,0) = req.image_point.x; + point_world.at(0,1) = req.image_point.y; + + cv::Mat point_image(1,1,CV_64FC2); + cv::perspectiveTransform(point_world, point_image, cam2world_); + if (point_world.rows != 1 || point_world.cols != 1) { + ROS_WARN("Point transformed to world dimensions has invalid dimensions"); + return false; + } + + // todo: check if this is valid + cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cv::Point2d(point_image.at(0,0),point_image.at(0,1))); + + geometry_msgs::PointStamped camera_point; + camera_point.header.frame_id = cam_model_.tfFrame(); + camera_point.point.x = cam_ray_point.x; + camera_point.point.y = cam_ray_point.y; + camera_point.point.z = cam_ray_point.z; + + try { + ros::Duration timeout(1.0 / 30); + tf_listener_.waitForTransform(cam_model_.tfFrame(), cam_model_.tfFrame(), + ros::Time::now(), timeout); + tf_listener_.transformPoint("tf_front_axis_middle", camera_point, res.world_point); + } + catch (tf::TransformException& ex) { + ROS_WARN("[ImageToWorld] TF exception, unable to transform:\n%s", ex.what()); + return false; + } + return true; } void WarpImageNodelet::onInit() diff --git a/src/warp_image_node.cpp b/src/warp_image_node.cpp index 3303f94..7d477dd 100644 --- a/src/warp_image_node.cpp +++ b/src/warp_image_node.cpp @@ -6,7 +6,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "warp_image_node"); ros::NodeHandle nh("~"); - drive_ros_image_recognition::WarpContent warp = drive_ros_image_recognition::WarpContent(nh); + drive_ros_image_recognition::WarpContent warp(nh); if (!warp.init()) { return 1; } diff --git a/srv/ImageToWorld.srv b/srv/ImageToWorld.srv index 41837c5..6cddacc 100644 --- a/srv/ImageToWorld.srv +++ b/srv/ImageToWorld.srv @@ -2,4 +2,4 @@ geometry_msgs/Point image_point --- #point in world coordinates -geometry_msgs/Point world_point +geometry_msgs/PointStamped world_point diff --git a/srv/WorldToImage.srv b/srv/WorldToImage.srv index 47e3f6d..53f1342 100644 --- a/srv/WorldToImage.srv +++ b/srv/WorldToImage.srv @@ -1,5 +1,5 @@ #point in world coordinates -geometry_msgs/Point world_point +geometry_msgs/PointStamped world_point --- #point in image coordinates geometry_msgs/Point image_point From 6ad6e965053c98f649db820bcce97c7cad60760a Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sun, 25 Jun 2017 17:53:44 +0200 Subject: [PATCH 14/94] Adjusted launch parameters, fixed wrong image initialization, added conditional compile of displays, added additional displays, runs --- cfg/NewRoadDetection.cfg | 4 +-- config/new_road_detection.yaml | 4 +-- .../new_road_detection.h | 1 + .../drive_ros_image_recognition/warp_image.h | 3 +- launch/new_road_detection.launch | 2 +- launch/new_road_detection_nodelet.launch | 2 +- src/new_road_detection.cpp | 28 +++++++++++++++---- src/warp_image.cpp | 11 ++++---- src/warp_image_node.cpp | 7 +++-- 9 files changed, 41 insertions(+), 21 deletions(-) diff --git a/cfg/NewRoadDetection.cfg b/cfg/NewRoadDetection.cfg index 424052a..86abf2c 100755 --- a/cfg/NewRoadDetection.cfg +++ b/cfg/NewRoadDetection.cfg @@ -15,8 +15,8 @@ gen.add("sobelThreshold", int_t, 0, "Sobel threshold", 50, 0, 200) gen.add("laneWidthOffsetInMeter", double_t, 0, "Lane width offset (m)", 0.1, 0.0, 2.0) gen.add("translateEnvironment", bool_t, 0, "Translate environment", False) gen.add("useWeights", bool_t, 0, "Use weights", False) -gen.add("renderDebugImage", bool_t, 0, "Render debug image", False) -gen.add("threads", int_t, 0, "Number of threads", 4, 0, 8) +gen.add("renderDebugImage", bool_t, 0, "Render debug image", True) +gen.add("threads", int_t, 0, "Number of threads", 0, 0, 8) exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "NewRoadDetection")) diff --git a/config/new_road_detection.yaml b/config/new_road_detection.yaml index 84d7c29..bea791b 100644 --- a/config/new_road_detection.yaml +++ b/config/new_road_detection.yaml @@ -9,5 +9,5 @@ new_road_detection: laneWidthOffsetInMeter: 0.1 translateEnvironment: false useWeights: false - renderDebugImage: false - threads: 4 + renderDebugImage: true + threads: 0 diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index a4bb738..e08c10b 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -119,6 +119,7 @@ class NewRoadDetection { ros::Publisher line_output_pub_; #ifdef PUBLISH_DEBUG image_transport::Publisher debug_img_pub_; + image_transport::Publisher detected_points_pub_; #endif ros::NodeHandle nh_; ros::NodeHandle pnh_; diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 6ca7176..f7b25ad 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -17,7 +17,7 @@ namespace drive_ros_image_recognition { class WarpContent { public: - WarpContent(const ros::NodeHandle& pnh); + WarpContent(const ros::NodeHandle &nh, const ros::NodeHandle& pnh); ~WarpContent(); bool init(); private: @@ -27,6 +27,7 @@ class WarpContent { bool imageToWorld(drive_ros_image_recognition::ImageToWorld::Request &req, drive_ros_image_recognition::ImageToWorld::Response &res); ros::NodeHandle pnh_; + ros::NodeHandle nh_; cv::Mat current_image_; // needs two components: camera model and homography for transformation // homography matrices for transformation diff --git a/launch/new_road_detection.launch b/launch/new_road_detection.launch index d74b004..1193a37 100644 --- a/launch/new_road_detection.launch +++ b/launch/new_road_detection.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/new_road_detection_nodelet.launch b/launch/new_road_detection_nodelet.launch index 7cfdcf9..0efca20 100644 --- a/launch/new_road_detection_nodelet.launch +++ b/launch/new_road_detection_nodelet.launch @@ -2,7 +2,7 @@ - + diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 1983925..2c3df53 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -27,7 +27,7 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand translateEnvironment_(false), useWeights_(false), renderDebugImage_(false), - numThreads_(4), + numThreads_(0), road_sub_(), #if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) debug_image_(0,0,CV_16UC1), @@ -36,6 +36,7 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand it_(pnh), #ifdef PUBLISH_DEBUG debug_img_pub_(), + detected_points_pub_(), #endif dsrv_server_(), dsrv_cb_(boost::bind(&NewRoadDetection::reconfigureCB, this, _1, _2)), @@ -111,10 +112,11 @@ bool NewRoadDetection::init() { #ifdef DRAW_DEBUG debug_img_pub_ = it_.advertise("debug_image_out", 10); + detected_points_pub_ = it_.advertise("detected_points_out", 10); #endif - imageToWorldClient_ = pnh_.serviceClient("ImageToWorld"); - worldToImageClient_ = pnh_.serviceClient("WorldToImage"); + imageToWorldClient_ = nh_.serviceClient("/ImageToWorld"); + worldToImageClient_ = nh_.serviceClient("/WorldToImage"); // todo: we have not decided on an interface for these debug channels yet // debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); @@ -140,7 +142,7 @@ bool NewRoadDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &w } else { - ROS_ERROR("Failed to call service add_two_ints"); + ROS_ERROR("Failed to call service ImageToWorld"); return false; } } @@ -161,7 +163,7 @@ bool NewRoadDetection::worldToImage(const cv::Point2f &world_point, cv::Point &i } else { - ROS_ERROR("Failed to call service add_two_ints"); + ROS_ERROR("Failed to call service WorldToImage"); return false; } } @@ -485,6 +487,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { // directly apply sobel operations on the image -> we only use the grad_x here as we only search horizontally // we perform order of 1 operations only // todo: check if CV_8UC1 is valid + current_image_sobel_.reset(new cv::Mat(current_image_->rows,current_image_->cols,CV_8UC1)); cv::Sobel( *current_image_, *current_image_sobel_, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter foundPoints = findBySobel(it,0.02,iDist,wDist); @@ -494,6 +497,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { } // draw unfiltered image points +#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) if(renderDebugImage_) { std::unique_lock lock(debugAllPointsMutex); cv::namedWindow("Unfiltered points", CV_WINDOW_NORMAL); @@ -501,8 +505,18 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { for (auto point : foundPoints) { cv::circle(found_points_mat, point, 2, cv::Scalar(255)); } + // draw search line + cv::line(found_points_mat, l.i_start, l.i_end, cv::Scalar(255)); +#ifdef DRAW_DEBUG cv::imshow("Unfiltered points", found_points_mat); + cv::waitKey(1); +#endif + +#ifdef PUBLISH_DEBUG + detected_points_pub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, found_points_mat).toImageMsg()); +#endif } +#endif //filter //TODO filter points that are in poluted regions (for example the car itself) @@ -545,6 +559,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { } // draw filtered points +#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) if(renderDebugImage_) { // todo: make some more efficient storage method here, will translate back and forth here for now std::unique_lock lock(debugValidPointsMutex); @@ -557,6 +572,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { } cv::imshow("Filtered Points", filtered_points_mat); } +#endif //Handle found points cv::Point2f diff; @@ -613,7 +629,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { // } std::unique_lock lock(mutex); - linesToProcess_ --; + linesToProcess_--; conditionLineProcessed_.notify_all(); } diff --git a/src/warp_image.cpp b/src/warp_image.cpp index a2363b4..5bd43d6 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -3,7 +3,8 @@ namespace drive_ros_image_recognition { -WarpContent::WarpContent(const ros::NodeHandle& pnh): +WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): + nh_(nh), pnh_(pnh), current_image_(), world2cam_(3,3,CV_64F,cv::Scalar(0.0)), @@ -106,8 +107,8 @@ bool WarpContent::init() { // initialize homography transformation subscriber cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); - worldToImageServer_ = pnh_.advertiseService("WorldToImage", &WarpContent::worldToImage, this); - imageToWorldServer_ = pnh_.advertiseService("ImageToWorld", &WarpContent::imageToWorld, this); + worldToImageServer_ = nh_.advertiseService("/WorldToImage", &WarpContent::worldToImage, this); + imageToWorldServer_ = nh_.advertiseService("/ImageToWorld", &WarpContent::imageToWorld, this); return true; } @@ -135,7 +136,7 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, // undistort and apply homography transformation cv::Mat undistorted_mat; - cv::undistort(current_image_, undistorted_mat, cam_model_.fullProjectionMatrix(), cam_model_.distortionCoeffs()); + cv::undistort(current_image_, undistorted_mat, cam_model_.fullIntrinsicMatrix(), cam_model_.distortionCoeffs()); undistort_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); // opionally: apply scaled homography // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); @@ -238,7 +239,7 @@ bool WarpContent::imageToWorld(drive_ros_image_recognition::ImageToWorld::Reques void WarpImageNodelet::onInit() { - my_content_.reset(new WarpContent(ros::NodeHandle(getPrivateNodeHandle()))); + my_content_.reset(new WarpContent(getNodeHandle(),ros::NodeHandle(getPrivateNodeHandle()))); if (!my_content_->init()) { ROS_ERROR("WarpImageNodelet failed to initialize!"); // nodelet failing will kill the entire loader anyway diff --git a/src/warp_image_node.cpp b/src/warp_image_node.cpp index 7d477dd..e06e618 100644 --- a/src/warp_image_node.cpp +++ b/src/warp_image_node.cpp @@ -4,9 +4,10 @@ int main(int argc, char** argv) { ros::init(argc, argv, "warp_image_node"); - ros::NodeHandle nh("~"); + ros::NodeHandle pnh("~"); + ros::NodeHandle nh; - drive_ros_image_recognition::WarpContent warp(nh); + drive_ros_image_recognition::WarpContent warp(nh,pnh); if (!warp.init()) { return 1; } @@ -16,7 +17,7 @@ int main(int argc, char** argv) #ifndef NDEBUG // give GDB time to attach - ros::Duration(1.0).sleep(); + ros::Duration(2.0).sleep(); #endif while (ros::ok()) { From 9fc658909c4a44a87e7fef1fb539ec6fd05f8dcc Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sun, 25 Jun 2017 21:32:35 +0200 Subject: [PATCH 15/94] Add filtered point debug display and publisher --- .../new_road_detection.h | 1 + src/new_road_detection.cpp | 22 ++++++++++++++----- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index e08c10b..cc2ca6f 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -120,6 +120,7 @@ class NewRoadDetection { #ifdef PUBLISH_DEBUG image_transport::Publisher debug_img_pub_; image_transport::Publisher detected_points_pub_; + image_transport::Publisher filtered_points_pub_; #endif ros::NodeHandle nh_; ros::NodeHandle pnh_; diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 2c3df53..b9c037d 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -37,6 +37,7 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand #ifdef PUBLISH_DEBUG debug_img_pub_(), detected_points_pub_(), + filtered_points_pub_(), #endif dsrv_server_(), dsrv_cb_(boost::bind(&NewRoadDetection::reconfigureCB, this, _1, _2)), @@ -113,6 +114,7 @@ bool NewRoadDetection::init() { #ifdef DRAW_DEBUG debug_img_pub_ = it_.advertise("debug_image_out", 10); detected_points_pub_ = it_.advertise("detected_points_out", 10); + filtered_points_pub_ = it_.advertise("filtered_points_out", 10); #endif imageToWorldClient_ = nh_.serviceClient("/ImageToWorld"); @@ -397,15 +399,17 @@ bool NewRoadDetection::find(){ worldToImage(l.w_start, l_w_start); worldToImage(l.w_end, l_w_end); cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); + // if the left side is out of the current view, use middle instead if(!img_rect.contains(l_w_start)){ // try middle lane -> should always be in image - l.w_start = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); + l.w_start = cv::Point2f(it_mid->x(),it_mid->y()); worldToImage(l.w_start, l_w_start); if(img_rect.contains(l_w_start)){ continue; } + // if the right side is out of the current view, use middle instead }else if(!img_rect.contains(l_w_end)){ - l.w_end = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); + l.w_end = cv::Point2f(it_mid->x(),it_mid->y()); } //transform them in image-coordinates @@ -471,10 +475,11 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { //calculate the offset float iDist = cv::norm(l.i_end - l.i_start); float wDist = cv::norm(l.w_end - l.w_start); - // float pxlPerDist = iDist/wDist*searchOffset_; - // lms::math::vertex2f iDiff = lms::math::vertex2f(l.i_start-l.i_end).normalize(); - // lms::math::vertex2f startLine = lms::math::vertex2f(l.i_start)+iDiff*pxlPerDist; - // lms::math::vertex2f endLine = lms::math::vertex2f(l.i_end)-iDiff*pxlPerDist; + // add search offset -> do not need this as we do it directly in the image +// float pxlPerDist = iDist/wDist*searchOffset_; +// cv::Point2f iDiff = (l.i_start-l.i_end)/norm(l.i_start-l.i_end); +// cv::Point2f startLine = cv::Point2f(l.i_start)+iDiff*pxlPerDist; +// cv::Point2f endLine = cv::Point2f(l.i_end)-iDiff*pxlPerDist; // //get all points in between // lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! @@ -570,7 +575,12 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { worldToImage(point, img_point); cv::circle(filtered_points_mat, img_point, 2, cv::Scalar(255)); } +#ifdef DRAW_DEBUG cv::imshow("Filtered Points", filtered_points_mat); +#endif +#ifdef PUBLISH_DEBUG + filtered_points_pub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, filtered_points_mat).toImageMsg()); +#endif } #endif From 626ace156d26c38ff9ff63436d9c1d9081c97b63 Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Mon, 26 Jun 2017 08:18:41 +0200 Subject: [PATCH 16/94] Readded condition for threading, added some more debug output --- src/new_road_detection.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index b9c037d..c1e340a 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -265,9 +265,9 @@ std::vector NewRoadDetection::findBySobel(cv::LineIterator it, //check if the points have the right distance float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? - //logger.debug("")<<"crossing found highLow: "< pxlPeakWidth*minLineWidthMul_ && pxlCounter < pxlPeakWidth*maxLineWidthMul_){ @@ -279,7 +279,7 @@ std::vector NewRoadDetection::findBySobel(cv::LineIterator it, cv::Point2f wMid; imageToWorld(it_backup.pos(),wMid); foundPoints.push_back(wMid); - //logger.debug("")<<"crossing FOUND VALID CROSSING"; + ROS_DEBUG("crossing FOUND VALID CROSSING"); } } pxlCounter = 0; @@ -419,8 +419,7 @@ bool NewRoadDetection::find(){ lines_.push_back(l); linesToProcess_++; - // todo: figure out what this does (no internet) - // conditionNewLine.notify_one(); + conditionNewLine_.notify_one(); } if(numThreads_ == 0) { From 66c070e0355f491cf39edc4c2f7c02fab7532a7c Mon Sep 17 00:00:00 2001 From: Simon089 Date: Thu, 29 Jun 2017 16:49:40 +0100 Subject: [PATCH 17/94] init commit street_crossing --- CMakeLists.txt | 23 ++ .../street_crossing.h | 102 ++++++++ src/street_crossing.cpp | 238 ++++++++++++++++++ src/street_crossing_node.cpp | 26 ++ 4 files changed, 389 insertions(+) create mode 100644 include/drive_ros_image_recognition/street_crossing.h create mode 100644 src/street_crossing.cpp create mode 100644 src/street_crossing_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b2cb86b..2d8b8a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -193,6 +193,29 @@ install(TARGETS image_publisher_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +################################################################################ +# Street crossing node (LMS port) +################################################################################ + +add_executable(street_crossing_node + src/street_crossing.cpp + src/street_crossing_node.cpp + ) + +add_dependencies(street_crossing_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(street_crossing_node + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + ) + +install(TARGETS street_crossing_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + ################################################################################ # Install other files ################################################################################ diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h new file mode 100644 index 0000000..69209ce --- /dev/null +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -0,0 +1,102 @@ +#ifndef STREET_CROSSING_H +#define STREET_CROSSING_H + +#include +#include +#include +#include +#include +#include + +#include "geometry_common.h" +#include "image_object.h" +#include "drive_ros_image_recognition/RoadLane.h" + +typedef boost::shared_ptr CvImagePtr; + +inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& imageIn) { + CvImagePtr cvPtr; + try + { + // hardcopies for now, might be possible to process on pointer if fast enough + // todo: make prettier + cv_bridge::CvImagePtr tmpPtr = cv_bridge::toCvCopy(*imageIn, ""); + cvPtr.reset(new cv::Mat(tmpPtr->image) ); + } + catch(cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return NULL; + } + + if(!cvPtr->data) + { + ROS_WARN("Empty image received, skipping!"); + return NULL; + } + return cvPtr; +} + +namespace drive_ros_image_recognition { +namespace detection { + +class StreetCrossingDetection { + +private: + struct SearchLine{ + cv::Point2f wStart; + cv::Point2f wEnd; + cv::Point2i iStart; + cv::Point2i iEnd; + }; + + // configs + int sobelThreshold; + float minLineWidthMul; + float maxLineWidthMul; + float lineWidth; // in m; todo: should be in config + + // variables + std::vector searchLines; + CvImagePtr currentImage; + + // communication + image_transport::ImageTransport imageTransport; + image_transport::Subscriber imageSubscriber; + // road inputs and outputs + ros::Subscriber roadSubscriber; + ros::Publisher linePublisher; + + drive_ros_image_recognition::RoadLane roadBuffer; + + ros::NodeHandle nh; + ros::NodeHandle pnh; + + void roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& roadIn); + void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); + + // help: how do publish debug points? +#ifdef PUBLISH_DEBUG +// image_transport::Publisher debug_img_pub_; + image_transport::Publisher detectedPointsPublisher; +#endif + + ros::ServiceClient worldToImageClient; + ros::ServiceClient imageToWorldClient; + bool imageToWorld(const cv::Point &imagePoint, cv::Point2f &worldPoint); + bool worldToImage(const cv::Point2f &worldPoint, cv::Point &imagePoint); + + // methods + bool findStartline(/*linestring &middleLine*/); + std::vector processSearchLine(SearchLine &sl); + +public: + StreetCrossingDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); + ~StreetCrossingDetection(); + bool init(); +}; + +} // namepsace drive_ros_image_recognition +} // namespace detection + +#endif // STREET_CROSSING_H diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp new file mode 100644 index 0000000..995fc88 --- /dev/null +++ b/src/street_crossing.cpp @@ -0,0 +1,238 @@ +#include +#include "opencv2/highgui/highgui.hpp" +#include +#include "drive_ros_image_recognition/warp_image.h" +#include "drive_ros_image_recognition/street_crossing.h" + +namespace drive_ros_image_recognition { +namespace detection { + +StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh_, const ros::NodeHandle pnh_) + : nh(nh_) // help: difference nh und pnh (whats the input) + , pnh(pnh_) + , imageTransport(pnh_) + , sobelThreshold(50) + , minLineWidthMul(0.5) + , maxLineWidthMul(2.0) + , lineWidth(0.4) +{ + // todo +} + +StreetCrossingDetection::~StreetCrossingDetection() { + +} + +bool StreetCrossingDetection::init() { + imageSubscriber = imageTransport.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); + roadSubscriber = pnh.subscribe("road_in", 100, &StreetCrossingDetection::roadCallback, this); + // todo: advertise lines + + // help: serviceClients + imageToWorldClient = nh.serviceClient("/ImageToWorld"); + worldToImageClient = nh.serviceClient("/WorldToImage"); + + // todo: debug channels + + return true; +} + +bool StreetCrossingDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &world_point) { + drive_ros_image_recognition::ImageToWorld srv; + geometry_msgs::Point image_point_send; + image_point_send.x = image_point.x; + image_point_send.y = image_point.y; + image_point_send.z = 0.0; + srv.request.image_point = image_point_send; + if (imageToWorldClient.call(srv)) + { + world_point.x = srv.response.world_point.point.x; + world_point.y = srv.response.world_point.point.y; + return true; + } + else + { + ROS_ERROR("Failed to call service ImageToWorld"); + return false; + } +} + +bool StreetCrossingDetection::worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { + drive_ros_image_recognition::WorldToImage srv; + geometry_msgs::PointStamped world_point_send; + world_point_send.point.x = world_point.x; + world_point_send.point.y = world_point.y; + world_point_send.point.z = 0.0; + // todo: check if this is valid, we assume the trajectory is defined in the axis reference frame + world_point_send.header.frame_id = "tf_front_axis_middle"; + srv.request.world_point = world_point_send; + if (worldToImageClient.call(srv)) + { + image_point.x = (int)srv.response.image_point.x; + image_point.y = (int)srv.response.image_point.y; + return true; + } + else + { + ROS_ERROR("Failed to call service WorldToImage"); + return false; + } +} + +// todo: +// callbacks +// dynamic reconfigure + +void StreetCrossingDetection::roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr &roadIn) { + roadBuffer = *roadIn; +} + +void StreetCrossingDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { + currentImage = convertImageMessage(imgIn); + if(roadBuffer.points.size() > 1) { + findStartline(); + } else { + ROS_WARN("Not searching for start line, since road-buffer has no points stored"); + } +} + +bool StreetCrossingDetection::findStartline(/*linestring& middleLine*/) { + float hightStartLine = 0.04; +// bool foundStartLine = false; + linestring rightMiddleLine, middleLine, tmp; + + searchLines.clear(); + + // Get middle line with distance and move to right + for(auto point : roadBuffer.points) { + boost::geometry::append(middleLine, point_xy(point.x, point.y)); + } + drive_ros_geometry_common::moveOrthogonal(middleLine, tmp, lineWidth / 2); + drive_ros_geometry_common::simplify(tmp, rightMiddleLine, hightStartLine + 0.01); + + //get all lines + for(int i = 1; i < rightMiddleLine.size(); i++) { + SearchLine l; + // todo: if this works, make it more efficient + l.wStart = cv::Point2f(rightMiddleLine.at(i - 1).x(), rightMiddleLine.at(i - 1).y()); + l.wEnd = cv::Point2f(rightMiddleLine.at(i).x(), rightMiddleLine.at(i).y()); + +// cv::Point l_w_start, l_w_end; +// worldToImage(l.w_start, l_w_start); +// worldToImage(l.w_end, l_w_end); + worldToImage(l.wStart, l.iStart); + worldToImage(l.wEnd, l.iEnd); + cv::Rect imgRect(cv::Point(),cv::Point(currentImage->cols, currentImage->rows)); +// if(!imgRect.contains(l_w_start)){ +// // try middle lane -> should always be in image +// l.w_start = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); +// worldToImage(l.w_start, l_w_start); +// if(imgRect.contains(l_w_start)){ +// continue; +// } +// }else if(!imgRect.contains(l_w_end)){ +// l.w_end = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); +// } + + //transform them in image-coordinates +// std::unique_lock lock(mutex); +// worldToImage(l.w_start,l.i_start); +// worldToImage(l.w_end,l.i_end); +// lines_.push_back(l); +// linesToProcess_++; + + if((imgRect.contains(l.iStart)) && (imgRect.contains(l.iEnd))) { + searchLines.push_back(l); + } + + // todo: figure out what this does (no internet) + // conditionNewLine.notify_one(); + } + + ROS_INFO("Number of search-lines to search stop-line: %lu", searchLines.size()); + + // Sobel + // todo: naming and usage of mat's + cv::Mat /*imgGaussian,*/ imgSobel, absGradiants; +// cv::GaussianBlur(*currentImage, imgGaussian, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT); + cv::Sobel(*currentImage, imgSobel, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT); + cv::convertScaleAbs(imgSobel, absGradiants); + + // use search-lines to find stop-line + // todo: if this works, parallize it! + std::vector linePoints; + for(auto sl : searchLines) { + auto tmp = processSearchLine(sl); + linePoints.insert(linePoints.end(), tmp.begin(), tmp.end()); + } + + + // Draw points on image + +} + +std::vector StreetCrossingDetection::processSearchLine(SearchLine &sl) { + std::vector foundPoints; + bool foundLowHigh = false; + int pxlCounter = 0; + cv::Point2f iStartPxlPeak; + cv::LineIterator lineIt(*currentImage, sl.iStart, sl.iEnd); + + float iDist = cv::norm(sl.iEnd - sl.iStart); + float wDist = cv::norm(sl.wEnd - sl.wStart); + + for(int i = 0; i < lineIt.count; i++, ++lineIt) { + // safety check : is the point inside the image +// if(!rect.contains(it.pos())){ +// ROS_WARN("StreetCrossing::processSearchLine: Received an invalid point outside the image to check for Sobel"); +// return foundPoints; +// } + + //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang + int sobel = (int)currentImage->at(lineIt.pos().x, lineIt.pos().y); + if(sobel > sobelThreshold) { + // found low-high pass (on the left side of the line) + if(!foundLowHigh) { + foundLowHigh = true; + pxlCounter = 0; + iStartPxlPeak.x = lineIt.pos().x; + iStartPxlPeak.y = lineIt.pos().y; + } + } else if(sobel < -sobelThreshold){ // hell-dunkel übergang + //check if we found a lowHigh + highLow border + if(foundLowHigh){ + //check if the points have the right distance + // TODO to bad, calculate for each road line (how should we use them for searching? + float pxlPeakWidth = iDist / wDist * lineWidth; //todo: understand this + + //logger.debug("")<<"crossing found highLow: "< (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { + // we found a valid point + // get the middle + cv::Point2f iMid = (iStartPxlPeak + cv::Point2f(lineIt.pos().x, lineIt.pos().y)) * .5; + cv::Point2f wMid; + imageToWorld(iMid, wMid); + foundPoints.push_back(wMid); + ROS_DEBUG("Found a stop-line"); + //logger.debug("")<<"crossing FOUND VALID CROSSING"; + } + } + pxlCounter = 0; + foundLowHigh = false; + //if not, we dont have to do anything + } + + // for calculation of line width + if(foundLowHigh){ + pxlCounter++; + } + } + return foundPoints; +} + +} //namepsace detection +} //namespace drive_ros_image_recognition diff --git a/src/street_crossing_node.cpp b/src/street_crossing_node.cpp new file mode 100644 index 0000000..cf025b0 --- /dev/null +++ b/src/street_crossing_node.cpp @@ -0,0 +1,26 @@ +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "street_crossing_detection"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + drive_ros_image_recognition::detection::StreetCrossingDetection streetCrossingDetection(nh, pnh); + if (!streetCrossingDetection.init()) { + return 1; + } + else { + ROS_INFO("Street crossing detection node succesfully initialized"); + } + +#ifndef NDEBUG + // give GDB time to attach + ros::Duration(1.0).sleep(); +#endif + + while (ros::ok()) { + ros::spin(); + } + return 0; +} From d6de0dfb44eec4f8ba9b7562c8ce645730a9478b Mon Sep 17 00:00:00 2001 From: FaHa Date: Wed, 5 Jul 2017 16:30:43 +0200 Subject: [PATCH 18/94] fixed typo in header guards --- include/drive_ros_image_recognition/line_recognition.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/drive_ros_image_recognition/line_recognition.h b/include/drive_ros_image_recognition/line_recognition.h index d499471..89662f0 100644 --- a/include/drive_ros_image_recognition/line_recognition.h +++ b/include/drive_ros_image_recognition/line_recognition.h @@ -1,5 +1,5 @@ #ifndef LINE_RECOGNITION_H -#define LINE_RECOGNTION_H +#define LINE_RECOGNITION_H #include #include From 46af5ee67fec217a2a7355c9ccd22463997b78cb Mon Sep 17 00:00:00 2001 From: FaHa Date: Wed, 5 Jul 2017 17:40:18 +0200 Subject: [PATCH 19/94] removed line recognition files --- CMakeLists.txt | 23 ------- .../line_recognition.h | 26 ------- src/line_recognition.cpp | 69 ------------------- src/line_recognition_node.cpp | 20 ------ 4 files changed, 138 deletions(-) delete mode 100644 include/drive_ros_image_recognition/line_recognition.h delete mode 100644 src/line_recognition.cpp delete mode 100644 src/line_recognition_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d8b8a5..715b82c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,29 +63,6 @@ include_directories( ${Boost_INCLUDE_DIRS} ) -################################################################################ -# Line recognition node -> will leave this in for now, might be useful -################################################################################ - -add_executable(line_recognition_node - src/line_recognition_node.cpp - src/line_recognition.cpp - src/edge_point.cpp -# src/line_point.cpp - ) - -add_dependencies(line_recognition_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(line_recognition_node - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ) - -install(TARGETS line_recognition_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) ################################################################################ # New road detection node (LMS port) diff --git a/include/drive_ros_image_recognition/line_recognition.h b/include/drive_ros_image_recognition/line_recognition.h deleted file mode 100644 index 89662f0..0000000 --- a/include/drive_ros_image_recognition/line_recognition.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef LINE_RECOGNITION_H -#define LINE_RECOGNITION_H - -#include -#include -#include -#include -#include - -namespace drive_ros_image_recognition { - -class LineRecognition { -public: - LineRecognition(ros::NodeHandle nh); - ~LineRecognition(); - - void img_callback(const sensor_msgs::ImageConstPtr& msg); - -private: - image_transport::ImageTransport it_; - image_transport::Subscriber img_sub_; -}; - -} // namespace drive_ros_image_recognition - -#endif // LINE_RECOGNTION_H diff --git a/src/line_recognition.cpp b/src/line_recognition.cpp deleted file mode 100644 index ceef0b3..0000000 --- a/src/line_recognition.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include - -namespace drive_ros_image_recognition { - -LineRecognition::LineRecognition( ros::NodeHandle nh ): - it_(nh) -{ - img_sub_ = it_.subscribe("img_in", 10, &LineRecognition::img_callback, this); -} - -LineRecognition::~LineRecognition() { -} - -void LineRecognition::img_callback(const sensor_msgs::ImageConstPtr& msg) { - cv::Mat cv_ptr; - try - { - // todo: check if this if we have 8 or 16 greyscale images - // hardcopies for now, might be possible to process on pointer if fast enough - cv_ptr = cv_bridge::toCvCopy(msg, "mono16")->image; - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - if( !cv_ptr.data) - { - ROS_WARN("Empty image received, skipping!"); - return; - } - - // define some processing constants - int scale = 1; - int delta = 0; - int ddepth = CV_16S; - cv::Mat grad; - - cv::Mat img_blurred; - cv::GaussianBlur( cv_ptr, img_blurred, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT ); - - // convert to grayscale - cv::cvtColor( img_blurred, img_blurred, CV_BGR2GRAY ); - - /// Create window - cv::namedWindow( "Sobel", CV_WINDOW_AUTOSIZE ); - - /// Generate grad_x and grad_y - cv::Mat grad_x, grad_y; - cv::Mat abs_grad_x, abs_grad_y; - - /// Gradient X - cv::Sobel( img_blurred, grad_x, ddepth, 1, 0, 3, scale, delta, cv::BORDER_DEFAULT ); - cv::convertScaleAbs( grad_x, abs_grad_x ); - - /// Gradient Y - cv::Sobel( img_blurred, grad_y, ddepth, 0, 1, 3, scale, delta, cv::BORDER_DEFAULT ); - cv::convertScaleAbs( grad_y, abs_grad_y ); - - /// Total Gradient (approximate) - cv::addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad ); - - cv::imshow( "Sobel", grad ); - - cv::waitKey(0); -} - -} // end drive_ros_image_recognition namespace diff --git a/src/line_recognition_node.cpp b/src/line_recognition_node.cpp deleted file mode 100644 index 61a18df..0000000 --- a/src/line_recognition_node.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "line_recognition"); - ros::NodeHandle nh; - - drive_ros_image_recognition::LineRecognition line_rec = drive_ros_image_recognition::LineRecognition(nh); - -#ifndef NDEBUG - // give GDB time to attach - ros::Duration(1.0).sleep(); -#endif - - while (ros::ok()) { - ros::spin(); - } - return 0; -} From 1dd89909e726d990050b2de2b7f7c6edb2b73768 Mon Sep 17 00:00:00 2001 From: FaHa Date: Wed, 5 Jul 2017 17:41:34 +0200 Subject: [PATCH 20/94] removed camera matrix yml (parameter come with camera info message) --- config/camera_matrix.yaml | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 config/camera_matrix.yaml diff --git a/config/camera_matrix.yaml b/config/camera_matrix.yaml deleted file mode 100644 index 5b6c775..0000000 --- a/config/camera_matrix.yaml +++ /dev/null @@ -1,6 +0,0 @@ -camera_matrix: - # ordered as: Fx, Fy, Cx, Cy - cam_mat: [4.558675e+02, 4.569925e+02, 3.633525e+02, 1.724759e+02] - # ordered as: K1, K2, K3, K4, K5, K6 (assumed as 0, not in code) - dist_coeffs: [-2.994090e-01, 9.807324e-02, -2.577654e-03, -9.520364e-04, 0.0, 0.0] - # assuming no tangential distortion (p1, p2 = 0) From 46ccc9296e974206d28e138515ebb1547a2d59a7 Mon Sep 17 00:00:00 2001 From: FaHa Date: Wed, 5 Jul 2017 23:43:53 +0200 Subject: [PATCH 21/94] removed image publisher node (moved to separate repo) --- CMakeLists.txt | 20 ------------ launch/image_publisher_node.launch | 7 ---- src/image_publisher_node.cpp | 52 ------------------------------ 3 files changed, 79 deletions(-) delete mode 100644 launch/image_publisher_node.launch delete mode 100644 src/image_publisher_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 715b82c..3f836c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -150,26 +150,6 @@ install(TARGETS warp_image_nodelet RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -################################################################################ -# Image publisher node -################################################################################ -add_executable(image_publisher_node - src/image_publisher_node.cpp - ) - -add_dependencies(image_publisher_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(image_publisher_node - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ) - -install(TARGETS image_publisher_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - ################################################################################ # Street crossing node (LMS port) ################################################################################ diff --git a/launch/image_publisher_node.launch b/launch/image_publisher_node.launch deleted file mode 100644 index c61c7fb..0000000 --- a/launch/image_publisher_node.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/src/image_publisher_node.cpp b/src/image_publisher_node.cpp deleted file mode 100644 index 835c45b..0000000 --- a/src/image_publisher_node.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include -#include -#include -#include -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "image_publisher_node"); - ros::NodeHandle nh("~"); - ros::Publisher pub = nh.advertise("img_out", 1); - - cv_bridge::CvImage cv_image; - sensor_msgs::Image ros_image; - - double publish_rate_received = 0.1; - if (!nh.getParam("publish_rate",publish_rate_received)) - ROS_WARN_STREAM("Unable to get 'publish_rate' parameter, using default: "< filenames; - cv::String folder = image_folder; - cv::glob(folder, filenames); - - if (filenames.size() == 0) { - ROS_ERROR("No images found in provided folder, shutting down"); - return 1; - } - - size_t i = 0; - while (ros::ok() && i Date: Wed, 5 Jul 2017 23:52:17 +0200 Subject: [PATCH 22/94] moved vision launch files to config repo --- launch/vision.launch | 20 ---------------- launch/vision_nodelet.launch | 45 ------------------------------------ 2 files changed, 65 deletions(-) delete mode 100644 launch/vision.launch delete mode 100644 launch/vision_nodelet.launch diff --git a/launch/vision.launch b/launch/vision.launch deleted file mode 100644 index 23e55af..0000000 --- a/launch/vision.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/launch/vision_nodelet.launch b/launch/vision_nodelet.launch deleted file mode 100644 index ccdf5bf..0000000 --- a/launch/vision_nodelet.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From d63a1ab969469fa017b412891c242b14a4c5d88a Mon Sep 17 00:00:00 2001 From: FaHa Date: Wed, 5 Jul 2017 23:55:23 +0200 Subject: [PATCH 23/94] removed line recognition launch files --- launch/line_recognition.launch | 5 ----- launch/line_recognition_debug.launch | 5 ----- 2 files changed, 10 deletions(-) delete mode 100644 launch/line_recognition.launch delete mode 100644 launch/line_recognition_debug.launch diff --git a/launch/line_recognition.launch b/launch/line_recognition.launch deleted file mode 100644 index a14132f..0000000 --- a/launch/line_recognition.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/launch/line_recognition_debug.launch b/launch/line_recognition_debug.launch deleted file mode 100644 index 6688e2f..0000000 --- a/launch/line_recognition_debug.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - From 89493fd7cacacc032772e484a0859fd149c31ee5 Mon Sep 17 00:00:00 2001 From: FaHa Date: Thu, 6 Jul 2017 15:43:43 +0200 Subject: [PATCH 24/94] removed old lms stuff --- .../drive_ros_image_recognition/edge_point.h | 148 ---------- .../image_object.h | 24 -- .../drive_ros_image_recognition/line_point.h | 76 ----- .../street_crossing.h | 1 - src/edge_point.cpp | 278 ------------------ src/line_point.cpp | 174 ----------- 6 files changed, 701 deletions(-) delete mode 100644 include/drive_ros_image_recognition/edge_point.h delete mode 100644 include/drive_ros_image_recognition/image_object.h delete mode 100644 include/drive_ros_image_recognition/line_point.h delete mode 100644 src/edge_point.cpp delete mode 100644 src/line_point.cpp diff --git a/include/drive_ros_image_recognition/edge_point.h b/include/drive_ros_image_recognition/edge_point.h deleted file mode 100644 index 812b524..0000000 --- a/include/drive_ros_image_recognition/edge_point.h +++ /dev/null @@ -1,148 +0,0 @@ -#ifndef EDGE_POINT_H -#define EDGE_POINT_H - -#include -#include -//#include "lms/imaging/draw_debug.h" -//#include "lms/imaging/image.h" -//#include "lms/deprecated.h" -//#include "lms/math/vertex.h" -//#include "lms/config.h" -#include "drive_ros_image_recognition/image_object.h" -//#include "lms/imaging_detection/sobel_array.h" -//#include "lms/imaging/image_factory.h" - -#include -#include - -namespace drive_ros_image_recognition{ -namespace detection{ -/** - * @brief Find a Low-High edge, High-Low edge or plane, beginning from a - * starting point and moving to a target point specified by angle and distance - * relative to the starting point. - * - * The sobel angle is computed for every point along the search line to - * find an adge. - */ - -// point2f inheritance to store point as before -class EdgePoint: public ImageObject, public cv::Point2f {//: public lms::math::vertex2f,public ImageObject { -public: - static constexpr int TYPE = 0; - enum class EdgeType {LOW_HIGH, HIGH_LOW, PLANE}; - - // completely get rid of sobel array stuff - struct EdgePointParam{ //:public SobelArray::SobelArrayParam{ - EdgePointParam() : x(0), y(0), target(nullptr), searchLength(0), - searchAngle(0), searchType(EdgeType::PLANE), sobelThreshold(0), - findMax(false) { - } - - // todo: replace with dynamic reconfigure or nodehandle params in the future (ROS-native) - virtual void fromConfig (const EdgePointParam *config) {//(const lms::Config *config){ - // todo: get rid of those params completely, together with sobelArray class, use smarter reconfigure instead - //SobelArrayParam::fromConfig(config); -// if(config->hasKey("sobelThreshold")) -// sobelThreshold = config->get("sobelThreshold",150); -// if(config->hasKey("findMax")) -// findMax = config->get("findMax",false); - //TODO searchType = config->get("searchType",EdgeType::PLANE); - - } - - inline EdgePointParam operator=(const EdgePointParam& a) { - x=a.x; - y=a.y; - target.reset(new cv::Mat(*a.target)); - searchLength = a.searchLength; - searchAngle = a.searchAngle; - searchType = a.searchType; - findMax = a.findMax; - sobelThreshold = a.sobelThreshold; - } - - int x; - int y; - - // now using OpenCV images - std::unique_ptr target; - float searchLength; - /** - * @brief searchAngle in rad, don't forget that y is pointing downwards! - */ - float searchAngle; - EdgeType searchType; - int sobelThreshold; - bool findMax; - }; - - typedef EdgePointParam parameterType; - -private: - EdgePointParam m_searchParam; - /** - * @brief m_sobelX < 0 if the pixels on the left are darker - */ - int m_sobelX; - - /** - * @brief m_sobelY < 0 if the pixels on the top are darker - */ - int m_sobelY; - float m_sobelNormal; - float m_sobelTangent; - EdgeType m_type; - - /** - * @brief setType "calculates" the type high_low/low_high edge - * @return the found type - */ - EdgePoint::EdgeType calculateType(); - -public: - void setSearchParam(const EdgePointParam &searchParam); - EdgePointParam &searchParam(){ - return m_searchParam; - } - - /** - * @brief Start searching for an edge. - * @return true if an edge of the specified type and the minimum threshold - * is found, otherwise false - */ - // was receiving debug parameter before - bool find() override; - bool find(const EdgePointParam &searchParam); //DRAWDEBUG_PARAM); - int sobelX(); - int sobelY(); - - /** - * @brief sobelAngle - * @return the angle from -PI to PI - */ - float sobelTangent(); - float sobelNormal(); - /** - * @brief findAlongLine starts at x,y and tries to find a sobel that is greater then the threshold - * @return true if an edge of the specified type was found - */ - bool findAlongLine(); - /** - TODO: refactor(not smart at all) - * @brief findAlongLine starts at x,y and tries to find a sobel that is greater then the threshold and is a local max/min - * @return true if an edge of the specified type was found - */ - bool findMaxALongLine(); - /** - * @brief type - * @return the type of the EdgePoint - */ - EdgeType type(); - int getType() const override; -}; - -} //namespace detection -} //namespace drive_ros_image_recognition - -#endif // IMAGE_EDGE_POINT diff --git a/include/drive_ros_image_recognition/image_object.h b/include/drive_ros_image_recognition/image_object.h deleted file mode 100644 index 8da6b9b..0000000 --- a/include/drive_ros_image_recognition/image_object.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef IMAGE_OBJECT_H -#define IMAGE_OBJECT_H - -namespace drive_ros_image_recognition { -namespace detection { -class ImageObject{ -public: - /** - * @brief getType - * @return the type of the ImageObject - */ - virtual int getType() const = 0; - - /** - * @brief Start searching for an edge. - * @return true if an edge of the specified type and the minimum threshold - * is found, otherwise false - */ - virtual bool find() = 0; -}; -} -} - -#endif //IMAGE_OBJECT_H diff --git a/include/drive_ros_image_recognition/line_point.h b/include/drive_ros_image_recognition/line_point.h deleted file mode 100644 index c2a7d48..0000000 --- a/include/drive_ros_image_recognition/line_point.h +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef LINE_POINT_H -#define LINE_POINT_H - -#include "drive_ros_image_recognition/edge_point.h" - -namespace drive_ros_image_recognition{ -namespace detection{ - - -class LinePoint: public ImageObject { - -public: - static constexpr int TYPE = 2; - - struct LinePointParam:public EdgePoint::EdgePointParam{ - LinePointParam():lineWidthMin(0),lineWidthMax(0),useSobel(false),edge(false){ - } - virtual void fromConfig(const lms::Config *config){ - EdgePointParam::fromConfig(config); - if(config->hasKey("lineWidthMax")) - lineWidthMax = config->get("lineWidthMax",10); - if(config->hasKey("lineWidthMin")) - lineWidthMin = config->get("lineWidthMin",1); - if(config->hasKey("edge")) - edge = config->get("edge",false); - if(config->hasKey("useSobel")) - useSobel = config->get("useSobel",false); - } - float lineWidthMin; - float lineWidthMax; - /** - * @brief useSobel if true the sobel-angle will be used to find the second point (not recommended!) - */ - bool useSobel; - /** - * @brief edge if set to true it will only search for a low_high edge! - */ - bool edge; - }; - - typedef LinePointParam parameterType; - - void setParam(const LinePointParam ¶m); - LinePointParam& param(); - const LinePointParam& param() const; - - bool find(const LinePointParam ¶m); - bool find(); - - EdgePoint low_high, high_low; - float getAngle(); - float getSlope(); - float distance(); - int getType() const override; - - int getX() const; - int getY() const; - - /** - * @brief findAlongLine starts at x,y and tries to find a sobel that is greater then the threshold - * @return true if an edge of the specified type was found - */ - bool findAlongLine(); - /** - * @brief findAlongLine starts at x,y and tries to find a sobel that is greater then the threshold and is a local max/min - * @return true if an edge of the specified type was found - */ - bool findMaxALongLine(); - -private: - LinePointParam m_LinePointParam; -}; - -} //namespace detection -} //namespace drive_ros_image_recognition -#endif // LINE_POINT_H diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 69209ce..1d1e988 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -9,7 +9,6 @@ #include #include "geometry_common.h" -#include "image_object.h" #include "drive_ros_image_recognition/RoadLane.h" typedef boost::shared_ptr CvImagePtr; diff --git a/src/edge_point.cpp b/src/edge_point.cpp deleted file mode 100644 index 352bd2a..0000000 --- a/src/edge_point.cpp +++ /dev/null @@ -1,278 +0,0 @@ -#include - -namespace drive_ros_image_recognition{ -namespace detection{ - -void EdgePoint::setSearchParam(const EdgePointParam &searchParam){ - m_searchParam = searchParam; -} - -// Sets parameter and searches -bool EdgePoint::find(const EdgePointParam &searchParam){ //DRAWDEBUG_PARAM_N){ - setSearchParam(searchParam); - return find(); -} - -bool EdgePoint::find(){ - // stored search paramter (=config) determines what sobel looks for (max or not) - if(m_searchParam.findMax){ - return findMaxALongLine(); - }else{ - return findAlongLine(); - } -} - -bool EdgePoint::findAlongLine(){ - // make sobel region size adjustible - int sobel_region_size = 5; - - //pointer so it can be set in the bresenhamLine-function - bool found= false; - //end-points for the bresenham-function - int xMax = m_searchParam.x+m_searchParam.searchLength*cos(m_searchParam.searchAngle); - int yMax = m_searchParam.y+m_searchParam.searchLength*sin(m_searchParam.searchAngle); - - // hardcopy image to be able to process it - cv::Mat img_processed = *m_searchParam.target; - - // sobel entire image (simpler to implement for now, test against convoluting just at image location) - int ddepth = CV_16S; - cv::GaussianBlur( img_processed, img_processed, cv::Size(sobel_region_size,sobel_region_size), 0); - cv::Mat grad_x, grad_y; - cv::Sobel( img_processed, grad_x, ddepth, 1, 0); - cv::Sobel( img_processed, grad_y, ddepth, 0, 1); - - // get line iterator for Bresenham and do the exact same - cv::LineIterator it(*m_searchParam.target,cv::Point2i(m_searchParam.x,m_searchParam.y),cv::Point2i(xMax,yMax)); - - for(int i = 0; i < it.count; i++, ++it) - { - //check if the point is valid - // transfer this from sobel array -// if(m_searchParam.useBlackList){ -// if(m_searchParam.blackList.contains(it.pos().x,it.pos().y)){ -// return true; -// } -// } - -#ifndef NDEBUG - // DEBUG DRAW POINT, MARK WITH CIRCLE - cv::namedWindow("Edge point debug",CV_WINDOW_NORMAL); - cv::Mat disp = *m_searchParam.target; - cv::circle(disp, it.pos(),1,cv::Scalar(255)); - cv::imshow("Edge point debug",disp); -#endif - // convolute at pixel as alternative (as in existing code) - // // gaussian blur around the search point (fixed region size) - // cv::Rect region(it.pos().x-sobel_region_size, it.pos().x-it.pos().x, sobel_region_size, sobel_region_size); - // cv::GaussianBlur(img_processed(region), img_processed(region), cv::Size(0, 0), 5); - // // todo: convolute with sobel kernels for x and y at location - - m_sobelX = grad_x.at(it.pos().x,it.pos().y); - m_sobelY = grad_x.at(it.pos().x,it.pos().y); - - if(pow(m_sobelX,2)+pow(m_sobelY,2) > pow(m_searchParam.sobelThreshold,2)){ - //found an edge - //set the type - calculateType(); - if(type() == m_searchParam.searchType){ - //found an edge you were looking for :) - //calculate the angle - - m_sobelNormal = atan2(m_sobelY,m_sobelX); - m_sobelTangent = m_sobelNormal; - //TODO doesnt care about rotation-count of the searchAngle % PI - if(-M_PI_2 x =it.pos().x; - this->y =it.pos().y; - - //TODO check if the sobel-angle is in the threshold - found = true; - //stop the bresenham - return found; - } - } - } - return found; - - // lms::math::bresenhamLine(m_searchParam.x,m_searchParam.y,xMax,yMax,[this,&found DRAWDEBUG_CAPTURE](int _x, int _y){ - // //check if points are inside the image - // if(_x < 0 || _x > m_searchParam.target->width() || _y < 0 || _y >m_searchParam.target->height()) - // return false; - // //check if the point is valid - // if(m_searchParam.useBlackList){ - // if(m_searchParam.blackList.contains(_x,_y)){ - // return true; - // } - // } - - // //draw debug point - // DRAWPOINT(_x,_y,0,0,255); - // //gauss surrounding - - // int xMin = _x-2; - // int xMax = _x+2; - // int yMin = _y-2; - // int yMax = _y+2; - // /* - // * TODO that could be optimized as the next pixel will be inside the gaussed rectangle! - // * That's why we don't need to gauss a rectangle. Gaussing 3 to 5 pixel instead of 9 would ne enough - // */ - // op::gaussBox(*m_searchParam.target,*m_searchParam.gaussBuffer,xMin,yMin,xMax,yMax); - // //for 5x5 sobel - // //sobel pxl - // /* - // std::cout<< "MATTTTTTTTTTTTTTT: "<data()+m_searchParam.gaussBuffer->width()*y + x ))<< " , "; - // } - // std::cout << std::endl; - // } - // */ - // //m_sobelX = op::imageOperator(*m_searchParam.gaussBuffer,_x,_y,&op::KERNEL_SOBEL_5_X[0][0],5,5); - // //m_sobelY = op::imageOperator(*m_searchParam.gaussBuffer,_x,_y,&op::KERNEL_SOBEL_5_Y[0][0],5,5); - // //m_searchParam.sobelThreshold = 1000; - // m_sobelX = -op::sobelX(_x,_y,*m_searchParam.gaussBuffer); - // m_sobelY = op::sobelY(_x,_y,*m_searchParam.gaussBuffer); - - // //check if gradient of sobel is big enough - // if(pow(m_sobelX,2)+pow(m_sobelY,2) > pow(m_searchParam.sobelThreshold,2)){ - // //found an edge - // //set the type - // calculateType(); - // if(type() == m_searchParam.searchType){ - // //found an edge you were looking for :) - // //calculate the angle - - // m_sobelNormal = atan2(m_sobelY,m_sobelX); - // m_sobelTangent = m_sobelNormal; - // //TODO doesnt care about rotation-count of the searchAngle % PI - // if(-M_PI_2 x =_x; - // this->y =_y; - - // //TODO check if the sobel-angle is in the threshold - // found = true; - // //stop the bresenham - // return false; - // } - // } - // //continue searching points! - // return true; - // }); - // return found; -} - -EdgePoint::EdgeType EdgePoint::calculateType() { - float x2 = cos(m_searchParam.searchAngle); - float y2 = -sin(m_searchParam.searchAngle); //- wegen nach unten zeigender y-Achse - float scalar = -sobelX()*x2+sobelY()*y2; - - //std::cout << "SOBEL-VAL"< 0){ - m_type = EdgeType::LOW_HIGH; - }else if(scalar < 0){ - m_type = EdgeType::HIGH_LOW; - }else{ - m_type = EdgeType::PLANE; - } - return m_type; -} - - -bool EdgePoint::findMaxALongLine(){ - // sobel entire image and check the direction - //SobelArray sa; - cv::Mat img_processed = *m_searchParam.target; - cv::Mat grad_x, grad_y; - int sobel_region_size = 2; - - int ddepth = CV_16S; - cv::GaussianBlur( img_processed, img_processed, cv::Size(sobel_region_size,sobel_region_size), 0); - cv::Sobel( img_processed, grad_x, ddepth, 1, 0); - cv::Sobel( img_processed, grad_y, ddepth, 0, 1); - float maxSobel = 0; - int maxIndex = -1; - - // add normal line iteration - int xMax = m_searchParam.x+m_searchParam.searchLength*cos(m_searchParam.searchAngle); - int yMax = m_searchParam.y+m_searchParam.searchLength*sin(m_searchParam.searchAngle); - // if(!sa.find(m_searchParam DRAWDEBUG_ARG)){ - // //Should never happen - // return false; - // } - - // get line iterator for Bresenham ( completely replaced sobel array) - cv::LineIterator it(*m_searchParam.target,cv::Point2i(m_searchParam.x,m_searchParam.y),cv::Point2i(xMax,yMax)); - -// for(uint i = 0; i < sa.sobelVals.size();i++ ){ - for(int i = 0; i < it.count; i++, ++it) - { -// SobelArray::SobelVal sv = sa.sobelVals[i]; - //check if it is smaller then the threshold - float currentSobel = pow(pow(grad_x.at(it.pos().x,it.pos().y),2)+pow(grad_y.at(it.pos().x,it.pos().y),2),0.5);//pow(pow(sv.sobelX,2)+pow(sv.sobelY,2),0.5); - if(currentSobel < m_searchParam.sobelThreshold || currentSobel < maxSobel){ - continue; - } - //found new maxSobel, have to check the type - m_sobelX = it.pos().x; - m_sobelY = it.pos().y; -// m_sobelX = sv.sobelX; -// m_sobelY = sv.sobelY; - if(m_searchParam.searchType !=calculateType()){ - //wrong type - continue; - } - - // set max position values directly after finding a new max - this->x = it.pos().x; - this->y = it.pos().y; -// maxIndex = i; - } - // do not need this -// if(maxIndex == -1) -// return false; - - //set pos etc. -// x = sa.sobelVals[maxIndex].xPos; -// y = sa.sobelVals[maxIndex].yPos; - return true; - -} - -int EdgePoint::sobelX(){ - return m_sobelX; -} - -int EdgePoint::sobelY(){ - return m_sobelY; -} - -float EdgePoint::sobelTangent(){ - return m_sobelTangent; -} - -float EdgePoint::sobelNormal(){ - return m_sobelNormal; -} - -EdgePoint::EdgeType EdgePoint::type(){ - return m_type; -} - -int EdgePoint::getType() const{ - return EdgePoint::TYPE; -} - -} // namespace detection -} // namespace drive_ros_image_recognition diff --git a/src/line_point.cpp b/src/line_point.cpp deleted file mode 100644 index b181a5f..0000000 --- a/src/line_point.cpp +++ /dev/null @@ -1,174 +0,0 @@ -//#include "lms/imaging_detection/line_point.h" -//#include "lms/imaging_detection/edge_point.h" -//#include -#include - -namespace drive_ros_image_recognition{ -namespace detection{ - - - -void LinePoint::setParam(const LinePointParam ¶m){ - m_LinePointParam = param; -} - -bool LinePoint::find(const LinePointParam ¶m){ - setParam(param); - return find(DRAWDEBUG_ARG_N); -} - -bool LinePoint::find(){ - if(m_LinePointParam.findMax){ - return findMaxALongLine(); - }else{ - return findAlongLine(); - } -} - -bool LinePoint::findMaxALongLine(){ - SobelArray sa; - float maxSobel = 0; - (void)maxSobel;//TODO - int maxIndex = -1; - if(!sa.find(m_LinePointParam)){ - //Should never happen - return false; - } - for(uint i = 0; i < sa.sobelVals.size();i++ ){ - SobelArray::SobelVal sv = sa.sobelVals[i]; - //check if it is smaller then the threshold - float currentSobel = pow(pow(sv.sobelX,2)+pow(sv.sobelY,2),0.5); - if(currentSobel < m_LinePointParam.sobelThreshold){ - continue; - } - //TODO find min and max - - - maxIndex = i; - } - if(maxIndex == -1) - return false; - //TODO set vals - return true; -} - - -bool LinePoint::findAlongLine(){ - - //try to find first point, if it fails return as no LinePoint can be found - EdgePoint::EdgePointParam param = m_LinePointParam; - //the first searchPoint is already set in the params, just need to set the EdgeType - m_LinePointParam.searchType = EdgePoint::EdgeType::LOW_HIGH; - //try to find the first edge - if(!low_high.find(m_LinePointParam)){ - //no edge found :( - return false; - } - - //draw the found point - DRAWCROSS(low_high.x,low_high.y,0,255,0); - //check if the user is only looking for one edge - if(m_LinePointParam.edge){ - //the low_high edge was found :) - return true; - } - - //set new values for second edge - param.searchType = EdgePoint::EdgeType::HIGH_LOW; - param.x = low_high.x; - param.y = low_high.y; - param.searchLength = m_LinePointParam.lineWidthMax; - //TODO use the sobel angle? - //TODO: Maybe do some error checking on the sobelAngle? - //param.searchAngle = low_high.sobelNormal(); - /* - std::cout << "#######################"<data()+param.gaussBuffer->width()*y + x ))<< " , "; - } - std::cout << std::endl; - } - - std::cout<<"LOW_HIGH: "<high and high->low edge! - DRAWCROSS(high_low.x,high_low.y,255,255,0); - - //check the width of the linePoint is valid - float _distance = distance(); - if(_distance < m_LinePointParam.lineWidthMin || _distance > m_LinePointParam.lineWidthMax){ - //width is not valid :( - return false; - } - return true; -} - -float LinePoint::distance(){ - return low_high.distance(high_low); -} - -float LinePoint::getAngle(){ - return atan2(high_low.y-low_high.y,high_low.x-low_high.x); -} - -float LinePoint::getSlope(){ - float dx = low_high.x-high_low.x; - float dy = low_high.y-high_low.y; - return dy/dx; -} - -LinePoint::LinePointParam& LinePoint::param(){ - return m_LinePointParam; -} - -const LinePoint::LinePointParam& LinePoint::param() const{ - return m_LinePointParam; -} -int LinePoint::getType() const{ - return LinePoint::TYPE; -} - - -int LinePoint::getX() const{ - if(param().edge){ //TODO check what type should be found - return low_high.x; - }else{ - return (low_high.x + high_low.x)/2; - } -} -int LinePoint::getY() const{ - if(param().edge){ //TODO check what type should be found - return low_high.y; - }else{ - return (low_high.y + high_low.y)/2; - } -} - - -} //namespace detection -} //namespace drive_ros_image_recognition From 36d5e36e29d47fa437431d6fdf4536ba67f22d2e Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Thu, 6 Jul 2017 16:21:33 +0200 Subject: [PATCH 25/94] publish debug image for street_crossing --- .../street_crossing.h | 10 +- src/street_crossing.cpp | 291 ++++++++++-------- 2 files changed, 166 insertions(+), 135 deletions(-) diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 69209ce..ed649eb 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -12,6 +12,8 @@ #include "image_object.h" #include "drive_ros_image_recognition/RoadLane.h" +//#define DRAW_DEBUG + typedef boost::shared_ptr CvImagePtr; inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& imageIn) { @@ -75,10 +77,10 @@ class StreetCrossingDetection { void roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& roadIn); void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); - // help: how do publish debug points? -#ifdef PUBLISH_DEBUG -// image_transport::Publisher debug_img_pub_; - image_transport::Publisher detectedPointsPublisher; +#ifdef DRAW_DEBUG + image_transport::Publisher debugImagePublisher; + cv::Mat debugImage; +// image_transport::Publisher detectedPointsPublisher; #endif ros::ServiceClient worldToImageClient; diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index 995fc88..d263a2e 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -1,5 +1,5 @@ #include -#include "opencv2/highgui/highgui.hpp" +#include #include #include "drive_ros_image_recognition/warp_image.h" #include "drive_ros_image_recognition/street_crossing.h" @@ -10,11 +10,14 @@ namespace detection { StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh_, const ros::NodeHandle pnh_) : nh(nh_) // help: difference nh und pnh (whats the input) , pnh(pnh_) - , imageTransport(pnh_) + , imageTransport(nh_) , sobelThreshold(50) , minLineWidthMul(0.5) , maxLineWidthMul(2.0) , lineWidth(0.4) +#ifdef DRAW_DEBUG + , debugImage(0, 0, CV_16S) +#endif { // todo } @@ -25,62 +28,65 @@ StreetCrossingDetection::~StreetCrossingDetection() { bool StreetCrossingDetection::init() { imageSubscriber = imageTransport.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); - roadSubscriber = pnh.subscribe("road_in", 100, &StreetCrossingDetection::roadCallback, this); + //roadSubscriber = pnh.subscribe("road_in", 100, &StreetCrossingDetection::roadCallback, this); // todo: advertise lines // help: serviceClients - imageToWorldClient = nh.serviceClient("/ImageToWorld"); - worldToImageClient = nh.serviceClient("/WorldToImage"); +// imageToWorldClient = nh.serviceClient("/ImageToWorld"); +// worldToImageClient = nh.serviceClient("/WorldToImage"); // todo: debug channels + // help: how to publish debug picture / points? +#ifdef DRAW_DEBUG + debugImagePublisher = imageTransport.advertise("/street_crossing/debug_image", 10); +#endif return true; } -bool StreetCrossingDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &world_point) { - drive_ros_image_recognition::ImageToWorld srv; - geometry_msgs::Point image_point_send; - image_point_send.x = image_point.x; - image_point_send.y = image_point.y; - image_point_send.z = 0.0; - srv.request.image_point = image_point_send; - if (imageToWorldClient.call(srv)) - { - world_point.x = srv.response.world_point.point.x; - world_point.y = srv.response.world_point.point.y; - return true; - } - else - { - ROS_ERROR("Failed to call service ImageToWorld"); - return false; - } -} - -bool StreetCrossingDetection::worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { - drive_ros_image_recognition::WorldToImage srv; - geometry_msgs::PointStamped world_point_send; - world_point_send.point.x = world_point.x; - world_point_send.point.y = world_point.y; - world_point_send.point.z = 0.0; - // todo: check if this is valid, we assume the trajectory is defined in the axis reference frame - world_point_send.header.frame_id = "tf_front_axis_middle"; - srv.request.world_point = world_point_send; - if (worldToImageClient.call(srv)) - { - image_point.x = (int)srv.response.image_point.x; - image_point.y = (int)srv.response.image_point.y; - return true; - } - else - { - ROS_ERROR("Failed to call service WorldToImage"); - return false; - } -} +//bool StreetCrossingDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &world_point) { +// drive_ros_image_recognition::ImageToWorld srv; +// geometry_msgs::Point image_point_send; +// image_point_send.x = image_point.x; +// image_point_send.y = image_point.y; +// image_point_send.z = 0.0; +// srv.request.image_point = image_point_send; +// if (imageToWorldClient.call(srv)) +// { +// world_point.x = srv.response.world_point.point.x; +// world_point.y = srv.response.world_point.point.y; +// return true; +// } +// else +// { +// ROS_ERROR("Failed to call service ImageToWorld"); +// return false; +// } +//} + +//bool StreetCrossingDetection::worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { +// drive_ros_image_recognition::WorldToImage srv; +// geometry_msgs::PointStamped world_point_send; +// world_point_send.point.x = world_point.x; +// world_point_send.point.y = world_point.y; +// world_point_send.point.z = 0.0; +// // todo: check if this is valid, we assume the trajectory is defined in the axis reference frame +// world_point_send.header.frame_id = "tf_front_axis_middle"; +// srv.request.world_point = world_point_send; +// if (worldToImageClient.call(srv)) +// { +// image_point.x = (int)srv.response.image_point.x; +// image_point.y = (int)srv.response.image_point.y; +// return true; +// } +// else +// { +// ROS_ERROR("Failed to call service WorldToImage"); +// return false; +// } +//} // todo: -// callbacks // dynamic reconfigure void StreetCrossingDetection::roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr &roadIn) { @@ -89,11 +95,15 @@ void StreetCrossingDetection::roadCallback(const drive_ros_image_recognition::Ro void StreetCrossingDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { currentImage = convertImageMessage(imgIn); - if(roadBuffer.points.size() > 1) { - findStartline(); - } else { - ROS_WARN("Not searching for start line, since road-buffer has no points stored"); - } +#ifdef DRAW_DEBUG + debugImage = currentImage->clone(); +#endif +// if(roadBuffer.points.size() > 1) { +// findStartline(); +// } else { +// ROS_WARN("Not searching for start line, since road-buffer has no points stored"); +// } + findStartline(); } bool StreetCrossingDetection::findStartline(/*linestring& middleLine*/) { @@ -104,122 +114,141 @@ bool StreetCrossingDetection::findStartline(/*linestring& middleLine*/) { searchLines.clear(); // Get middle line with distance and move to right - for(auto point : roadBuffer.points) { - boost::geometry::append(middleLine, point_xy(point.x, point.y)); - } - drive_ros_geometry_common::moveOrthogonal(middleLine, tmp, lineWidth / 2); - drive_ros_geometry_common::simplify(tmp, rightMiddleLine, hightStartLine + 0.01); +// for(auto point : roadBuffer.points) { +// boost::geometry::append(middleLine, point_xy(point.x, point.y)); +// } +// drive_ros_geometry_common::moveOrthogonal(middleLine, tmp, lineWidth / 2); +// drive_ros_geometry_common::simplify(tmp, rightMiddleLine, hightStartLine + 0.01); //get all lines - for(int i = 1; i < rightMiddleLine.size(); i++) { - SearchLine l; - // todo: if this works, make it more efficient - l.wStart = cv::Point2f(rightMiddleLine.at(i - 1).x(), rightMiddleLine.at(i - 1).y()); - l.wEnd = cv::Point2f(rightMiddleLine.at(i).x(), rightMiddleLine.at(i).y()); - -// cv::Point l_w_start, l_w_end; -// worldToImage(l.w_start, l_w_start); -// worldToImage(l.w_end, l_w_end); - worldToImage(l.wStart, l.iStart); - worldToImage(l.wEnd, l.iEnd); - cv::Rect imgRect(cv::Point(),cv::Point(currentImage->cols, currentImage->rows)); -// if(!imgRect.contains(l_w_start)){ -// // try middle lane -> should always be in image -// l.w_start = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); -// worldToImage(l.w_start, l_w_start); -// if(imgRect.contains(l_w_start)){ -// continue; -// } -// }else if(!imgRect.contains(l_w_end)){ -// l.w_end = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); -// } - - //transform them in image-coordinates -// std::unique_lock lock(mutex); -// worldToImage(l.w_start,l.i_start); -// worldToImage(l.w_end,l.i_end); -// lines_.push_back(l); -// linesToProcess_++; - - if((imgRect.contains(l.iStart)) && (imgRect.contains(l.iEnd))) { - searchLines.push_back(l); - } - - // todo: figure out what this does (no internet) - // conditionNewLine.notify_one(); - } +// for(int i = 1; i < rightMiddleLine.size(); i++) { +// SearchLine l; +// // todo: if this works, make it more efficient +// l.wStart = cv::Point2f(rightMiddleLine.at(i - 1).x(), rightMiddleLine.at(i - 1).y()); +// l.wEnd = cv::Point2f(rightMiddleLine.at(i).x(), rightMiddleLine.at(i).y()); + +//// cv::Point l_w_start, l_w_end; +//// worldToImage(l.w_start, l_w_start); +//// worldToImage(l.w_end, l_w_end); +// worldToImage(l.wStart, l.iStart); +// worldToImage(l.wEnd, l.iEnd); +// cv::Rect imgRect(cv::Point(),cv::Point(currentImage->cols, currentImage->rows)); +//// if(!imgRect.contains(l_w_start)){ +//// // try middle lane -> should always be in image +//// l.w_start = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); +//// worldToImage(l.w_start, l_w_start); +//// if(imgRect.contains(l_w_start)){ +//// continue; +//// } +//// }else if(!imgRect.contains(l_w_end)){ +//// l.w_end = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); +//// } + +// //transform them in image-coordinates +//// std::unique_lock lock(mutex); +//// worldToImage(l.w_start,l.i_start); +//// worldToImage(l.w_end,l.i_end); +//// lines_.push_back(l); +//// linesToProcess_++; + +//// if((imgRect.contains(l.iStart)) && (imgRect.contains(l.iEnd))) { +//// searchLines.push_back(l); +//// } + +// // todo: figure out what this does (no internet) +// // conditionNewLine.notify_one(); +// } - ROS_INFO("Number of search-lines to search stop-line: %lu", searchLines.size()); +// ROS_INFO("Number of search-lines to search stop-line: %lu", searchLines.size()); // Sobel - // todo: naming and usage of mat's cv::Mat /*imgGaussian,*/ imgSobel, absGradiants; -// cv::GaussianBlur(*currentImage, imgGaussian, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT); - cv::Sobel(*currentImage, imgSobel, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT); - cv::convertScaleAbs(imgSobel, absGradiants); - - // use search-lines to find stop-line - // todo: if this works, parallize it! - std::vector linePoints; - for(auto sl : searchLines) { - auto tmp = processSearchLine(sl); - linePoints.insert(linePoints.end(), tmp.begin(), tmp.end()); - } - - - // Draw points on image + // cv::GaussianBlur(*currentImage, imgGaussian, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT); + cv::Sobel(*currentImage, imgSobel, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT); +// cv::convertScaleAbs(imgSobel, absGradiants); + cv::convertScaleAbs(imgSobel, debugImage); + + // use search-lines to find stop-line + // todo: if this works, parallize it! + std::vector linePoints; + // for(auto sl : searchLines) { + // auto tmp = processSearchLine(sl); + // linePoints.insert(linePoints.end(), tmp.begin(), tmp.end()); + // } + + SearchLine mySl; + mySl.iStart = cv::Point2f(currentImage->cols / 2, currentImage->rows * 0.5); + mySl.iEnd = cv::Point2f(currentImage->cols / 2, currentImage->rows * .75); + +// imageToWorld(mySl.iStart, mySl.wStart); +// imageToWorld(mySl.iEnd, mySl.wEnd); + linePoints = processSearchLine(mySl); + + // Draw / publish points + #ifdef DRAW_DEBUG + for(auto point : linePoints) { + ROS_INFO("drawing line-point"); + cv::circle(debugImage, point, 2, cv::Scalar(255, 0, 0)); + } +// cv::circle(debugImage, mySl.iStart, 2, cv::Scalar(0, 255, 0)); +// cv::circle(debugImage, mySl.iEnd, 2, cv::Scalar(0, 255, 0)) + cv::line(debugImage, mySl.iStart, mySl.iEnd, cv::Scalar(255)); + debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debugImage).toImageMsg()); + #endif -} + } -std::vector StreetCrossingDetection::processSearchLine(SearchLine &sl) { - std::vector foundPoints; + std::vector StreetCrossingDetection::processSearchLine(SearchLine &sl) { + std::vector foundPoints; bool foundLowHigh = false; int pxlCounter = 0; cv::Point2f iStartPxlPeak; cv::LineIterator lineIt(*currentImage, sl.iStart, sl.iEnd); + ROS_INFO("processSearchLine"); float iDist = cv::norm(sl.iEnd - sl.iStart); - float wDist = cv::norm(sl.wEnd - sl.wStart); +// float wDist = cv::norm(sl.wEnd - sl.wStart); + cv::Rect rect(cv::Point(),cv::Point(currentImage->rows, currentImage->cols)); for(int i = 0; i < lineIt.count; i++, ++lineIt) { // safety check : is the point inside the image -// if(!rect.contains(it.pos())){ -// ROS_WARN("StreetCrossing::processSearchLine: Received an invalid point outside the image to check for Sobel"); -// return foundPoints; -// } + if(!rect.contains(lineIt.pos())){ + ROS_WARN("StreetCrossing::processSearchLine: Received an invalid point outside the image to check for Sobel"); + return foundPoints; + } - //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang + // Search for a bright pixel. Searching from bottom to top int sobel = (int)currentImage->at(lineIt.pos().x, lineIt.pos().y); if(sobel > sobelThreshold) { - // found low-high pass (on the left side of the line) + // found low-high pass if(!foundLowHigh) { foundLowHigh = true; pxlCounter = 0; iStartPxlPeak.x = lineIt.pos().x; iStartPxlPeak.y = lineIt.pos().y; } - } else if(sobel < -sobelThreshold){ // hell-dunkel übergang + } else if(sobel < -sobelThreshold){ + // found high-low pass //check if we found a lowHigh + highLow border if(foundLowHigh){ //check if the points have the right distance // TODO to bad, calculate for each road line (how should we use them for searching? - float pxlPeakWidth = iDist / wDist * lineWidth; //todo: understand this +// float pxlPeakWidth = iDist / wDist * lineWidth; //todo: understand this //logger.debug("")<<"crossing found highLow: "< (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { - // we found a valid point - // get the middle + // returns the middle of a line from multiple points +// if((pxlCounter > (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { + // we found a valid point, get the middle cv::Point2f iMid = (iStartPxlPeak + cv::Point2f(lineIt.pos().x, lineIt.pos().y)) * .5; - cv::Point2f wMid; - imageToWorld(iMid, wMid); - foundPoints.push_back(wMid); - ROS_DEBUG("Found a stop-line"); +// cv::Point2f wMid; +// imageToWorld(iMid, wMid); +// foundPoints.push_back(wMid); +// ROS_DEBUG("Found a stop-line"); //logger.debug("")<<"crossing FOUND VALID CROSSING"; - } +// } } pxlCounter = 0; foundLowHigh = false; From bad6f19680aae7efb98aaac22eec95c5850288af Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Fri, 7 Jul 2017 13:24:57 +0200 Subject: [PATCH 26/94] street_crossing recognizes horizontal lines in front of the car --- .../street_crossing.h | 27 +-- src/street_crossing.cpp | 225 +++++------------- 2 files changed, 75 insertions(+), 177 deletions(-) diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 1d1e988..7755fb8 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -8,9 +8,11 @@ #include #include -#include "geometry_common.h" #include "drive_ros_image_recognition/RoadLane.h" +#define DRAW_DEUBG // todo: not very nice to define this here + +// todo: a lot of this stuff is used by new_road_detection too, so we should put this in a header / class typedef boost::shared_ptr CvImagePtr; inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& imageIn) { @@ -53,40 +55,31 @@ class StreetCrossingDetection { int sobelThreshold; float minLineWidthMul; float maxLineWidthMul; - float lineWidth; // in m; todo: should be in config + float lineWidth; // todo: should be in config // variables std::vector searchLines; CvImagePtr currentImage; + CvImagePtr currentSobelImage; // communication image_transport::ImageTransport imageTransport; image_transport::Subscriber imageSubscriber; - // road inputs and outputs - ros::Subscriber roadSubscriber; - ros::Publisher linePublisher; - drive_ros_image_recognition::RoadLane roadBuffer; +// ros::Publisher linePublisher; ros::NodeHandle nh; ros::NodeHandle pnh; - void roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& roadIn); void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); - // help: how do publish debug points? -#ifdef PUBLISH_DEBUG -// image_transport::Publisher debug_img_pub_; - image_transport::Publisher detectedPointsPublisher; +#ifdef DRAW_DEUBG + image_transport::Publisher debugImagePublisher; + cv::Mat debugImage; #endif - ros::ServiceClient worldToImageClient; - ros::ServiceClient imageToWorldClient; - bool imageToWorld(const cv::Point &imagePoint, cv::Point2f &worldPoint); - bool worldToImage(const cv::Point2f &worldPoint, cv::Point &imagePoint); - // methods - bool findStartline(/*linestring &middleLine*/); + bool findStartline(); std::vector processSearchLine(SearchLine &sl); public: diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index 995fc88..08796f2 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -1,5 +1,5 @@ #include -#include "opencv2/highgui/highgui.hpp" +#include #include #include "drive_ros_image_recognition/warp_image.h" #include "drive_ros_image_recognition/street_crossing.h" @@ -8,15 +8,17 @@ namespace drive_ros_image_recognition { namespace detection { StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh_, const ros::NodeHandle pnh_) - : nh(nh_) // help: difference nh und pnh (whats the input) + : nh(nh_) , pnh(pnh_) - , imageTransport(pnh_) + , imageTransport(nh_) , sobelThreshold(50) , minLineWidthMul(0.5) , maxLineWidthMul(2.0) , lineWidth(0.4) + #ifdef DRAW_DEBUG + , debugImage(0, 0, CV_8UC1) + #endif { - // todo } StreetCrossingDetection::~StreetCrossingDetection() { @@ -25,149 +27,51 @@ StreetCrossingDetection::~StreetCrossingDetection() { bool StreetCrossingDetection::init() { imageSubscriber = imageTransport.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); - roadSubscriber = pnh.subscribe("road_in", 100, &StreetCrossingDetection::roadCallback, this); // todo: advertise lines - // help: serviceClients - imageToWorldClient = nh.serviceClient("/ImageToWorld"); - worldToImageClient = nh.serviceClient("/WorldToImage"); - - // todo: debug channels +#ifdef DRAW_DEBUG + debugImagePublisher = imageTransport.advertise("/street_crossing/debug_image", 10); +#endif return true; } -bool StreetCrossingDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &world_point) { - drive_ros_image_recognition::ImageToWorld srv; - geometry_msgs::Point image_point_send; - image_point_send.x = image_point.x; - image_point_send.y = image_point.y; - image_point_send.z = 0.0; - srv.request.image_point = image_point_send; - if (imageToWorldClient.call(srv)) - { - world_point.x = srv.response.world_point.point.x; - world_point.y = srv.response.world_point.point.y; - return true; - } - else - { - ROS_ERROR("Failed to call service ImageToWorld"); - return false; - } -} - -bool StreetCrossingDetection::worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { - drive_ros_image_recognition::WorldToImage srv; - geometry_msgs::PointStamped world_point_send; - world_point_send.point.x = world_point.x; - world_point_send.point.y = world_point.y; - world_point_send.point.z = 0.0; - // todo: check if this is valid, we assume the trajectory is defined in the axis reference frame - world_point_send.header.frame_id = "tf_front_axis_middle"; - srv.request.world_point = world_point_send; - if (worldToImageClient.call(srv)) - { - image_point.x = (int)srv.response.image_point.x; - image_point.y = (int)srv.response.image_point.y; - return true; - } - else - { - ROS_ERROR("Failed to call service WorldToImage"); - return false; - } -} - // todo: -// callbacks // dynamic reconfigure -void StreetCrossingDetection::roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr &roadIn) { - roadBuffer = *roadIn; -} - void StreetCrossingDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { currentImage = convertImageMessage(imgIn); - if(roadBuffer.points.size() > 1) { - findStartline(); - } else { - ROS_WARN("Not searching for start line, since road-buffer has no points stored"); - } + findStartline(); } -bool StreetCrossingDetection::findStartline(/*linestring& middleLine*/) { - float hightStartLine = 0.04; -// bool foundStartLine = false; - linestring rightMiddleLine, middleLine, tmp; - - searchLines.clear(); - - // Get middle line with distance and move to right - for(auto point : roadBuffer.points) { - boost::geometry::append(middleLine, point_xy(point.x, point.y)); - } - drive_ros_geometry_common::moveOrthogonal(middleLine, tmp, lineWidth / 2); - drive_ros_geometry_common::simplify(tmp, rightMiddleLine, hightStartLine + 0.01); - - //get all lines - for(int i = 1; i < rightMiddleLine.size(); i++) { - SearchLine l; - // todo: if this works, make it more efficient - l.wStart = cv::Point2f(rightMiddleLine.at(i - 1).x(), rightMiddleLine.at(i - 1).y()); - l.wEnd = cv::Point2f(rightMiddleLine.at(i).x(), rightMiddleLine.at(i).y()); - -// cv::Point l_w_start, l_w_end; -// worldToImage(l.w_start, l_w_start); -// worldToImage(l.w_end, l_w_end); - worldToImage(l.wStart, l.iStart); - worldToImage(l.wEnd, l.iEnd); - cv::Rect imgRect(cv::Point(),cv::Point(currentImage->cols, currentImage->rows)); -// if(!imgRect.contains(l_w_start)){ -// // try middle lane -> should always be in image -// l.w_start = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); -// worldToImage(l.w_start, l_w_start); -// if(imgRect.contains(l_w_start)){ -// continue; -// } -// }else if(!imgRect.contains(l_w_end)){ -// l.w_end = cv::Point2f(bg::get<0>(*it_mid),bg::get<1>(*it_mid)); -// } - - //transform them in image-coordinates -// std::unique_lock lock(mutex); -// worldToImage(l.w_start,l.i_start); -// worldToImage(l.w_end,l.i_end); -// lines_.push_back(l); -// linesToProcess_++; - - if((imgRect.contains(l.iStart)) && (imgRect.contains(l.iEnd))) { - searchLines.push_back(l); - } - - // todo: figure out what this does (no internet) - // conditionNewLine.notify_one(); - } - - ROS_INFO("Number of search-lines to search stop-line: %lu", searchLines.size()); - - // Sobel - // todo: naming and usage of mat's - cv::Mat /*imgGaussian,*/ imgSobel, absGradiants; -// cv::GaussianBlur(*currentImage, imgGaussian, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT); - cv::Sobel(*currentImage, imgSobel, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT); - cv::convertScaleAbs(imgSobel, absGradiants); - - // use search-lines to find stop-line - // todo: if this works, parallize it! - std::vector linePoints; - for(auto sl : searchLines) { - auto tmp = processSearchLine(sl); - linePoints.insert(linePoints.end(), tmp.begin(), tmp.end()); - } - - - // Draw points on image +bool StreetCrossingDetection::findStartline() { + std::vector linePoints; + SearchLine mySl; + + // Sobel + currentSobelImage.reset(new cv::Mat(currentImage->rows, currentImage->cols, CV_8UC1)); + cv::Sobel(*currentImage, *currentSobelImage, -1, 0, 1); + + // use search-line to find stop-line + mySl.iStart = cv::Point2f(currentImage->cols / 2, currentImage->rows * .8); + mySl.iEnd = cv::Point2f(currentImage->cols / 2, currentImage->rows * .4); + // imageToWorld(mySl.iStart, mySl.wStart); + // imageToWorld(mySl.iEnd, mySl.wEnd); + linePoints = processSearchLine(mySl); + + + // Draw / publish points +#ifdef DRAW_DEBUG + // debugImage = currentSobelImage->clone(); + debugImage = currentImage->clone(); + // Image dimensions:1280x344 + for(auto point : linePoints) { + cv::line(debugImage, cv::Point(point.x - 40, point.y), cv::Point(point.x + 40, point.y), cv::Scalar(255)); + } + // draw searchline + cv::line(debugImage, mySl.iStart, mySl.iEnd, cv::Scalar(255)); + debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debugImage).toImageMsg()); +#endif } @@ -176,54 +80,55 @@ std::vector StreetCrossingDetection::processSearchLine(SearchLine & bool foundLowHigh = false; int pxlCounter = 0; cv::Point2f iStartPxlPeak; - cv::LineIterator lineIt(*currentImage, sl.iStart, sl.iEnd); - - float iDist = cv::norm(sl.iEnd - sl.iStart); - float wDist = cv::norm(sl.wEnd - sl.wStart); + cv::LineIterator lineIt(*currentSobelImage, sl.iStart, sl.iEnd); + cv::Rect rect(0, 0, currentSobelImage->cols, currentSobelImage->rows); + // float iDist = cv::norm(sl.iEnd - sl.iStart); + // float wDist = cv::norm(sl.wEnd - sl.wStart); for(int i = 0; i < lineIt.count; i++, ++lineIt) { // safety check : is the point inside the image -// if(!rect.contains(it.pos())){ -// ROS_WARN("StreetCrossing::processSearchLine: Received an invalid point outside the image to check for Sobel"); -// return foundPoints; -// } + if(!rect.contains(lineIt.pos())){ + ROS_WARN("Received an invalid point outside the image to check for Sobel (%i,%i)", lineIt.pos().x, lineIt.pos().y); + return foundPoints; + } - //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang - int sobel = (int)currentImage->at(lineIt.pos().x, lineIt.pos().y); + // Search for a bright pixel. Searching from bottom to top + int sobel = currentSobelImage->at(lineIt.pos()); if(sobel > sobelThreshold) { - // found low-high pass (on the left side of the line) + // found low-high pass if(!foundLowHigh) { foundLowHigh = true; pxlCounter = 0; iStartPxlPeak.x = lineIt.pos().x; iStartPxlPeak.y = lineIt.pos().y; } - } else if(sobel < -sobelThreshold){ // hell-dunkel übergang - //check if we found a lowHigh + highLow border + } else if(sobel < -sobelThreshold) { + // found high-low pass + // check if we found a lowHigh + highLow border if(foundLowHigh){ //check if the points have the right distance // TODO to bad, calculate for each road line (how should we use them for searching? - float pxlPeakWidth = iDist / wDist * lineWidth; //todo: understand this + // float pxlPeakWidth = iDist / wDist * lineWidth; // todo: understand this //logger.debug("")<<"crossing found highLow: "< (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { - // we found a valid point - // get the middle - cv::Point2f iMid = (iStartPxlPeak + cv::Point2f(lineIt.pos().x, lineIt.pos().y)) * .5; - cv::Point2f wMid; - imageToWorld(iMid, wMid); - foundPoints.push_back(wMid); - ROS_DEBUG("Found a stop-line"); - //logger.debug("")<<"crossing FOUND VALID CROSSING"; - } + // todo: check if the line we found has the correct height + // if((pxlCounter > (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { + // return the middle of the line we found + // cv::Point2f iMid((iStartPxlPeak.x + lineIt.pos().x) *.5, (iStartPxlPeak.x + lineIt.pos().y) * .5); + + foundPoints.push_back(lineIt.pos()); + // cv::Point2f wMid; + // imageToWorld(iMid, wMid); + // foundPoints.push_back(wMid); + // ROS_DEBUG("Found a stop-line"); + // } } pxlCounter = 0; foundLowHigh = false; - //if not, we dont have to do anything + // if not, we dont have to do anything } // for calculation of line width From 848a8f45742bff4c296a905b6c161ed82f02ae80 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Fri, 7 Jul 2017 13:38:46 +0200 Subject: [PATCH 27/94] add street_crossing.launch --- CMakeLists.txt | 2 +- launch/street_crossing.launch | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) create mode 100644 launch/street_crossing.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f836c6..c2b4996 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -159,7 +159,7 @@ add_executable(street_crossing_node src/street_crossing_node.cpp ) -add_dependencies(street_crossing_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(street_crossing_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(street_crossing_node ${catkin_LIBRARIES} diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch new file mode 100644 index 0000000..4767eca --- /dev/null +++ b/launch/street_crossing.launch @@ -0,0 +1,7 @@ + + + + + + + From f857203f409680ad84aa2e6015c151cef61ed985 Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Wed, 12 Jul 2017 01:40:26 +0200 Subject: [PATCH 28/94] Move shared functions to header, remove services, todo: add parameter and homography updates to shared classes, add to crossing_detection --- CMakeLists.txt | 10 +- .../common_image_operations.h | 422 ++++++++++++++++++ .../new_road_detection.h | 65 +-- .../street_crossing.h | 52 ++- .../drive_ros_image_recognition/warp_image.h | 11 +- src/new_road_detection.cpp | 234 ++-------- src/street_crossing.cpp | 130 +++--- src/warp_image.cpp | 132 +----- srv/ImageToWorld.srv | 5 - srv/WorldToImage.srv | 5 - 10 files changed, 551 insertions(+), 515 deletions(-) create mode 100644 include/drive_ros_image_recognition/common_image_operations.h delete mode 100644 srv/ImageToWorld.srv delete mode 100644 srv/WorldToImage.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index c2b4996..3d8d23e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,9 +5,9 @@ project(drive_ros_image_recognition) SET(CMAKE_BUILD_TYPE Debug) # enable to compile with OpenCV window displays -add_definitions(-DDRAW_DEBUG) +#add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images -add_definitions(-DPUBLISH_DEBUG) +#add_definitions(-DPUBLISH_DEBUG) add_definitions(-std=c++11) @@ -33,12 +33,6 @@ add_message_files( RoadLane.msg ) -add_service_files( - FILES - WorldToImage.srv - ImageToWorld.srv -) - generate_messages( DEPENDENCIES std_msgs diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h new file mode 100644 index 0000000..b7e66e7 --- /dev/null +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -0,0 +1,422 @@ +#ifndef COMMON_IMAGE_OPERATIONS_H +#define COMMON_IMAGE_OPERATIONS_H + +#include +#include +#include +#include +#include +#include + +typedef boost::shared_ptr CvImagePtr; + +inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { + CvImagePtr cv_ptr; + try + { + // hardcopies for now, might be possible to process on pointer if fast enough + // todo: make prettier + cv_bridge::CvImagePtr temp_ptr = cv_bridge::toCvCopy(*img_in, ""); + cv_ptr.reset(new cv::Mat(temp_ptr->image) ); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return NULL; + } + + if( !cv_ptr->data) + { + ROS_WARN("Empty image received, skipping!"); + return NULL; + } + return cv_ptr; +} + + +namespace drive_ros_image_recognition { + +inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const std::string mat_param) { + // retrieve world2map and map2world homography matrices + std::vector temp_vals; + + if (!pnh.getParam("homography_matrix/"+mat_param, temp_vals)) { + ROS_ERROR("Unable to load homography matrix from configuration file!"); + return false; + } + if (temp_vals.size() != 9) { + ROS_ERROR("Retreived homography matrix does not have 9 values!"); + return false; + } + for(unsigned i=0; i < temp_vals.size(); i++) { + // todo: check: according to warp_cpp.h, those two might actually be the other way round + mat.at(i) = temp_vals[i]; + } + ROS_DEBUG_STREAM("Paramater matrix loaded as: "<(0,0) = image_point.x; + mat_point_image.at(0,1) = image_point.y; + cv::Mat mat_point_world(1,1,CV_64FC2); + cv::perspectiveTransform(mat_point_image, mat_point_world, cam2world_); + cv::Point2f cam_point(mat_point_world.at(0,0),mat_point_world.at(0,1)); + + // todo: check if it is valid to assume the z depth if we used homography before + cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); + + geometry_msgs::PointStamped camera_point; + camera_point.header.frame_id = cam_model_.tfFrame(); + camera_point.point.x = cam_ray_point.x; + camera_point.point.y = cam_ray_point.y; + camera_point.point.z = cam_ray_point.z; + geometry_msgs::PointStamped geom_world_point; + + try { + ros::Duration timeout(1.0 / 30); + tf_listener_.waitForTransform(cam_model_.tfFrame(), "tf_front_axis_middle", + ros::Time::now(), timeout); + tf_listener_.transformPoint("tf_front_axis_middle", camera_point, geom_world_point); + } + catch (tf::TransformException& ex) { + ROS_WARN("[ImageToWorld] TF exception, unable to transform:\n%s", ex.what()); + return false; + } + world_point.x = geom_world_point.point.x; + world_point.y = geom_world_point.point.y; + return true; + } + +private: + image_geometry::PinholeCameraModel cam_model_; + cv::Mat cam2world_; + tf::TransformListener tf_listener_; +#ifdef USE_WORLD2CAM_HOMOGRAPHY + cv::Mat world2cam_; +#endif +}; // class TransformHelper + +//std::vector processSearchLine(SearchLine &sl, const cv::Mat& current_image, const search_direction search_dir) { +// std::vector foundPoints; +// bool foundLowHigh = false; +// int pxlCounter = 0; +// cv::Point2f iStartPxlPeak; + +// cv::LineIterator it(*currentSobelImage, sl.iStart, sl.iEnd); + +// found_points = findBySobel(lineIt, current_image, lineWidth_, iDist, wDist, search_dir); +// cv::Rect rect(0, 0, currentSobelImage->cols, currentSobelImage->rows); +// // float iDist = cv::norm(sl.iEnd - sl.iStart); +// // float wDist = cv::norm(sl.wEnd - sl.wStart); + +// for(int i = 0; i < lineIt.count; i++, ++lineIt) { +// // safety check : is the point inside the image +// if(!rect.contains(lineIt.pos())){ + +// ROS_WARN("Received an invalid point outside the image to check for Sobel (%i,%i)", lineIt.pos().x, lineIt.pos().y); +// return foundPoints; +// } + + + +// Search for a bright pixel. Searching from bottom to top + +// int sobel = currentSobelImage->at(lineIt.pos()); + +// if(sobel > sobelThreshold) { +// // found low-high pass +// if(!foundLowHigh) { +// foundLowHigh = true; +// pxlCounter = 0; +// iStartPxlPeak.x = lineIt.pos().x; +// iStartPxlPeak.y = lineIt.pos().y; +// } +// } else if(sobel < -sobelThreshold) { +// // found high-low pass +// // check if we found a lowHigh + highLow border +// if(foundLowHigh){ +// //check if the points have the right distance +// // TODO to bad, calculate for each road line (how should we use them for searching? +// // float pxlPeakWidth = iDist / wDist * lineWidth; // todo: understand this + +// //logger.debug("")<<"crossing found highLow: "< (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { +// // return the middle of the line we found +// // cv::Point2f iMid((iStartPxlPeak.x + lineIt.pos().x) *.5, (iStartPxlPeak.x + lineIt.pos().y) * .5); + +// foundPoints.push_back(lineIt.pos()); +// // cv::Point2f wMid; +// // imageToWorld(iMid, wMid); +// // foundPoints.push_back(wMid); +// // ROS_DEBUG("Found a stop-line"); +// // } +// } +// pxlCounter = 0; +// foundLowHigh = false; +// // if not, we dont have to do anything +// } + +// // for calculation of line width +// if(foundLowHigh){ +// pxlCounter++; +// } +// } +// return foundPoints; +//} + +class ImageOperator { + +public: + + ImageOperator(TransformHelper& helper): + helper_(helper) + { + } + + std::vector findBySobel(const SearchLine &sl, + const cv::Mat& current_image, + const float lineWidth, + const float iDist, + const float wDist, + const search_direction search_dir) + { + std::vector foundPoints; + bool foundLowHigh = false; + int pxlCounter = 0; + cv::Mat current_image_sobel(current_image.rows,current_image.cols,CV_8UC1,cv::Scalar(0)); + cv::LineIterator it(current_image_sobel, sl.iStart, sl.iEnd); + cv::LineIterator it_backup = it; + if (search_dir == search_direction::x) + cv::Sobel( current_image, current_image_sobel, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); + else if(search_dir == search_direction::y) { + // todo: in Simon's code this used a kernel size of 1 which did not really make sense, but check if this is valid anyway + cv::Sobel( current_image, current_image_sobel, CV_8UC1, -1, 0, 3, 1, 0, cv::BORDER_DEFAULT); + } + + cv::Rect rect(cv::Point(),cv::Point(current_image.rows, current_image.cols)); + + for(int i = 0; i < it.count; i++, ++it) + { + // safety check : is the point inside the image + if(!rect.contains(it.pos())){ + ROS_WARN("Received an invalid point outside the image to check for Sobel"); + return foundPoints; + } + + //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang + int sobel = (int)current_image_sobel.at(it.pos().x,it.pos().y); + if(sobel > config_.sobelThreshold){ + // found low-high pass (on the left side of the line) + if(!foundLowHigh){ + foundLowHigh = true; + pxlCounter = 0; + } + }else if(sobel < -config_.sobelThreshold){ //hell-dunkel übergang + //check if we found a lowHigh + highLow border + if(foundLowHigh){ + //check if the points have the right distance + float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? + + ROS_DEBUG_STREAM("crossing found highLow: "< pxlPeakWidth*config_.minLineWidthMul && pxlCounter < pxlPeakWidth*config_.maxLineWidthMul){ + //we found a valid point + //get the middle + // todo: make this more elegant in both sobel and brightness (no std::advance for lineIterator) + for (int iter = 0; iter<(i-pxlCounter/2); iter++, ++it_backup) { + } + cv::Point2f wMid; + helper_.imageToWorld(it_backup.pos(),wMid); + foundPoints.push_back(wMid); + ROS_DEBUG("crossing FOUND VALID CROSSING"); + } + } + pxlCounter = 0; + foundLowHigh = false; + //if not, we dont have to do anything + } + + // for calculation of line width + if(foundLowHigh){ + pxlCounter++; + } + } + return foundPoints; + } + + std::vector findByBrightness( const SearchLine &sl, + const cv::Mat& current_image, + const float lineWidth, + const float iDist, + const float wDist, + const search_direction search_dir) + { + std::vector foundPoints; + std::vector color; + cv::Rect rect(cv::Point(),cv::Point(current_image.rows, current_image.cols)); + cv::LineIterator it(current_image, sl.iStart, sl.iEnd); + cv::LineIterator it_debug = it; + + for(int i = 0; i < it.count; i++, ++it) { + // safety check : is the point inside the image + if(!rect.contains(it.pos())){ + ROS_WARN("Received an invalid point outside the image to check for brightness"); + color.push_back(0); + continue; + } + color.push_back(current_image.at(it.pos().x,it.pos().y)); + +#ifdef DRAW_DEBUG + if (config_.renderDebugImage) { + cv::Mat debug_image = current_image.clone(); + cv::namedWindow("Unfiltered points processed by brightness search", CV_WINDOW_NORMAL); + cv::circle(debug_image, it.pos(), 2, cv::Scalar(255)); + cv::imshow("Unfiltered points processed by brightness search", debug_image); + } +#endif + } + + //detect peaks + float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? + int tCounter = 0; + // todo: make this more efficient + for(int k = 0; k < (int)color.size(); k++){ + if(color[k]>config_.brightness_threshold){ + tCounter++; + }else{ + if(tCounter - k != 0 && tCounter > pxlPeakWidth*config_.minLineWidthMul && tCounter < pxlPeakWidth*config_.maxLineWidthMul){ + for (int i = 0; i < tCounter; i++, ++it_debug) { + } + // get points for debug drawing on the way + cv::Point point_first = it_debug.pos(); + for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { + } + cv::Point point_mid = it_debug.pos(); + for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { + } + cv::Point point_end = it_debug.pos(); + +#ifdef DRAW_DEBUG + if (config_.renderDebugImage) { + cv::namedWindow("Line detected by brightness", CV_WINDOW_NORMAL); + cv::Mat debug_image = current_image.clone(); + + cv::line(debug_image, point_first, point_end, cv::Scalar(255)); + cv::imshow("Line detected by brightness", debug_image); + } +#endif + + //we found a valid point + //get the middle + cv::Point2f wMid; + helper_.imageToWorld(point_mid,wMid); + foundPoints.push_back(wMid); + } + tCounter = 0; + } + } + return foundPoints; + } + + void setConfig(const NewRoadDetectionConfig& config) { + config_ = config; + } + +private: + TransformHelper helper_; + NewRoadDetectionConfig config_; + +}; // class ImageOperator + +} // end namespace + +#endif // COMMON_IMAGE_OPERATIONS_H diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index cc2ca6f..a407f50 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -4,20 +4,13 @@ #include #include #include -#include #include #include #include #include #include +#include #include - -//#include -//#include -//#include -//#include -//#include - #include // for multithreading @@ -25,31 +18,6 @@ #include #include -typedef boost::shared_ptr CvImagePtr; - -inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { - CvImagePtr cv_ptr; - try - { - // hardcopies for now, might be possible to process on pointer if fast enough - // todo: make prettier - cv_bridge::CvImagePtr temp_ptr = cv_bridge::toCvCopy(*img_in, ""); - cv_ptr.reset(new cv::Mat(temp_ptr->image) ); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return NULL; - } - - if( !cv_ptr->data) - { - ROS_WARN("Empty image received, skipping!"); - return NULL; - } - return cv_ptr; -} - namespace drive_ros_image_recognition { /** @@ -83,18 +51,6 @@ class NewRoadDetection { // lms::WriteDataChannel debugTranslatedPoints; // lms::ReadDataChannel car; - struct SearchLine{ - cv::Point2f w_start; - cv::Point2f w_end; - cv::Point2i i_start; - cv::Point2i i_end; - - - cv::Point2f w_left; - cv::Point2f w_mid; - cv::Point2f w_right; - }; - std::list lines_; std::mutex mutex; @@ -133,25 +89,10 @@ class NewRoadDetection { dynamic_reconfigure::Server::CallbackType dsrv_cb_; void reconfigureCB(drive_ros_image_recognition::NewRoadDetectionConfig& config, uint32_t level); bool find(); - - ros::ServiceClient worldToImageClient_; - ros::ServiceClient imageToWorldClient_; - - bool imageToWorld(const cv::Point &image_point, cv::Point2f &world_point); - bool worldToImage(const cv::Point2f &world_point, cv::Point &image_point); - - std::vector findBySobel(cv::LineIterator it, - const float lineWidth, - const float iDist, - const float wDist); - - std::vector findByBrightness(cv::LineIterator it, - const float lineWidth, - const float iDist, - const float wDist); - void processSearchLine(const SearchLine &line); void threadFunction(); + TransformHelper transform_helper_; + ImageOperator image_operator_; public: NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 89ec74c..17b7db7 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -7,37 +7,35 @@ #include #include #include +#include #include "drive_ros_image_recognition/RoadLane.h" - -#define DRAW_DEUBG // todo: not very nice to define this here - // todo: a lot of this stuff is used by new_road_detection too, so we should put this in a header / class -typedef boost::shared_ptr CvImagePtr; - -inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& imageIn) { - CvImagePtr cvPtr; - try - { - // hardcopies for now, might be possible to process on pointer if fast enough - // todo: make prettier - cv_bridge::CvImagePtr tmpPtr = cv_bridge::toCvCopy(*imageIn, ""); - cvPtr.reset(new cv::Mat(tmpPtr->image) ); - } - catch(cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return NULL; - } - - if(!cvPtr->data) - { - ROS_WARN("Empty image received, skipping!"); - return NULL; - } - return cvPtr; -} +//typedef boost::shared_ptr CvImagePtr; + +//inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& imageIn) { +// CvImagePtr cvPtr; +// try +// { +// // hardcopies for now, might be possible to process on pointer if fast enough +// // todo: make prettier +// cv_bridge::CvImagePtr tmpPtr = cv_bridge::toCvCopy(*imageIn, ""); +// cvPtr.reset(new cv::Mat(tmpPtr->image) ); +// } +// catch(cv_bridge::Exception& e) +// { +// ROS_ERROR("cv_bridge exception: %s", e.what()); +// return NULL; +// } + +// if(!cvPtr->data) +// { +// ROS_WARN("Empty image received, skipping!"); +// return NULL; +// } +// return cvPtr; +//} namespace drive_ros_image_recognition { namespace detection { diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index f7b25ad..1ccbbae 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -1,4 +1,4 @@ -#ifndef WARP_IMAGE_Hag +#ifndef WARP_IMAGE_H #define WARP_IMAGE_H #include @@ -7,9 +7,7 @@ #include #include #include -#include -#include -#include +#include #include @@ -22,10 +20,6 @@ class WarpContent { bool init(); private: void world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg); - bool worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, - drive_ros_image_recognition::WorldToImage::Response &res); - bool imageToWorld(drive_ros_image_recognition::ImageToWorld::Request &req, - drive_ros_image_recognition::ImageToWorld::Response &res); ros::NodeHandle pnh_; ros::NodeHandle nh_; cv::Mat current_image_; @@ -45,7 +39,6 @@ class WarpContent { ros::ServiceServer imageToWorldServer_; // todo: moving to camera model subscription image_geometry::PinholeCameraModel cam_model_; - tf::TransformListener tf_listener_; }; class WarpImageNodelet : public nodelet::Nodelet { diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index c1e340a..9aab26e 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -1,14 +1,5 @@ #include "drive_ros_image_recognition/new_road_detection.h" -#include -#include #include -//#include -//#include -//#include -//#include -//#include -//#include -//#include namespace drive_ros_image_recognition { @@ -49,9 +40,9 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand conditionLineProcessed_(), current_image_(), current_image_sobel_(), - worldToImageClient_(), - imageToWorldClient_(), - road_buffer_() + road_buffer_(), + transform_helper_(), + image_operator_(transform_helper_) { } @@ -117,9 +108,6 @@ bool NewRoadDetection::init() { filtered_points_pub_ = it_.advertise("filtered_points_out", 10); #endif - imageToWorldClient_ = nh_.serviceClient("/ImageToWorld"); - worldToImageClient_ = nh_.serviceClient("/WorldToImage"); - // todo: we have not decided on an interface for these debug channels yet // debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); // debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); @@ -130,46 +118,6 @@ bool NewRoadDetection::init() { return true; } -bool NewRoadDetection::imageToWorld(const cv::Point &image_point, cv::Point2f &world_point) { - drive_ros_image_recognition::ImageToWorld srv; - geometry_msgs::Point image_point_send; - image_point_send.x = image_point.x; - image_point_send.y = image_point.y; - image_point_send.z = 0.0; - srv.request.image_point = image_point_send; - if (imageToWorldClient_.call(srv)) - { - world_point.x = srv.response.world_point.point.x; - world_point.y = srv.response.world_point.point.y; - } - else - { - ROS_ERROR("Failed to call service ImageToWorld"); - return false; - } -} - -bool NewRoadDetection::worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { - drive_ros_image_recognition::WorldToImage srv; - geometry_msgs::PointStamped world_point_send; - world_point_send.point.x = world_point.x; - world_point_send.point.y = world_point.y; - world_point_send.point.z = 0.0; - // todo: check if this is valid, we assume the trajectory is defined in the axis reference frame - world_point_send.header.frame_id = "tf_front_axis_middle"; - srv.request.world_point = world_point_send; - if (worldToImageClient_.call(srv)) - { - image_point.x = (int)srv.response.image_point.x; - image_point.y = (int)srv.response.image_point.y; - } - else - { - ROS_ERROR("Failed to call service WorldToImage"); - return false; - } -} - void NewRoadDetection::roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_) { // use simple singular buffer road_buffer_ = *road_in_; @@ -231,136 +179,6 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr& img_in) { return; } -std::vector NewRoadDetection::findBySobel(cv::LineIterator it, - const float lineWidth, - const float iDist, - const float wDist) -{ - std::vector foundPoints; - bool foundLowHigh = false; - int pxlCounter = 0; - cv::LineIterator it_backup = it; - - cv::Rect rect(cv::Point(),cv::Point(current_image_->rows, current_image_->cols)); - - for(int i = 0; i < it.count; i++, ++it) - { - // safety check : is the point inside the image - if(!rect.contains(it.pos())){ - ROS_WARN("Received an invalid point outside the image to check for Sobel"); - return foundPoints; - } - - //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang - int sobel = (int)current_image_sobel_->at(it.pos().x,it.pos().y); - if(sobel > sobelThreshold_){ - // found low-high pass (on the left side of the line) - if(!foundLowHigh){ - foundLowHigh = true; - pxlCounter = 0; - } - }else if(sobel < -sobelThreshold_){ //hell-dunkel übergang - //check if we found a lowHigh + highLow border - if(foundLowHigh){ - //check if the points have the right distance - float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? - - ROS_DEBUG_STREAM("crossing found highLow: "< pxlPeakWidth*minLineWidthMul_ && pxlCounter < pxlPeakWidth*maxLineWidthMul_){ - //we found a valid point - //get the middle - // todo: make this more elegant in both sobel and brightness (no std::advance for lineIterator) - for (int iter = 0; iter<(i-pxlCounter/2); iter++, ++it_backup) { - } - cv::Point2f wMid; - imageToWorld(it_backup.pos(),wMid); - foundPoints.push_back(wMid); - ROS_DEBUG("crossing FOUND VALID CROSSING"); - } - } - pxlCounter = 0; - foundLowHigh = false; - //if not, we dont have to do anything - } - - // for calculation of line width - if(foundLowHigh){ - pxlCounter++; - } - } - return foundPoints; -} - -std::vector NewRoadDetection::findByBrightness( cv::LineIterator it, - const float lineWidth, - const float iDist, - const float wDist ) -{ - std::vector foundPoints; - std::vector color; - cv::Rect rect(cv::Point(),cv::Point(current_image_->rows, current_image_->cols)); - cv::LineIterator it_debug = it; - - for(int i = 0; i < it.count; i++, ++it) { - // safety check : is the point inside the image - if(!rect.contains(it.pos())){ - ROS_WARN("Received an invalid point outside the image to check for brightness"); - color.push_back(0); - continue; - } - color.push_back(current_image_->at(it.pos().x,it.pos().y)); - - if (renderDebugImage_) { - cv::namedWindow("Unfiltered points processed by brightness search", CV_WINDOW_NORMAL); - cv::circle(debug_image_, it.pos(), 2, cv::Scalar(255)); - cv::imshow("Unfiltered points processed by brightness search", debug_image_); - } - } - - //detect peaks - float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? - int tCounter = 0; - // todo: make this more efficient - for(int k = 0; k < (int)color.size(); k++){ - if(color[k]>brightness_threshold_){ - tCounter++; - }else{ - if(tCounter - k != 0 && tCounter > pxlPeakWidth*minLineWidthMul_ && tCounter < pxlPeakWidth*maxLineWidthMul_){ - for (int i = 0; i < tCounter; i++, ++it_debug) { - } - // get points for debug drawing on the way - cv::Point point_first = it_debug.pos(); - for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { - } - cv::Point point_mid = it_debug.pos(); - for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { - } - cv::Point point_end = it_debug.pos(); - - if (renderDebugImage_) { - cv::namedWindow("Line detected by brightness", CV_WINDOW_NORMAL); - debug_image_ = current_image_->clone(); - - cv::line(debug_image_, point_first, point_end, cv::Scalar(255)); - cv::imshow("Line detected by brightness", debug_image_); - } - - //we found a valid point - //get the middle - cv::Point2f wMid; - imageToWorld(point_mid,wMid); - foundPoints.push_back(wMid); - } - tCounter = 0; - } - } - return foundPoints; -} - bool NewRoadDetection::find(){ //clear old lines ROS_INFO("Creating new lines"); @@ -388,34 +206,34 @@ bool NewRoadDetection::find(){ for(int it = 0; itx(),it_left->y()); - l.w_end = cv::Point2f(it_right->x(),it_right->y()); - l.w_left = cv::Point2f(it_left->x(),it_left->y()); - l.w_mid = cv::Point2f(it_mid->x(),it_mid->y()); - l.w_right = cv::Point2f(it_right->x(),it_right->y()); + l.wStart = cv::Point2f(it_left->x(),it_left->y()); + l.wEnd = cv::Point2f(it_right->x(),it_right->y()); + l.wLeft = cv::Point2f(it_left->x(),it_left->y()); + l.wMid = cv::Point2f(it_mid->x(),it_mid->y()); + l.wRight = cv::Point2f(it_right->x(),it_right->y()); //check if the part is valid (middle should be always visible) cv::Point l_w_start, l_w_end; - worldToImage(l.w_start, l_w_start); - worldToImage(l.w_end, l_w_end); + transform_helper_.worldToImage(l.wStart, l.iStart); + transform_helper_.worldToImage(l.wEnd, l.iEnd); cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); // if the left side is out of the current view, use middle instead if(!img_rect.contains(l_w_start)){ // try middle lane -> should always be in image - l.w_start = cv::Point2f(it_mid->x(),it_mid->y()); - worldToImage(l.w_start, l_w_start); + l.wStart = cv::Point2f(it_mid->x(),it_mid->y()); + transform_helper_.worldToImage(l.wMid, l.iStart); if(img_rect.contains(l_w_start)){ continue; } // if the right side is out of the current view, use middle instead }else if(!img_rect.contains(l_w_end)){ - l.w_end = cv::Point2f(it_mid->x(),it_mid->y()); + l.wEnd = cv::Point2f(it_mid->x(),it_mid->y()); } //transform them in image-coordinates std::unique_lock lock(mutex); - worldToImage(l.w_start,l.i_start); - worldToImage(l.w_end,l.i_end); + transform_helper_.worldToImage(l.wStart,l.iStart); + transform_helper_.worldToImage(l.wEnd,l.iEnd); lines_.push_back(l); linesToProcess_++; @@ -472,8 +290,8 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { std::vector yv; //calculate the offset - float iDist = cv::norm(l.i_end - l.i_start); - float wDist = cv::norm(l.w_end - l.w_start); + float iDist = cv::norm(l.iEnd - l.iStart); + float wDist = cv::norm(l.wEnd - l.wStart); // add search offset -> do not need this as we do it directly in the image // float pxlPerDist = iDist/wDist*searchOffset_; // cv::Point2f iDiff = (l.i_start-l.i_end)/norm(l.i_start-l.i_end); @@ -483,7 +301,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { // lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! // generate Bresenham search line - cv::LineIterator it(*current_image_,l.i_start,l.i_end); + cv::LineIterator it(*current_image_,l.iStart,l.iEnd); std::vector foundPoints; //find points @@ -494,10 +312,10 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { current_image_sobel_.reset(new cv::Mat(current_image_->rows,current_image_->cols,CV_8UC1)); cv::Sobel( *current_image_, *current_image_sobel_, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter - foundPoints = findBySobel(it,0.02,iDist,wDist); + foundPoints = image_operator_.findBySobel(l,*current_image_,0.02,iDist,wDist,search_direction::x); }else{ // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter - foundPoints = findByBrightness(it,0.02,iDist,wDist); + foundPoints = image_operator_.findByBrightness(l,*current_image_,0.02,iDist,wDist,search_direction::y); } // draw unfiltered image points @@ -510,7 +328,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { cv::circle(found_points_mat, point, 2, cv::Scalar(255)); } // draw search line - cv::line(found_points_mat, l.i_start, l.i_end, cv::Scalar(255)); + cv::line(found_points_mat, l.iStart, l.iEnd, cv::Scalar(255)); #ifdef DRAW_DEBUG cv::imshow("Unfiltered points", found_points_mat); cv::waitKey(1); @@ -571,7 +389,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { cv::Mat filtered_points_mat = current_image_->clone(); cv::Point img_point; for (auto point : validPoints) { - worldToImage(point, img_point); + transform_helper_.worldToImage(point, img_point); cv::circle(filtered_points_mat, img_point, 2, cv::Scalar(255)); } #ifdef DRAW_DEBUG @@ -587,11 +405,11 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { cv::Point2f diff; std::vector distances; for(int i = 0; i < (int)validPoints.size(); i++){ - float distanceToLeft = cv::norm(validPoints[i]-l.w_left); - float distanceToRight= cv::norm(validPoints[i]-l.w_right); - float distanceToMid= cv::norm(validPoints[i]-l.w_mid); + float distanceToLeft = cv::norm(validPoints[i]-l.wLeft); + float distanceToRight= cv::norm(validPoints[i]-l.wRight); + float distanceToMid= cv::norm(validPoints[i]-l.wMid); // normalize distance - diff = (l.w_left - l.w_right)/cv::norm(l.w_left-l.w_right); + diff = (l.wLeft - l.wRight)/cv::norm(l.wLeft-l.wRight); if(distanceToLeft < distanceToMid && distanceToLeft < distanceToRight){ //left point validPoints[i]-=diff*0.4; diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index a81efb0..54896c2 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -1,7 +1,7 @@ #include #include #include -#include "drive_ros_image_recognition/warp_image.h" +//#include "drive_ros_image_recognition/warp_image.h" #include "drive_ros_image_recognition/street_crossing.h" namespace drive_ros_image_recognition { @@ -74,71 +74,71 @@ bool StreetCrossingDetection::findStartline() { #endif } - std::vector StreetCrossingDetection::processSearchLine(SearchLine &sl) { - std::vector foundPoints; - bool foundLowHigh = false; - int pxlCounter = 0; - cv::Point2f iStartPxlPeak; - - cv::LineIterator lineIt(*currentSobelImage, sl.iStart, sl.iEnd); - cv::Rect rect(0, 0, currentSobelImage->cols, currentSobelImage->rows); - // float iDist = cv::norm(sl.iEnd - sl.iStart); - // float wDist = cv::norm(sl.wEnd - sl.wStart); - - for(int i = 0; i < lineIt.count; i++, ++lineIt) { - // safety check : is the point inside the image - if(!rect.contains(lineIt.pos())){ - - ROS_WARN("Received an invalid point outside the image to check for Sobel (%i,%i)", lineIt.pos().x, lineIt.pos().y); - return foundPoints; - } - - // Search for a bright pixel. Searching from bottom to top - int sobel = currentSobelImage->at(lineIt.pos()); - - if(sobel > sobelThreshold) { - // found low-high pass - if(!foundLowHigh) { - foundLowHigh = true; - pxlCounter = 0; - iStartPxlPeak.x = lineIt.pos().x; - iStartPxlPeak.y = lineIt.pos().y; - } - } else if(sobel < -sobelThreshold) { - // found high-low pass - // check if we found a lowHigh + highLow border - if(foundLowHigh){ - //check if the points have the right distance - // TODO to bad, calculate for each road line (how should we use them for searching? - // float pxlPeakWidth = iDist / wDist * lineWidth; // todo: understand this - - //logger.debug("")<<"crossing found highLow: "< (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { - // return the middle of the line we found - // cv::Point2f iMid((iStartPxlPeak.x + lineIt.pos().x) *.5, (iStartPxlPeak.x + lineIt.pos().y) * .5); - - foundPoints.push_back(lineIt.pos()); - // cv::Point2f wMid; - // imageToWorld(iMid, wMid); - // foundPoints.push_back(wMid); - // ROS_DEBUG("Found a stop-line"); - // } - } - pxlCounter = 0; - foundLowHigh = false; - // if not, we dont have to do anything - } - - // for calculation of line width - if(foundLowHigh){ - pxlCounter++; +std::vector StreetCrossingDetection::processSearchLine(SearchLine &sl) { + std::vector foundPoints; + bool foundLowHigh = false; + int pxlCounter = 0; + cv::Point2f iStartPxlPeak; + + cv::LineIterator lineIt(*currentSobelImage, sl.iStart, sl.iEnd); + cv::Rect rect(0, 0, currentSobelImage->cols, currentSobelImage->rows); + // float iDist = cv::norm(sl.iEnd - sl.iStart); + // float wDist = cv::norm(sl.wEnd - sl.wStart); + + for(int i = 0; i < lineIt.count; i++, ++lineIt) { + // safety check : is the point inside the image + if(!rect.contains(lineIt.pos())){ + + ROS_WARN("Received an invalid point outside the image to check for Sobel (%i,%i)", lineIt.pos().x, lineIt.pos().y); + return foundPoints; + } + + // Search for a bright pixel. Searching from bottom to top + int sobel = currentSobelImage->at(lineIt.pos()); + + if(sobel > sobelThreshold) { + // found low-high pass + if(!foundLowHigh) { + foundLowHigh = true; + pxlCounter = 0; + iStartPxlPeak.x = lineIt.pos().x; + iStartPxlPeak.y = lineIt.pos().y; + } + } else if(sobel < -sobelThreshold) { + // found high-low pass + // check if we found a lowHigh + highLow border + if(foundLowHigh){ + //check if the points have the right distance + // TODO to bad, calculate for each road line (how should we use them for searching? + // float pxlPeakWidth = iDist / wDist * lineWidth; // todo: understand this + + //logger.debug("")<<"crossing found highLow: "< (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { + // return the middle of the line we found + // cv::Point2f iMid((iStartPxlPeak.x + lineIt.pos().x) *.5, (iStartPxlPeak.x + lineIt.pos().y) * .5); + + foundPoints.push_back(lineIt.pos()); + // cv::Point2f wMid; + // imageToWorld(iMid, wMid); + // foundPoints.push_back(wMid); + // ROS_DEBUG("Found a stop-line"); + // } + } + pxlCounter = 0; + foundLowHigh = false; + // if not, we dont have to do anything + } + + // for calculation of line width + if(foundLowHigh){ + pxlCounter++; + } } - } - return foundPoints; + return foundPoints; } } //namepsace detection diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 5bd43d6..15cbfdf 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -15,8 +15,7 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): cam_sub_(), worldToImageServer_(), imageToWorldServer_(), - cam_model_(), - tf_listener_() + cam_model_() { img_pub_ = it_.advertise("warped_out", 1); undistort_pub_ = it_.advertise("undistort_out", 1); @@ -26,40 +25,14 @@ WarpContent::~WarpContent() { } bool WarpContent::init() { - // retrieve world2map and map2world homography matrices - std::vector temp_vals; +// if (!getHomographyMatParam(pnh_, world2cam_, "world2cam")) +// return false; - if (!pnh_.getParam("homography_matrix/world2cam", temp_vals)) { - ROS_ERROR("Unable to load world2cam homography matrix from configuration file!"); - return false; - } - if (temp_vals.size() != 9) { - ROS_ERROR("Retreived world2cam homography matrix does not have 9 values!"); - return false; - } - for(unsigned i=0; i < temp_vals.size(); i++) { - // todo: check: according to warp_cpp.h, those two might actually be the other way round - world2cam_.at(i) = temp_vals[i]; - } - ROS_DEBUG_STREAM("World2cam loaded as: "<(i) = temp_vals[i]; - } - ROS_DEBUG_STREAM("Cam2World loaded as: "< temp_vals; if (!pnh_.getParam("camera_matrix/cam_mat", temp_vals)) { ROS_ERROR("Unable to load camera matrix parameters from configuration file!"); return false; @@ -107,8 +80,6 @@ bool WarpContent::init() { // initialize homography transformation subscriber cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); - worldToImageServer_ = nh_.advertiseService("/WorldToImage", &WarpContent::worldToImage, this); - imageToWorldServer_ = nh_.advertiseService("/ImageToWorld", &WarpContent::imageToWorld, this); return true; } @@ -146,97 +117,6 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, // img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } -bool WarpContent::worldToImage(drive_ros_image_recognition::WorldToImage::Request &req, - drive_ros_image_recognition::WorldToImage::Response &res) -{ -// ROS_DEBUG("WorldToImage service: transforming incoming world coordinates to image coordinates"); -// cv::Mat point_world(1,1,CV_64FC2); -// point_world.at(0,0) = req.world_point.x; -// point_world.at(0,1) = req.world_point.y; -// point_world.at(0,2) = req.world_point.z; - - cv::Point3d world_point(req.world_point.point.x, req.world_point.point.y, req.world_point.point.z); - cv::Point2d world_point_cam = cam_model_.project3dToPixel(world_point); - -// cv::Mat point_image(1,1,CV_64FC2); -// cv::perspectiveTransform(point_world, point_image, world2cam_); -// if (point_world.rows != 1 || point_world.cols != 1) { -// ROS_WARN("Point transformed to image dimensions has invalid dimensions"); -// return false; -// } - -// res.image_point.x = point_image.at(0,0); -// res.image_point.y = point_image.at(0,1); -// res.image_point.z = 0.0; - - res.image_point.x = world_point_cam.x; - res.image_point.y = world_point_cam.y; - res.image_point.z = 0.0; - - return true; -} - -bool WarpContent::imageToWorld(drive_ros_image_recognition::ImageToWorld::Request &req, - drive_ros_image_recognition::ImageToWorld::Response &res) -{ -// ROS_DEBUG("WorldToImage service: transforming incoming image coordinates to world coordinates"); -// cv::Mat point_world(1,1,CV_64FC2); -// point_world.at(0,0) = req.image_point.x; -// point_world.at(0,1) = req.image_point.y; - -// cv::Mat point_image(1,1,CV_64FC2); -// cv::perspectiveTransform(point_world, point_image, cam2world_); -// if (point_world.rows != 1 || point_world.cols != 1) { -// ROS_WARN("Point transformed to world dimensions has invalid dimensions"); -// return false; -// } -// res.world_point.x = point_image.at(0,0); -// res.world_point.y = point_image.at(0,1); -// res.world_point.z = point_image.at(0,2); -// // todo: move to sensor_msgs::CameraInfo for transformation, uses tf as base, could use as unified interface -// return true; - - // assuming we are defining our road points in the middle from axis frame id -// geometry_msgs::PointStamped point_out; -// cv::Point2d image_point_; -// cv::Point3d world_point_; -// image_point_.x = req.image_point.x; -// image_point_.y = req.image_point.y; -// world_point_ = cam_model_.projectPixelTo3dRay(image_point_); -// cam_model_. - cv::Mat point_world(1,1,CV_64FC2); - point_world.at(0,0) = req.image_point.x; - point_world.at(0,1) = req.image_point.y; - - cv::Mat point_image(1,1,CV_64FC2); - cv::perspectiveTransform(point_world, point_image, cam2world_); - if (point_world.rows != 1 || point_world.cols != 1) { - ROS_WARN("Point transformed to world dimensions has invalid dimensions"); - return false; - } - - // todo: check if this is valid - cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cv::Point2d(point_image.at(0,0),point_image.at(0,1))); - - geometry_msgs::PointStamped camera_point; - camera_point.header.frame_id = cam_model_.tfFrame(); - camera_point.point.x = cam_ray_point.x; - camera_point.point.y = cam_ray_point.y; - camera_point.point.z = cam_ray_point.z; - - try { - ros::Duration timeout(1.0 / 30); - tf_listener_.waitForTransform(cam_model_.tfFrame(), cam_model_.tfFrame(), - ros::Time::now(), timeout); - tf_listener_.transformPoint("tf_front_axis_middle", camera_point, res.world_point); - } - catch (tf::TransformException& ex) { - ROS_WARN("[ImageToWorld] TF exception, unable to transform:\n%s", ex.what()); - return false; - } - return true; -} - void WarpImageNodelet::onInit() { my_content_.reset(new WarpContent(getNodeHandle(),ros::NodeHandle(getPrivateNodeHandle()))); diff --git a/srv/ImageToWorld.srv b/srv/ImageToWorld.srv deleted file mode 100644 index 6cddacc..0000000 --- a/srv/ImageToWorld.srv +++ /dev/null @@ -1,5 +0,0 @@ -#point in image coordinates -geometry_msgs/Point image_point ---- -#point in world coordinates -geometry_msgs/PointStamped world_point diff --git a/srv/WorldToImage.srv b/srv/WorldToImage.srv deleted file mode 100644 index 53f1342..0000000 --- a/srv/WorldToImage.srv +++ /dev/null @@ -1,5 +0,0 @@ -#point in world coordinates -geometry_msgs/PointStamped world_point ---- -#point in image coordinates -geometry_msgs/Point image_point From cfa9018fd025a854753ef4bc5dee8d9b850716cc Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Wed, 12 Jul 2017 13:56:47 +0200 Subject: [PATCH 29/94] Modified common operation classes for correct initialization, moved namespaces on crossing detection (todo: use common sobel function) --- .../common_image_operations.h | 208 +++++++++--------- .../new_road_detection.h | 3 +- .../street_crossing.h | 12 +- src/new_road_detection.cpp | 26 +-- src/street_crossing.cpp | 7 +- src/street_crossing_node.cpp | 2 +- src/warp_image.cpp | 8 +- 7 files changed, 130 insertions(+), 136 deletions(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index b7e66e7..cad9a32 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -7,6 +7,7 @@ #include #include #include +#include typedef boost::shared_ptr CvImagePtr; @@ -36,16 +37,21 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) namespace drive_ros_image_recognition { +inline void cam_info_sub(const sensor_msgs::CameraInfoConstPtr& incoming_cam_info, image_geometry::PinholeCameraModel& cam_model) { + cam_model.fromCameraInfo(incoming_cam_info); + ROS_INFO("Received camera info callback, processing for use in TransformHelper"); +} + inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const std::string mat_param) { // retrieve world2map and map2world homography matrices std::vector temp_vals; if (!pnh.getParam("homography_matrix/"+mat_param, temp_vals)) { - ROS_ERROR("Unable to load homography matrix from configuration file!"); + ROS_ERROR("Unable to load homography matrix %s from configuration file!", mat_param.c_str()); return false; } if (temp_vals.size() != 9) { - ROS_ERROR("Retreived homography matrix does not have 9 values!"); + ROS_ERROR("Retreived homography matrix %s does not have 9 values", mat_param.c_str()); return false; } for(unsigned i=0; i < temp_vals.size(); i++) { @@ -82,10 +88,28 @@ class TransformHelper { world2cam_(), #endif cam_model_(), - cam2world_() + cam2world_(), + cam_info_sub_() { } + TransformHelper& operator=(TransformHelper&& other) + { + std::swap(cam_model_, other.cam_model_); + std::swap(cam2world_, other.cam2world_); + // tf listener also not swappable, should auto-initialize +#ifdef USE_WORLD2CAM_HOMOGRAPHY + std::swap(world2cam_, other.world2cam_); +#endif + // cannot swap subscriber, need to bind to new object + ros::NodeHandle nh; + cam_info_sub_ = nh.subscribe(other.cam_info_sub_.getTopic(), 1, + boost::bind(&cam_info_sub, _1, + getCameraModelReference()) ); + return *this; + } + + // todo: properly reinitialize subscriber with new binding to this object TransformHelper(const TransformHelper& helper): tf_listener_() { @@ -94,6 +118,10 @@ class TransformHelper { world2cam_ = helper.world2cam_; #endif cam_model_ = helper.cam_model_; + ros::NodeHandle nh; + cam_info_sub_ = nh.subscribe(helper.cam_info_sub_.getTopic(), 1, + boost::bind(&cam_info_sub, _1, + getCameraModelReference()) ); } TransformHelper(const cv::Mat& cam2world, const image_geometry::PinholeCameraModel& cam_model @@ -106,13 +134,29 @@ class TransformHelper { world2cam_(world2cam), #endif cam_model_(cam_model), - tf_listener_() + tf_listener_(), + cam_info_sub_() { } ~TransformHelper() { } + bool init(ros::NodeHandle& pnh) { +#ifdef USE_WORLD2CAM_HOMOGRAPHY + if (!getHomographyMatParam(pnh, world2cam_, "world2cam")) + return false; +#endif + + if (!getHomographyMatParam(pnh, cam2world_, "cam2world")) + return false; + + // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated + cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, + boost::bind(&cam_info_sub, _1, + getCameraModelReference()) ); + } + void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { cam_model_ = cam_model; } @@ -169,6 +213,21 @@ class TransformHelper { return true; } + void setcam2worldMat(const cv::Mat& cam2world) { + cam2world_ = cam2world; + } + + // to subscribe to camerainfo messages + image_geometry::PinholeCameraModel& getCameraModelReference() { + return cam_model_; + } + +#ifdef USE_WORLD2CAM_HOMOGRAPHY + void setworld2camMat(const cv::Mat& world2cam) { + world2cam_ = world2cam; + } +#endif + private: image_geometry::PinholeCameraModel cam_model_; cv::Mat cam2world_; @@ -176,84 +235,31 @@ class TransformHelper { #ifdef USE_WORLD2CAM_HOMOGRAPHY cv::Mat world2cam_; #endif + ros::Subscriber cam_info_sub_; }; // class TransformHelper -//std::vector processSearchLine(SearchLine &sl, const cv::Mat& current_image, const search_direction search_dir) { -// std::vector foundPoints; -// bool foundLowHigh = false; -// int pxlCounter = 0; -// cv::Point2f iStartPxlPeak; - -// cv::LineIterator it(*currentSobelImage, sl.iStart, sl.iEnd); - -// found_points = findBySobel(lineIt, current_image, lineWidth_, iDist, wDist, search_dir); -// cv::Rect rect(0, 0, currentSobelImage->cols, currentSobelImage->rows); -// // float iDist = cv::norm(sl.iEnd - sl.iStart); -// // float wDist = cv::norm(sl.wEnd - sl.wStart); - -// for(int i = 0; i < lineIt.count; i++, ++lineIt) { -// // safety check : is the point inside the image -// if(!rect.contains(lineIt.pos())){ - -// ROS_WARN("Received an invalid point outside the image to check for Sobel (%i,%i)", lineIt.pos().x, lineIt.pos().y); -// return foundPoints; -// } - - - -// Search for a bright pixel. Searching from bottom to top - -// int sobel = currentSobelImage->at(lineIt.pos()); - -// if(sobel > sobelThreshold) { -// // found low-high pass -// if(!foundLowHigh) { -// foundLowHigh = true; -// pxlCounter = 0; -// iStartPxlPeak.x = lineIt.pos().x; -// iStartPxlPeak.y = lineIt.pos().y; -// } -// } else if(sobel < -sobelThreshold) { -// // found high-low pass -// // check if we found a lowHigh + highLow border -// if(foundLowHigh){ -// //check if the points have the right distance -// // TODO to bad, calculate for each road line (how should we use them for searching? -// // float pxlPeakWidth = iDist / wDist * lineWidth; // todo: understand this - -// //logger.debug("")<<"crossing found highLow: "< (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { -// // return the middle of the line we found -// // cv::Point2f iMid((iStartPxlPeak.x + lineIt.pos().x) *.5, (iStartPxlPeak.x + lineIt.pos().y) * .5); - -// foundPoints.push_back(lineIt.pos()); -// // cv::Point2f wMid; -// // imageToWorld(iMid, wMid); -// // foundPoints.push_back(wMid); -// // ROS_DEBUG("Found a stop-line"); -// // } -// } -// pxlCounter = 0; -// foundLowHigh = false; -// // if not, we dont have to do anything -// } - -// // for calculation of line width -// if(foundLowHigh){ -// pxlCounter++; -// } -// } -// return foundPoints; -//} - class ImageOperator { public: + ImageOperator(): + helper_() + { + } + + ImageOperator(ImageOperator& image_operator): + helper_(image_operator.helper_), + config_(image_operator.config_) + { + } + + ImageOperator& operator=(ImageOperator&& other) + { + std::swap(helper_, other.helper_); + std::swap(config_, other.config_); + return *this; + } + ImageOperator(TransformHelper& helper): helper_(helper) { @@ -275,7 +281,7 @@ class ImageOperator { if (search_dir == search_direction::x) cv::Sobel( current_image, current_image_sobel, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); else if(search_dir == search_direction::y) { - // todo: in Simon's code this used a kernel size of 1 which did not really make sense, but check if this is valid anyway + // todo: check direction cv::Sobel( current_image, current_image_sobel, CV_8UC1, -1, 0, 3, 1, 0, cv::BORDER_DEFAULT); } @@ -375,36 +381,38 @@ class ImageOperator { }else{ if(tCounter - k != 0 && tCounter > pxlPeakWidth*config_.minLineWidthMul && tCounter < pxlPeakWidth*config_.maxLineWidthMul){ for (int i = 0; i < tCounter; i++, ++it_debug) { - } - // get points for debug drawing on the way - cv::Point point_first = it_debug.pos(); - for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { - } - cv::Point point_mid = it_debug.pos(); - for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { - } - cv::Point point_end = it_debug.pos(); - #ifdef DRAW_DEBUG - if (config_.renderDebugImage) { - cv::namedWindow("Line detected by brightness", CV_WINDOW_NORMAL); - cv::Mat debug_image = current_image.clone(); + // get points for debug drawing on the way + cv::Point point_first = it_debug.pos(); +#endif + for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { + } + cv::Point point_mid = it_debug.pos(); +#ifdef DRAW_DEBUG + for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { + } + cv::Point point_end = it_debug.pos(); - cv::line(debug_image, point_first, point_end, cv::Scalar(255)); - cv::imshow("Line detected by brightness", debug_image); - } + if (config_.renderDebugImage) { + cv::namedWindow("Line detected by brightness", CV_WINDOW_NORMAL); + cv::Mat debug_image = current_image.clone(); + + cv::line(debug_image, point_first, point_end, cv::Scalar(255)); + cv::imshow("Line detected by brightness", debug_image); + } #endif - //we found a valid point - //get the middle - cv::Point2f wMid; - helper_.imageToWorld(point_mid,wMid); - foundPoints.push_back(wMid); + //we found a valid point + //get the middle + cv::Point2f wMid; + helper_.imageToWorld(point_mid,wMid); + foundPoints.push_back(wMid); + } + tCounter = 0; } - tCounter = 0; } + return foundPoints; } - return foundPoints; } void setConfig(const NewRoadDetectionConfig& config) { diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index a407f50..e121275 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -62,7 +62,6 @@ class NewRoadDetection { bool threadsRunning_; int linesToProcess_; CvImagePtr current_image_; - CvImagePtr current_image_sobel_; #if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) cv::Mat debug_image_; @@ -70,6 +69,8 @@ class NewRoadDetection { image_transport::ImageTransport it_; image_transport::Subscriber img_sub_; + // for TransformHelper + ros::Subscriber cam_info_sub_; // road inputs and outputs ros::Subscriber road_sub_; ros::Publisher line_output_pub_; diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 17b7db7..d5ce0d4 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -38,18 +38,10 @@ //} namespace drive_ros_image_recognition { -namespace detection { class StreetCrossingDetection { private: - struct SearchLine{ - cv::Point2f wStart; - cv::Point2f wEnd; - cv::Point2i iStart; - cv::Point2i iEnd; - }; - // configs int sobelThreshold; float minLineWidthMul; @@ -82,6 +74,9 @@ class StreetCrossingDetection { bool findStartline(); std::vector processSearchLine(SearchLine &sl); + TransformHelper transform_helper_; + ImageOperator image_operator_; + public: StreetCrossingDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); ~StreetCrossingDetection(); @@ -89,6 +84,5 @@ class StreetCrossingDetection { }; } // namepsace drive_ros_image_recognition -} // namespace detection #endif // STREET_CROSSING_H diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 9aab26e..2f32ddb 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -39,10 +39,10 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand conditionNewLine_(), conditionLineProcessed_(), current_image_(), - current_image_sobel_(), road_buffer_(), transform_helper_(), - image_operator_(transform_helper_) + image_operator_(), + cam_info_sub_() { } @@ -108,6 +108,9 @@ bool NewRoadDetection::init() { filtered_points_pub_ = it_.advertise("filtered_points_out", 10); #endif + transform_helper_.init(pnh_); + image_operator_ = ImageOperator(transform_helper_); + // todo: we have not decided on an interface for these debug channels yet // debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); // debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); @@ -306,11 +309,6 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { std::vector foundPoints; //find points if(findPointsBySobel_){ - // directly apply sobel operations on the image -> we only use the grad_x here as we only search horizontally - // we perform order of 1 operations only - // todo: check if CV_8UC1 is valid - current_image_sobel_.reset(new cv::Mat(current_image_->rows,current_image_->cols,CV_8UC1)); - cv::Sobel( *current_image_, *current_image_sobel_, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter foundPoints = image_operator_.findBySobel(l,*current_image_,0.02,iDist,wDist,search_direction::x); }else{ @@ -430,12 +428,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { else weights.push_back(1); } - /* - if(renderDebugImage){ - for(lms::math::vertex2f &v:foundPoints) - debugTranslatedPoints->points().push_back(v); - } - */ + // todo: check how this gets handled and adjust the interface drive_ros_image_recognition::RoadLane lane_out; lane_out.header.stamp = ros::Time::now(); @@ -448,12 +441,6 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { } lane_out.roadStateType = drive_ros_image_recognition::RoadLane::UNKNOWN; line_output_pub_.publish(lane_out); - // lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); - // if(localCourse.isValid()){ - // localCourse->addPoints(foundPoints,weights); - // }else{ - // logger.error("localCourse invalid!"); - // } std::unique_lock lock(mutex); linesToProcess_--; @@ -461,6 +448,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { } void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::NewRoadDetectionConfig& config, uint32_t level){ + image_operator_.setConfig(config); searchOffset_ = config.searchOffset; findPointsBySobel_ = config.findBySobel; renderDebugImage_ = config.renderDebugImage; diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index 54896c2..c7a947f 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -5,7 +5,6 @@ #include "drive_ros_image_recognition/street_crossing.h" namespace drive_ros_image_recognition { -namespace detection { StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh_, const ros::NodeHandle pnh_) : nh(nh_) @@ -15,6 +14,8 @@ StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh_, cons , minLineWidthMul(0.5) , maxLineWidthMul(2.0) , lineWidth(0.4) + , transform_helper_() + , image_operator_() #ifdef DRAW_DEBUG , debugImage(0, 0, CV_8UC1) #endif @@ -33,6 +34,9 @@ bool StreetCrossingDetection::init() { debugImagePublisher = imageTransport.advertise("/street_crossing/debug_image", 10); #endif + transform_helper_.init(pnh); + image_operator_ = ImageOperator(transform_helper_); + return true; } @@ -141,5 +145,4 @@ std::vector StreetCrossingDetection::processSearchLine(SearchLine & return foundPoints; } -} //namepsace detection } //namespace drive_ros_image_recognition diff --git a/src/street_crossing_node.cpp b/src/street_crossing_node.cpp index cf025b0..46f85a2 100644 --- a/src/street_crossing_node.cpp +++ b/src/street_crossing_node.cpp @@ -6,7 +6,7 @@ int main(int argc, char** argv) ros::NodeHandle nh; ros::NodeHandle pnh("~"); - drive_ros_image_recognition::detection::StreetCrossingDetection streetCrossingDetection(nh, pnh); + drive_ros_image_recognition::StreetCrossingDetection streetCrossingDetection(nh, pnh); if (!streetCrossingDetection.init()) { return 1; } diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 15cbfdf..a6e15c0 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -25,11 +25,11 @@ WarpContent::~WarpContent() { } bool WarpContent::init() { -// if (!getHomographyMatParam(pnh_, world2cam_, "world2cam")) -// return false; + if (!getHomographyMatParam(pnh_, world2cam_, "world2cam")) + return false; -// if (!getHomographyMatParam(pnh_, cam2world_, "cam2world")) -// return false; + if (!getHomographyMatParam(pnh_, cam2world_, "cam2world")) + return false; // retreive camera model matrix for undistortion std::vector temp_vals; From 275fe96e403a2103d3ff249fcb3e99b467856417 Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Wed, 12 Jul 2017 18:42:38 +0200 Subject: [PATCH 30/94] Added placeholder code for obstacles in crossing_detection --- src/new_road_detection.cpp | 2 -- src/street_crossing.cpp | 12 +++++++++--- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 2f32ddb..30c1fbf 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -304,8 +304,6 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { // lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! // generate Bresenham search line - cv::LineIterator it(*current_image_,l.iStart,l.iEnd); - std::vector foundPoints; //find points if(findPointsBySobel_){ diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index c7a947f..bc7a0c4 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -52,17 +52,22 @@ bool StreetCrossingDetection::findStartline() { std::vector linePoints; SearchLine mySl; - // Sobel + // Sobel -> will leave this in for now, even though we actually sobel two times like this currentSobelImage.reset(new cv::Mat(currentImage->rows, currentImage->cols, CV_8UC1)); cv::Sobel(*currentImage, *currentSobelImage, -1, 0, 1); // use search-line to find stop-line mySl.iStart = cv::Point2f(currentImage->cols / 2, currentImage->rows * .8); mySl.iEnd = cv::Point2f(currentImage->cols / 2, currentImage->rows * .4); - // imageToWorld(mySl.iStart, mySl.wStart); - // imageToWorld(mySl.iEnd, mySl.wEnd); + linePoints = processSearchLine(mySl); + // after we have a working calibration, we can simply use common helper function to check additional search lines after they have been generated +// transform_helper_.imageToWorld(mySl.iStart, mySl.wStart); +// transform_helper_.imageToWorld(mySl.iEnd, mySl.wEnd); +// float iDist = cv::norm(mySL.iEnd - l.iStart); +// float wDist = cv::norm(l.wEnd - l.wStart); +// foundPoints = image_operator_.findBySobel(mySl,*currentImage,0.02,iDist,wDist,search_direction::y); // Draw / publish points #ifdef DRAW_DEBUG @@ -80,6 +85,7 @@ bool StreetCrossingDetection::findStartline() { std::vector StreetCrossingDetection::processSearchLine(SearchLine &sl) { std::vector foundPoints; + bool foundLowHigh = false; int pxlCounter = 0; cv::Point2f iStartPxlPeak; From cfa90da2a3a4e3eea43c559b4ea39312581e9ac2 Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Wed, 12 Jul 2017 23:36:53 +0200 Subject: [PATCH 31/94] Adjusted image processing to avoid iterations and unify both brightness and sobel-based appraoches, enabled debug image processing by default (does not rely on transformations --- .../common_image_operations.h | 229 +++++++++--------- src/new_road_detection.cpp | 20 +- src/street_crossing.cpp | 28 ++- 3 files changed, 144 insertions(+), 133 deletions(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index cad9a32..2b51ce7 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -67,6 +67,11 @@ enum search_direction { y = true }; +enum search_method { + sobel = false, + brightness = true +}; + struct SearchLine{ cv::Point2f wStart; cv::Point2f wEnd; @@ -265,102 +270,40 @@ class ImageOperator { { } - std::vector findBySobel(const SearchLine &sl, + std::vector returnValidPoints(const SearchLine &sl, const cv::Mat& current_image, const float lineWidth, - const float iDist, - const float wDist, - const search_direction search_dir) + const search_direction search_dir, + const search_method method) { + std::vector imagePoints; + std::vector lineWidths; std::vector foundPoints; - bool foundLowHigh = false; - int pxlCounter = 0; - cv::Mat current_image_sobel(current_image.rows,current_image.cols,CV_8UC1,cv::Scalar(0)); - cv::LineIterator it(current_image_sobel, sl.iStart, sl.iEnd); - cv::LineIterator it_backup = it; - if (search_dir == search_direction::x) - cv::Sobel( current_image, current_image_sobel, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); - else if(search_dir == search_direction::y) { - // todo: check direction - cv::Sobel( current_image, current_image_sobel, CV_8UC1, -1, 0, 3, 1, 0, cv::BORDER_DEFAULT); - } - cv::Rect rect(cv::Point(),cv::Point(current_image.rows, current_image.cols)); - - for(int i = 0; i < it.count; i++, ++it) - { - // safety check : is the point inside the image - if(!rect.contains(it.pos())){ - ROS_WARN("Received an invalid point outside the image to check for Sobel"); - return foundPoints; - } + float iDist = cv::norm(sl.iEnd - sl.iStart); + float wDist = cv::norm(sl.wEnd - sl.wStart); + float pxlPeakWidth = iDist/wDist*lineWidth; + findByLineSearch(sl, current_image, search_dir, method, imagePoints, lineWidths); - //da wir von links nach rechts suchen ist positiver sobel ein dunkel-hell übergang - int sobel = (int)current_image_sobel.at(it.pos().x,it.pos().y); - if(sobel > config_.sobelThreshold){ - // found low-high pass (on the left side of the line) - if(!foundLowHigh){ - foundLowHigh = true; - pxlCounter = 0; - } - }else if(sobel < -config_.sobelThreshold){ //hell-dunkel übergang - //check if we found a lowHigh + highLow border - if(foundLowHigh){ - //check if the points have the right distance - float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? - - ROS_DEBUG_STREAM("crossing found highLow: "< pxlPeakWidth*config_.minLineWidthMul && pxlCounter < pxlPeakWidth*config_.maxLineWidthMul){ - //we found a valid point - //get the middle - // todo: make this more elegant in both sobel and brightness (no std::advance for lineIterator) - for (int iter = 0; iter<(i-pxlCounter/2); iter++, ++it_backup) { - } - cv::Point2f wMid; - helper_.imageToWorld(it_backup.pos(),wMid); - foundPoints.push_back(wMid); - ROS_DEBUG("crossing FOUND VALID CROSSING"); - } - } - pxlCounter = 0; - foundLowHigh = false; - //if not, we dont have to do anything - } + cv::Point2f wMid; - // for calculation of line width - if(foundLowHigh){ - pxlCounter++; + for (int i=0; i pxlPeakWidth*config_.minLineWidthMul && lineWidths[i] < pxlPeakWidth*config_.maxLineWidthMul) { + helper_.imageToWorld(imagePoints[i], wMid); + foundPoints.push_back(wMid); } } return foundPoints; } - std::vector findByBrightness( const SearchLine &sl, - const cv::Mat& current_image, - const float lineWidth, - const float iDist, - const float wDist, - const search_direction search_dir) + void findByLineSearch(const SearchLine& sl, + const cv::Mat& current_image, + const search_direction search_dir, + const search_method search_meth, + std::vector& image_points, + std::vector& line_widths + ) { - std::vector foundPoints; - std::vector color; - cv::Rect rect(cv::Point(),cv::Point(current_image.rows, current_image.cols)); - cv::LineIterator it(current_image, sl.iStart, sl.iEnd); - cv::LineIterator it_debug = it; - - for(int i = 0; i < it.count; i++, ++it) { - // safety check : is the point inside the image - if(!rect.contains(it.pos())){ - ROS_WARN("Received an invalid point outside the image to check for brightness"); - color.push_back(0); - continue; - } - color.push_back(current_image.at(it.pos().x,it.pos().y)); - #ifdef DRAW_DEBUG if (config_.renderDebugImage) { cv::Mat debug_image = current_image.clone(); @@ -369,50 +312,102 @@ class ImageOperator { cv::imshow("Unfiltered points processed by brightness search", debug_image); } #endif + + // step 1: threshold image with mat: + if (search_meth == search_method::brightness) + cv::threshold( current_image, current_image, config_.brightness_threshold, 255, CV_THRESH_BINARY ); + else if (search_meth == search_method::sobel) { + if (search_dir == search_direction::x) + cv::Sobel( current_image, current_image, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); + else if(search_dir == search_direction::y) { + cv::Sobel( current_image, current_image, CV_8UC1, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT); + } } - //detect peaks - float pxlPeakWidth = iDist/wDist*lineWidth; //TODO to bad, calculate for each road line (how should we use them for searching? - int tCounter = 0; - // todo: make this more efficient - for(int k = 0; k < (int)color.size(); k++){ - if(color[k]>config_.brightness_threshold){ - tCounter++; - }else{ - if(tCounter - k != 0 && tCounter > pxlPeakWidth*config_.minLineWidthMul && tCounter < pxlPeakWidth*config_.maxLineWidthMul){ - for (int i = 0; i < tCounter; i++, ++it_debug) { #ifdef DRAW_DEBUG - // get points for debug drawing on the way - cv::Point point_first = it_debug.pos(); + // thresholded image + if (config_.renderDebugImage) { + cv::Mat debug_image = current_image.clone(); + cv::namedWindow("Thresholded image in brightness search", CV_WINDOW_NORMAL); + cv::circle(debug_image, it.pos(), 2, cv::Scalar(255)); + cv::imshow("Thresholded image in brightness search", debug_image); + } #endif - for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { - } - cv::Point point_mid = it_debug.pos(); -#ifdef DRAW_DEBUG - for (int i = 0; i < (k-tCounter)/2; i++, ++it_debug) { - } - cv::Point point_end = it_debug.pos(); + // step 2: line mask to get only relevant pixels in search area + cv::Mat line_mat(current_image.rows, current_image.cols, CV_8UC1, cv::Scalar(0)); + cv::line(line_mat, sl.iStart, sl.iEnd, cv::Scalar(255)); - if (config_.renderDebugImage) { - cv::namedWindow("Line detected by brightness", CV_WINDOW_NORMAL); - cv::Mat debug_image = current_image.clone(); + // step3: binary threshold with line mask + cv::Mat overlay_mat; + cv::bitwise_and(current_image, line_mat, overlay_mat); - cv::line(debug_image, point_first, point_end, cv::Scalar(255)); - cv::imshow("Line detected by brightness", debug_image); - } +#ifdef DRAW_DEBUG + // line thresholded image + if (config_.renderDebugImage) { + cv::namedWindow("Line mask overlay", CV_WINDOW_NORMAL); + cv::imshow("Line mask overlay", overlay_mat); + } #endif - //we found a valid point - //get the middle - cv::Point2f wMid; - helper_.imageToWorld(point_mid,wMid); - foundPoints.push_back(wMid); - } - tCounter = 0; - } + // step4: get line thickness from image + int line_thickness = (int)cv::sum(overlay_mat)[0]; + + // step5: get midpoint of line + cv::Mat locations; + cv::findNonZero(overlay_mat, locations); + double min, max; + if (search_dir == search_direction::x) { + cv::minMaxLoc(locations.col(0), &min, &max); + } else if (search_dir == search_direction::y) { + cv::minMaxLoc(locations.col(1), &min, &max); + } + + if (search_dir == search_direction::x) { + line_widths.push_back(max-min); + image_points.push_back(cv::Point(min+line_thickness*0.5, sl.iStart.y)); + } else if (search_dir == search_direction::y) { + line_widths.push_back(max-min); + image_points.push_back(cv::Point(sl.iStart.x,min+line_thickness*0.5)); + } + return; + } + + // debug to perform line check + void debugPointsImage (const cv::Mat& current_image, + const search_direction search_dir, + const search_method search_meth + ) { + std::vector image_points; + std::vector image_widths; + cv::Mat debug_image; + cv::cvtColor(current_image, debug_image, CV_GRAY2RGB); + + int search_steps = 5; + int pixel_step = 50; + SearchLine lines[search_steps]; + for (int it = 0; it < search_steps*pixel_step; it+=pixel_step) { + if (search_dir == search_direction::x) { + lines[it].iStart = cv::Point(0,it*pixel_step); + lines[it].iEnd = cv::Point(current_image.rows, it*pixel_step); + } else if (search_dir == search_direction::y) { + lines[it].iStart = cv::Point(it*pixel_step, 0); + lines[it].iEnd = cv::Point(it*pixel_step, current_image.cols); } - return foundPoints; + cv::line(debug_image, lines[it].iStart, lines[it].iEnd, cv::Scalar(0,255,0)); + findByLineSearch(lines[it], + current_image, + search_dir, + search_meth, + image_points, + image_widths + ); + for (auto point: image_points) + cv::circle(debug_image,point,2,cv::Scalar(255,0,0),2); } + cv::namedWindow("Debug lineCheck", CV_WINDOW_NORMAL); + cv::imshow("Debug lineCheck", debug_image); + cv::waitKey(0); + return; } void setConfig(const NewRoadDetectionConfig& config) { diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 30c1fbf..481883f 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -143,7 +143,10 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr& img_in) { //if we have a road(?), try to find the line if(road_buffer_.points.size() > 1){ - find(); //TODO use bool from find + //find(); //TODO use bool from find + search_direction search_dir = search_direction::x; + search_method search_meth = search_method::sobel; + image_operator_.debugPointsImage(*current_image_, search_dir, search_meth); } else { ROS_WARN("Road buffer has no points stored"); } @@ -289,12 +292,13 @@ void NewRoadDetection::threadFunction() { } void NewRoadDetection::processSearchLine(const SearchLine &l) { - std::vector xv; - std::vector yv; +// std::vector xv; +// std::vector yv; //calculate the offset - float iDist = cv::norm(l.iEnd - l.iStart); - float wDist = cv::norm(l.wEnd - l.wStart); + // done in search function +// float iDist = cv::norm(l.iEnd - l.iStart); +// float wDist = cv::norm(l.wEnd - l.wStart); // add search offset -> do not need this as we do it directly in the image // float pxlPerDist = iDist/wDist*searchOffset_; // cv::Point2f iDiff = (l.i_start-l.i_end)/norm(l.i_start-l.i_end); @@ -303,15 +307,15 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { // //get all points in between // lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! - // generate Bresenham search line std::vector foundPoints; + //find points if(findPointsBySobel_){ // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter - foundPoints = image_operator_.findBySobel(l,*current_image_,0.02,iDist,wDist,search_direction::x); + foundPoints = image_operator_.returnValidPoints(l, *current_image_, 0.02, search_direction::x, search_method::sobel); }else{ // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter - foundPoints = image_operator_.findByBrightness(l,*current_image_,0.02,iDist,wDist,search_direction::y); + foundPoints = image_operator_.returnValidPoints(l, *current_image_, 0.02, search_direction::x, search_method::brightness); } // draw unfiltered image points diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index bc7a0c4..aa55c18 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -60,14 +60,26 @@ bool StreetCrossingDetection::findStartline() { mySl.iStart = cv::Point2f(currentImage->cols / 2, currentImage->rows * .8); mySl.iEnd = cv::Point2f(currentImage->cols / 2, currentImage->rows * .4); - linePoints = processSearchLine(mySl); - - // after we have a working calibration, we can simply use common helper function to check additional search lines after they have been generated -// transform_helper_.imageToWorld(mySl.iStart, mySl.wStart); -// transform_helper_.imageToWorld(mySl.iEnd, mySl.wEnd); -// float iDist = cv::norm(mySL.iEnd - l.iStart); -// float wDist = cv::norm(l.wEnd - l.wStart); -// foundPoints = image_operator_.findBySobel(mySl,*currentImage,0.02,iDist,wDist,search_direction::y); +// linePoints = processSearchLine(mySl); + + // test a single line + cv::Mat debug_image; + cv::cvtColor(*currentImage, debug_image, CV_GRAY2RGB); + search_direction search_dir = search_direction::y; + search_method search_meth = search_method::sobel; + std::vector image_points; + std::vector line_widths; + image_operator_.findByLineSearch(mySl,*currentImage,search_dir,search_meth,image_points,line_widths); + cv::line(debug_image, mySl.iStart, mySl.iEnd, cv::Scalar(0,255,0)); + for (auto point: image_points) { + cv::circle(debug_image,point,2,cv::Scalar(0,0,255),2); + } + cv::namedWindow("Crossing Search debug",CV_WINDOW_NORMAL); + cv::imshow("Crossing Search debug",debug_image); + cv::waitKey(0); + + // test array of lines + image_operator_.debugPointsImage(*currentImage, search_dir, search_meth); // Draw / publish points #ifdef DRAW_DEBUG From b81d1676b2e1a6dbd2ca660936261404e96b3f8d Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Thu, 13 Jul 2017 01:31:20 +0200 Subject: [PATCH 32/94] Remove old camera model yamls from launch files, remove invalid parameter loading from WarpImage and NewRoadDetection, fix bugs in NewRoadDetection --- CMakeLists.txt | 2 + .../common_image_operations.h | 6 ++- launch/new_road_detection.launch | 2 +- launch/new_road_detection_debug.launch | 13 +++++ launch/warp_image.launch | 1 - launch/warp_image_nodelet.launch | 2 - src/new_road_detection_node.cpp | 10 ++-- src/warp_image.cpp | 49 +------------------ 8 files changed, 26 insertions(+), 59 deletions(-) create mode 100644 launch/new_road_detection_debug.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d8d23e..295927c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,8 @@ SET(CMAKE_BUILD_TYPE Debug) #add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images #add_definitions(-DPUBLISH_DEBUG) +# enable to use World2Cam homography +#add_definitions(-DUSE_WORLD2CAM_HOMOGRAPHY) add_definitions(-std=c++11) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 2b51ce7..1b45dbb 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -44,7 +44,7 @@ inline void cam_info_sub(const sensor_msgs::CameraInfoConstPtr& incoming_cam_inf inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const std::string mat_param) { // retrieve world2map and map2world homography matrices - std::vector temp_vals; + std::vector temp_vals(9); if (!pnh.getParam("homography_matrix/"+mat_param, temp_vals)) { ROS_ERROR("Unable to load homography matrix %s from configuration file!", mat_param.c_str()); @@ -55,7 +55,7 @@ inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const return false; } for(unsigned i=0; i < temp_vals.size(); i++) { - // todo: check: according to warp_cpp.h, those two might actually be the other way round + mat = cv::Mat::zeros(3,3,CV_64F); mat.at(i) = temp_vals[i]; } ROS_DEBUG_STREAM("Paramater matrix loaded as: "< - + diff --git a/launch/new_road_detection_debug.launch b/launch/new_road_detection_debug.launch new file mode 100644 index 0000000..00af9cd --- /dev/null +++ b/launch/new_road_detection_debug.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + diff --git a/launch/warp_image.launch b/launch/warp_image.launch index 48f9e90..26f0777 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -5,7 +5,6 @@ - diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch index acde656..18cbfb3 100644 --- a/launch/warp_image_nodelet.launch +++ b/launch/warp_image_nodelet.launch @@ -11,7 +11,6 @@ - @@ -23,7 +22,6 @@ - diff --git a/src/new_road_detection_node.cpp b/src/new_road_detection_node.cpp index c647277..c05f2ab 100644 --- a/src/new_road_detection_node.cpp +++ b/src/new_road_detection_node.cpp @@ -6,6 +6,11 @@ int main(int argc, char** argv) ros::NodeHandle nh; ros::NodeHandle pnh("~"); +#ifndef NDEBUG + // give GDB time to attach + ros::Duration(2.0).sleep(); +#endif + drive_ros_image_recognition::NewRoadDetection new_road_detection(nh,pnh); if (!new_road_detection.init()) { return 1; @@ -14,11 +19,6 @@ int main(int argc, char** argv) ROS_INFO("New road detection node succesfully initialized"); } -#ifndef NDEBUG - // give GDB time to attach - ros::Duration(1.0).sleep(); -#endif - while (ros::ok()) { ros::spin(); } diff --git a/src/warp_image.cpp b/src/warp_image.cpp index a6e15c0..7773989 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -31,54 +31,7 @@ bool WarpContent::init() { if (!getHomographyMatParam(pnh_, cam2world_, "cam2world")) return false; - // retreive camera model matrix for undistortion - std::vector temp_vals; - if (!pnh_.getParam("camera_matrix/cam_mat", temp_vals)) { - ROS_ERROR("Unable to load camera matrix parameters from configuration file!"); - return false; - } - if (temp_vals.size() != 4) { - ROS_ERROR("Retreived camera matrix does not have 4 values!"); - return false; - } - // (row,column) indexing - // Fx: - cam_mat_.at(0,0) = temp_vals[0]; - // Fy: - cam_mat_.at(1,1) = temp_vals[1]; - // Cx: - cam_mat_.at(0,2) = temp_vals[2]; - // Cy: - cam_mat_.at(1,2) = temp_vals[3]; - cam_mat_.at(2,2) = 1.0; - ROS_DEBUG_STREAM("Camera matrix loaded as: "<(0,0) = temp_vals[0]; - // K2 - dist_coeffs_.at(1,0) = temp_vals[1]; - // K3 - dist_coeffs_.at(4,0) = temp_vals[2]; - // K4 - dist_coeffs_.at(5,0) = temp_vals[3]; - // K5 - dist_coeffs_.at(6,0) = temp_vals[4]; - // K6 - dist_coeffs_.at(7,0) = temp_vals[5]; - ROS_DEBUG_STREAM("Distortion coefficients loaded as: "< Date: Fri, 14 Jul 2017 01:03:12 +0200 Subject: [PATCH 33/94] Add synchronized subscriptions for road and incoming image, working debug imaging, todo: add thresholding operations --- CMakeLists.txt | 3 +- .../common_image_operations.h | 129 +++++++++++------- .../new_road_detection.h | 25 ++-- .../street_crossing.h | 2 +- launch/new_road_detection.launch | 2 +- launch/new_road_detection_debug.launch | 2 +- package.xml | 2 + scripts/publish_road.sh | 2 +- src/new_road_detection.cpp | 54 ++++---- 9 files changed, 125 insertions(+), 96 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 295927c..0c49053 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(drive_ros_image_recognition) SET(CMAKE_BUILD_TYPE Debug) # enable to compile with OpenCV window displays -#add_definitions(-DDRAW_DEBUG) +add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images #add_definitions(-DPUBLISH_DEBUG) # enable to use World2Cam homography @@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation nodelet tf + message_filters ) #include Boost for lane line processing diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 1b45dbb..0a6219e 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -17,8 +17,9 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { // hardcopies for now, might be possible to process on pointer if fast enough // todo: make prettier - cv_bridge::CvImagePtr temp_ptr = cv_bridge::toCvCopy(*img_in, ""); - cv_ptr.reset(new cv::Mat(temp_ptr->image) ); +// cv_bridge::CvImagePtr temp_ptr = cv_bridge::toCvCopy(img_in, ""); + cv_ptr.reset(new cv::Mat(cv_bridge::toCvCopy(img_in, "")->image.clone())); +// cv_ptr.reset(new cv::Mat(temp_ptr->image.clone())); } catch (cv_bridge::Exception& e) { @@ -39,7 +40,6 @@ namespace drive_ros_image_recognition { inline void cam_info_sub(const sensor_msgs::CameraInfoConstPtr& incoming_cam_info, image_geometry::PinholeCameraModel& cam_model) { cam_model.fromCameraInfo(incoming_cam_info); - ROS_INFO("Received camera info callback, processing for use in TransformHelper"); } inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const std::string mat_param) { @@ -107,10 +107,11 @@ class TransformHelper { std::swap(world2cam_, other.world2cam_); #endif // cannot swap subscriber, need to bind to new object - ros::NodeHandle nh; - cam_info_sub_ = nh.subscribe(other.cam_info_sub_.getTopic(), 1, + ros::NodeHandle pnh("~"); + cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); + other.cam_info_sub_.shutdown(); return *this; } @@ -123,8 +124,8 @@ class TransformHelper { world2cam_ = helper.world2cam_; #endif cam_model_ = helper.cam_model_; - ros::NodeHandle nh; - cam_info_sub_ = nh.subscribe(helper.cam_info_sub_.getTopic(), 1, + ros::NodeHandle pnh("~"); + cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); } @@ -304,33 +305,27 @@ class ImageOperator { std::vector& line_widths ) { -#ifdef DRAW_DEBUG - if (config_.renderDebugImage) { - cv::Mat debug_image = current_image.clone(); - cv::namedWindow("Unfiltered points processed by brightness search", CV_WINDOW_NORMAL); - cv::circle(debug_image, it.pos(), 2, cv::Scalar(255)); - cv::imshow("Unfiltered points processed by brightness search", debug_image); - } -#endif // step 1: threshold image with mat: if (search_meth == search_method::brightness) cv::threshold( current_image, current_image, config_.brightness_threshold, 255, CV_THRESH_BINARY ); else if (search_meth == search_method::sobel) { if (search_dir == search_direction::x) - cv::Sobel( current_image, current_image, CV_8UC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); + cv::Sobel( current_image, current_image, CV_8UC1, 1, 0, 1, 1, 0, cv::BORDER_DEFAULT ); else if(search_dir == search_direction::y) { - cv::Sobel( current_image, current_image, CV_8UC1, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT); + cv::Sobel( current_image, current_image, CV_8UC1, 0, 1, 1, 1, 0, cv::BORDER_DEFAULT); } } #ifdef DRAW_DEBUG - // thresholded image - if (config_.renderDebugImage) { - cv::Mat debug_image = current_image.clone(); - cv::namedWindow("Thresholded image in brightness search", CV_WINDOW_NORMAL); - cv::circle(debug_image, it.pos(), 2, cv::Scalar(255)); - cv::imshow("Thresholded image in brightness search", debug_image); + // display initial image + cv::Mat debug_image = current_image.clone(); + if (search_meth == search_method::brightness) { + cv::namedWindow("Thresholded image in brightness search", CV_WINDOW_NORMAL); + cv::imshow("Thresholded image in brightness search", debug_image); + } else if (search_meth == search_method::sobel) { + cv::namedWindow("Sobel image in line search", CV_WINDOW_NORMAL); + cv::imshow("Sobel image in line search", debug_image); } #endif // step 2: line mask to get only relevant pixels in search area @@ -340,37 +335,67 @@ class ImageOperator { // step3: binary threshold with line mask cv::Mat overlay_mat; cv::bitwise_and(current_image, line_mat, overlay_mat); +// cv::Mat upper_thresh; +// cv::threshold(overlay_mat(cv::Rect(cv::Point(sl.iStart.x,sl.iStart.y),cv::Point(sl.iEnd.x,sl.iEnd.y+1))), +// upper_thresh, config_.sobelThreshold, 255, CV_THRESH_BINARY); +// cv::Mat lower_thresh; +// cv::threshold(overlay_mat(cv::Rect(cv::Point(sl.iStart.x,sl.iStart.y),cv::Point(sl.iEnd.x,sl.iEnd.y+1))), +// lower_thresh, -config_.sobelThreshold, 255, CV_THRESH_BINARY); +// lower_thresh = upper_thresh + lower_thresh; #ifdef DRAW_DEBUG // line thresholded image - if (config_.renderDebugImage) { - cv::namedWindow("Line mask overlay", CV_WINDOW_NORMAL); - cv::imshow("Line mask overlay", overlay_mat); - } + cv::namedWindow("Line mask overlay", CV_WINDOW_NORMAL); + cv::imshow("Line mask overlay", overlay_mat); +// cv::threshold(overlay_mat, +// upper_thresh, config_.sobelThreshold, 255, CV_THRESH_BINARY); +// cv::imshow("Line mask overlay", overlay_mat(cv::Rect(cv::Point(sl.iStart.x,sl.iStart.y),cv::Point(sl.iEnd.x,sl.iEnd.y+1)))); +// cv::imshow("Line mask overlay",upper_thresh); #endif - // step4: get line thickness from image - // todo: handle cases with multiple line segments - int line_thickness = (int)cv::sum(overlay_mat)[0]; - + // step4: get connected components (can be multiple) and calculate midpoints + std::vector > line_contours; + std::vector hierarchy; + // todo: use some kind of connected contours based on threshold value + cv::findContours(overlay_mat, line_contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); - // step5: get midpoint of line - cv::Mat locations; - cv::findNonZero(overlay_mat, locations); - double min, max; - if (search_dir == search_direction::x) { - cv::minMaxLoc(locations.col(0), &min, &max); - } else if (search_dir == search_direction::y) { - cv::minMaxLoc(locations.col(1), &min, &max); - } +#ifdef DRAW_DEBUG + // line contours + cv::namedWindow("Line contours", CV_WINDOW_NORMAL); + cv::Mat line_contour_image = current_image.clone(); + for (int i=0; i 6) { + line_widths.push_back(bounds.br().x-bounds.tl().x); + image_points.push_back(bounds.br()+bounds.br()-bounds.tl()); + } } +// cv::Mat locations; +// cv::findNonZero(overlay_mat, locations); +// if (!locations.empty()) { +// double min, max; +// if (search_dir == search_direction::x) { +// cv::minMaxLoc(locations.col(0), &min, &max); +// } else if (search_dir == search_direction::y) { +// cv::minMaxLoc(locations.col(1), &min, &max); +// } + +// if (search_dir == search_direction::x) { +// line_widths.push_back(max-min); +// image_points.push_back(cv::Point(min+line_thickness*0.5, sl.iStart.y)); +// } else if (search_dir == search_direction::y) { +// line_widths.push_back(max-min); +// image_points.push_back(cv::Point(sl.iStart.x,min+line_thickness*0.5)); +// } +// } return; } @@ -384,18 +409,18 @@ class ImageOperator { cv::Mat debug_image; cv::cvtColor(current_image, debug_image, CV_GRAY2RGB); - int search_steps = 5; + int search_steps = 6; int pixel_step = 50; SearchLine lines[search_steps]; - for (int it = 0; it < search_steps*pixel_step; it+=pixel_step) { + for (int it = 3; it < search_steps; ++it) { if (search_dir == search_direction::x) { lines[it].iStart = cv::Point(0,it*pixel_step); - lines[it].iEnd = cv::Point(current_image.rows, it*pixel_step); + lines[it].iEnd = cv::Point(current_image.cols, it*pixel_step); } else if (search_dir == search_direction::y) { lines[it].iStart = cv::Point(it*pixel_step, 0); - lines[it].iEnd = cv::Point(it*pixel_step, current_image.cols); + lines[it].iEnd = cv::Point(it*pixel_step, current_image.rows); } - cv::line(debug_image, lines[it].iStart, lines[it].iEnd, cv::Scalar(0,255,0)); + cv::line(debug_image, lines[it].iStart, lines[it].iEnd, cv::Scalar(255,255,255)); findByLineSearch(lines[it], current_image, search_dir, @@ -408,7 +433,7 @@ class ImageOperator { } cv::namedWindow("Debug lineCheck", CV_WINDOW_NORMAL); cv::imshow("Debug lineCheck", debug_image); - cv::waitKey(0); + cv::waitKey(1); return; } diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index e121275..b4109f5 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -12,6 +12,10 @@ #include #include #include +#include +#include +#include +#include // for multithreading #include @@ -62,17 +66,13 @@ class NewRoadDetection { bool threadsRunning_; int linesToProcess_; CvImagePtr current_image_; - -#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) - cv::Mat debug_image_; -#endif - image_transport::ImageTransport it_; - image_transport::Subscriber img_sub_; - // for TransformHelper - ros::Subscriber cam_info_sub_; // road inputs and outputs - ros::Subscriber road_sub_; + image_transport::Subscriber img_sub_debug_; +// std::unique_ptr img_sub_; +// std::unique_ptr > road_sub_; +// typedef message_filters::sync_policies::ApproximateTime SyncImageToRoad; +// std::unique_ptr > sync_; ros::Publisher line_output_pub_; #ifdef PUBLISH_DEBUG image_transport::Publisher debug_img_pub_; @@ -81,13 +81,12 @@ class NewRoadDetection { #endif ros::NodeHandle nh_; ros::NodeHandle pnh_; - drive_ros_image_recognition::RoadLane road_buffer_; - - void roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_); - void imageCallback(const sensor_msgs::ImageConstPtr& img_in); + std::vector road_points_buffer_; dynamic_reconfigure::Server dsrv_server_; dynamic_reconfigure::Server::CallbackType dsrv_cb_; + void debugImageCallback(const sensor_msgs::ImageConstPtr& img_in); + void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in); void reconfigureCB(drive_ros_image_recognition::NewRoadDetectionConfig& config, uint32_t level); bool find(); void processSearchLine(const SearchLine &line); diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index d5ce0d4..77845f6 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -65,7 +65,7 @@ class StreetCrossingDetection { void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); -#ifdef DRAW_DEUBG +#ifdef DRAW_DEBUG image_transport::Publisher debugImagePublisher; cv::Mat debugImage; #endif diff --git a/launch/new_road_detection.launch b/launch/new_road_detection.launch index cd99c69..1193a37 100644 --- a/launch/new_road_detection.launch +++ b/launch/new_road_detection.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/new_road_detection_debug.launch b/launch/new_road_detection_debug.launch index 00af9cd..666a678 100644 --- a/launch/new_road_detection_debug.launch +++ b/launch/new_road_detection_debug.launch @@ -1,6 +1,6 @@ - + diff --git a/package.xml b/package.xml index 8effea1..8647986 100644 --- a/package.xml +++ b/package.xml @@ -50,6 +50,7 @@ nodelet geometry_msgs tf + message_filters message_generation @@ -63,6 +64,7 @@ nodelet geometry_msgs tf + message_filters message_runtime diff --git a/scripts/publish_road.sh b/scripts/publish_road.sh index 6b9c03e..32602b9 100755 --- a/scripts/publish_road.sh +++ b/scripts/publish_road.sh @@ -12,4 +12,4 @@ points: - x: 1.0 y: 0.0 z: 0.0 -roadStateType: 0" -r 10 +roadStateType: 0" -r 100 diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 481883f..1a496a9 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -6,7 +6,9 @@ namespace drive_ros_image_recognition { NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh): nh_(nh), pnh_(pnh), - img_sub_(), +// img_sub_(), +// road_sub_(), +// sync_(), searchOffset_(0.15), distanceBetweenSearchlines_(0.1), minLineWidthMul_(0.5), @@ -19,10 +21,6 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand useWeights_(false), renderDebugImage_(false), numThreads_(0), - road_sub_(), -#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) - debug_image_(0,0,CV_16UC1), -#endif line_output_pub_(), it_(pnh), #ifdef PUBLISH_DEBUG @@ -39,10 +37,10 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand conditionNewLine_(), conditionLineProcessed_(), current_image_(), - road_buffer_(), + road_points_buffer_(), transform_helper_(), image_operator_(), - cam_info_sub_() + img_sub_debug_() { } @@ -98,11 +96,18 @@ bool NewRoadDetection::init() { dsrv_server_.setCallback(dsrv_cb_); - img_sub_ = it_.subscribe("img_in", 10, &NewRoadDetection::imageCallback, this); - road_sub_ = pnh_.subscribe("road_in", 100, &NewRoadDetection::roadCallback, this); + // to synchronize incoming images and the road, we use message filters +// img_sub_.reset(new image_transport::SubscriberFilter(it_,"img_in", 1000)); +// road_sub_.reset(new message_filters::Subscriber(pnh_,"road_in", 1000)); +// sync_.reset(new message_filters::Synchronizer(SyncImageToRoad(100), *img_sub_, *road_sub_)); +// sync_->setAgePenalty(1.0); +// sync_->registerCallback(boost::bind(&NewRoadDetection::syncCallback, this, _1, _2)); + + img_sub_debug_ = it_.subscribe("img_in", 1000, &NewRoadDetection::debugImageCallback, this); + line_output_pub_ = pnh_.advertise("line_out",10); -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG debug_img_pub_ = it_.advertise("debug_image_out", 10); detected_points_pub_ = it_.advertise("detected_points_out", 10); filtered_points_pub_ = it_.advertise("filtered_points_out", 10); @@ -121,18 +126,18 @@ bool NewRoadDetection::init() { return true; } -void NewRoadDetection::roadCallback(const drive_ros_image_recognition::RoadLaneConstPtr& road_in_) { - // use simple singular buffer - road_buffer_ = *road_in_; +void NewRoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) { + current_image_ = convertImageMessage(img_in); + // crop image to 410 width + cv::Rect roi(cv::Point(current_image_->cols/2-(410/2),0),cv::Point(current_image_->cols/2+(410/2),current_image_->rows)); + search_direction search_dir = search_direction::x; + search_method search_meth = search_method::sobel; + image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth); } -void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr& img_in) { +void NewRoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in){ current_image_ = convertImageMessage(img_in); -#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) - if (renderDebugImage_) { - debug_image_ = current_image_->clone(); - } -#endif + road_points_buffer_ = road_in->points; // todo: adjust this time value // if (img_in->header.stamp.toSec() - road_buffer_.header.stamp.toSec() > 0.1) { @@ -142,15 +147,13 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr& img_in) { // } //if we have a road(?), try to find the line - if(road_buffer_.points.size() > 1){ - //find(); //TODO use bool from find - search_direction search_dir = search_direction::x; - search_method search_meth = search_method::sobel; - image_operator_.debugPointsImage(*current_image_, search_dir, search_meth); + if(road_in->points.size() > 1){ + find(); //TODO use bool from find } else { ROS_WARN("Road buffer has no points stored"); } + // float dx = 0; // float dy = 0; // float dPhi = 0; @@ -182,7 +185,6 @@ void NewRoadDetection::imageCallback(const sensor_msgs::ImageConstPtr& img_in) { // ROS_ERROR("LocalCourse invalid, aborting the callback!"); // return; // } - return; } bool NewRoadDetection::find(){ @@ -193,7 +195,7 @@ bool NewRoadDetection::find(){ //create left/mid/right lane linestring mid, left, right; - for (auto point : road_buffer_.points) { + for (auto point : road_points_buffer_) { boost::geometry::append(mid,point_xy(point.x, point.y)); } // simplify mid (previously done with distance-based algorithm From 427c70b12e44b9d15bfd6aef7d819270155c34ca Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sat, 15 Jul 2017 21:55:14 +0200 Subject: [PATCH 34/94] Fixed sobel and brightness line checks, moved code to unfied header, added nodelet to crossing detection, adjusted launch files, renamed some components in crossing detection --- CMakeLists.txt | 26 +- ...NewRoadDetection.cfg => LineDetection.cfg} | 9 +- .../common_image_operations.h | 740 +++++++++--------- .../new_road_detection.h | 18 +- .../street_crossing.h | 65 +- launch/new_road_detection.launch | 2 + launch/new_road_detection_debug.launch | 3 +- launch/new_road_detection_nodelet.launch | 6 +- launch/street_crossing.launch | 2 + launch/street_crossing_nodelet.launch | 26 + nodelets.xml | 9 + src/new_road_detection.cpp | 95 +-- src/street_crossing.cpp | 176 ++--- 13 files changed, 554 insertions(+), 623 deletions(-) rename cfg/{NewRoadDetection.cfg => LineDetection.cfg} (71%) create mode 100644 launch/street_crossing_nodelet.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c49053..5fef719 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,7 @@ generate_messages( ) generate_dynamic_reconfigure_options( - cfg/NewRoadDetection.cfg + cfg/LineDetection.cfg ) catkin_package( @@ -148,7 +148,29 @@ install(TARGETS warp_image_nodelet ) ################################################################################ -# Street crossing node (LMS port) +# Street crossing detection node (LMS port) +################################################################################ + +add_library(street_crossing_nodelet + src/street_crossing.cpp + ) + +add_dependencies(street_crossing_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(street_crossing_nodelet + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + ) + +install(TARGETS street_crossing_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +################################################################################ +# Street crossing detection node (LMS port) ################################################################################ add_executable(street_crossing_node diff --git a/cfg/NewRoadDetection.cfg b/cfg/LineDetection.cfg similarity index 71% rename from cfg/NewRoadDetection.cfg rename to cfg/LineDetection.cfg index 86abf2c..9011b9c 100755 --- a/cfg/NewRoadDetection.cfg +++ b/cfg/LineDetection.cfg @@ -10,13 +10,14 @@ gen.add("distanceBetweenSearchlines", double_t, 0, "Distance between search line gen.add("minLineWidthMul", double_t, 0, "minLineWidthMul", 0.5, 0.0, 1.0) gen.add("maxLineWidthMul", double_t, 0, "maxLineWidthMul", 2, 0.0, 5.0) gen.add("findBySobel", bool_t, 0, "Use sobel", True) -gen.add("brightness_threshold", int_t, 0, "Brightness threshold", 50, 0, 200) +gen.add("brightness_threshold", int_t, 0, "Brightness threshold", 150, 0, 255) +#gen.add("blob_area_threshold", int_t, 0, "Blob area threshold", 5000, 0, 10000) +#gen.add("blob_aspect_threshold", double_t, 0, "Blob aspect ratio threshold", 2.0, 0.0, 5.0) gen.add("sobelThreshold", int_t, 0, "Sobel threshold", 50, 0, 200) gen.add("laneWidthOffsetInMeter", double_t, 0, "Lane width offset (m)", 0.1, 0.0, 2.0) gen.add("translateEnvironment", bool_t, 0, "Translate environment", False) gen.add("useWeights", bool_t, 0, "Use weights", False) -gen.add("renderDebugImage", bool_t, 0, "Render debug image", True) -gen.add("threads", int_t, 0, "Number of threads", 0, 0, 8) +gen.add("lineWidth", double_t, 0, "Line width (world coordinates)", 0.4, 0.0, 1.0) -exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "NewRoadDetection")) +exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "LineDetection")) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 0a6219e..af0414e 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -6,444 +6,472 @@ #include #include #include -#include +#include #include typedef boost::shared_ptr CvImagePtr; inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) { - CvImagePtr cv_ptr; - try - { - // hardcopies for now, might be possible to process on pointer if fast enough - // todo: make prettier -// cv_bridge::CvImagePtr temp_ptr = cv_bridge::toCvCopy(img_in, ""); - cv_ptr.reset(new cv::Mat(cv_bridge::toCvCopy(img_in, "")->image.clone())); -// cv_ptr.reset(new cv::Mat(temp_ptr->image.clone())); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return NULL; - } - - if( !cv_ptr->data) - { - ROS_WARN("Empty image received, skipping!"); - return NULL; - } - return cv_ptr; + CvImagePtr cv_ptr; + try + { + // hardcopies for now, might be possible to process on pointer if fast enough + // todo: make prettier + // cv_bridge::CvImagePtr temp_ptr = cv_bridge::toCvCopy(img_in, ""); + cv_ptr.reset(new cv::Mat(cv_bridge::toCvCopy(img_in, "")->image.clone())); + // cv_ptr.reset(new cv::Mat(temp_ptr->image.clone())); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return NULL; + } + + if( !cv_ptr->data) + { + ROS_WARN("Empty image received, skipping!"); + return NULL; + } + return cv_ptr; } namespace drive_ros_image_recognition { inline void cam_info_sub(const sensor_msgs::CameraInfoConstPtr& incoming_cam_info, image_geometry::PinholeCameraModel& cam_model) { - cam_model.fromCameraInfo(incoming_cam_info); + cam_model.fromCameraInfo(incoming_cam_info); } inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const std::string mat_param) { - // retrieve world2map and map2world homography matrices - std::vector temp_vals(9); - - if (!pnh.getParam("homography_matrix/"+mat_param, temp_vals)) { - ROS_ERROR("Unable to load homography matrix %s from configuration file!", mat_param.c_str()); - return false; - } - if (temp_vals.size() != 9) { - ROS_ERROR("Retreived homography matrix %s does not have 9 values", mat_param.c_str()); - return false; - } - for(unsigned i=0; i < temp_vals.size(); i++) { - mat = cv::Mat::zeros(3,3,CV_64F); - mat.at(i) = temp_vals[i]; - } - ROS_DEBUG_STREAM("Paramater matrix loaded as: "< temp_vals(9); + + if (!pnh.getParam("homography_matrix/"+mat_param, temp_vals)) { + ROS_ERROR("Unable to load homography matrix %s from configuration file!", mat_param.c_str()); + return false; + } + if (temp_vals.size() != 9) { + ROS_ERROR("Retreived homography matrix %s does not have 9 values", mat_param.c_str()); + return false; + } + for(unsigned i=0; i < temp_vals.size(); i++) { + mat = cv::Mat::zeros(3,3,CV_64F); + mat.at(i) = temp_vals[i]; + } + ROS_DEBUG_STREAM("Paramater matrix loaded as: "<("/camera1/camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); - other.cam_info_sub_.shutdown(); - return *this; - } - - // todo: properly reinitialize subscriber with new binding to this object - TransformHelper(const TransformHelper& helper): + // cannot swap subscriber, need to bind to new object + ros::NodeHandle pnh("~"); + cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, + boost::bind(&cam_info_sub, _1, + getCameraModelReference()) ); + other.cam_info_sub_.shutdown(); + return *this; + } + + // todo: properly reinitialize subscriber with new binding to this object + TransformHelper(const TransformHelper& helper): tf_listener_() - { - cam2world_ = helper.cam2world_; + { + cam2world_ = helper.cam2world_; #ifdef USE_WORLD2CAM_HOMOGRAPHY - world2cam_ = helper.world2cam_; + world2cam_ = helper.world2cam_; #endif - cam_model_ = helper.cam_model_; - ros::NodeHandle pnh("~"); - cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); - } - - TransformHelper(const cv::Mat& cam2world, const image_geometry::PinholeCameraModel& cam_model + cam_model_ = helper.cam_model_; + ros::NodeHandle pnh("~"); + cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, + boost::bind(&cam_info_sub, _1, + getCameraModelReference()) ); + } + + TransformHelper(const cv::Mat& cam2world, const image_geometry::PinholeCameraModel& cam_model #ifdef USE_WORLD2CAM_HOMOGRAPHY - const cv::Mat& world2cam + const cv::Mat& world2cam #endif - ): - cam2world_(cam2world), - #ifdef USE_WORLD2CAM_HOMOGRAPHY - world2cam_(world2cam), - #endif - cam_model_(cam_model), - tf_listener_(), - cam_info_sub_() - { - } - - ~TransformHelper() { - } - - bool init(ros::NodeHandle& pnh) { + ): + cam2world_(cam2world), + #ifdef USE_WORLD2CAM_HOMOGRAPHY + world2cam_(world2cam), + #endif + cam_model_(cam_model), + tf_listener_(), + cam_info_sub_() + { + } + + ~TransformHelper() { + } + + bool init(ros::NodeHandle& pnh) { #ifdef USE_WORLD2CAM_HOMOGRAPHY - if (!getHomographyMatParam(pnh, world2cam_, "world2cam")) - return false; + if (!getHomographyMatParam(pnh, world2cam_, "world2cam")) + return false; #endif - if (!getHomographyMatParam(pnh, cam2world_, "cam2world")) - return false; + if (!getHomographyMatParam(pnh, cam2world_, "cam2world")) + return false; - // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated - cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); - } + // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated + cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, + boost::bind(&cam_info_sub, _1, + getCameraModelReference()) ); + } - void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { - cam_model_ = cam_model; - } + void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { + cam_model_ = cam_model; + } - bool worldToImage(const cv::Point3f& world_point, - cv::Point& image_point) { + bool worldToImage(const cv::Point3f& world_point, + cv::Point& image_point) { #ifdef USE_WORLD2CAM_HOMOGRAPHY - // using homography instead of camera model - cv::perspectiveTransform(world_point, image_point, world2cam_); - if (point_world.rows != 1 || point_world.cols != 1) { - ROS_WARN("Point transformed to image dimensions has invalid dimensions"); - return false; - } + // using homography instead of camera model + cv::perspectiveTransform(world_point, image_point, world2cam_); + if (point_world.rows != 1 || point_world.cols != 1) { + ROS_WARN("Point transformed to image dimensions has invalid dimensions"); + return false; + } #else - image_point = cam_model_.project3dToPixel(world_point); + image_point = cam_model_.project3dToPixel(world_point); #endif - return true; - } - - bool worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { - return worldToImage(cv::Point3f(world_point.x, world_point.y, 0.0f), image_point); + return true; + } + + bool worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { + return worldToImage(cv::Point3f(world_point.x, world_point.y, 0.0f), image_point); + } + + bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point) { + cv::Mat mat_point_image(1,1,CV_64FC2); + mat_point_image.at(0,0) = image_point.x; + mat_point_image.at(0,1) = image_point.y; + cv::Mat mat_point_world(1,1,CV_64FC2); + cv::perspectiveTransform(mat_point_image, mat_point_world, cam2world_); + cv::Point2f cam_point(mat_point_world.at(0,0),mat_point_world.at(0,1)); + + // todo: check if it is valid to assume the z depth if we used homography before + cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); + + geometry_msgs::PointStamped camera_point; + camera_point.header.frame_id = cam_model_.tfFrame(); + camera_point.point.x = cam_ray_point.x; + camera_point.point.y = cam_ray_point.y; + camera_point.point.z = cam_ray_point.z; + geometry_msgs::PointStamped geom_world_point; + + try { + ros::Duration timeout(1.0 / 30); + tf_listener_.waitForTransform(cam_model_.tfFrame(), "tf_front_axis_middle", + ros::Time::now(), timeout); + tf_listener_.transformPoint("tf_front_axis_middle", camera_point, geom_world_point); } - - bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point) { - cv::Mat mat_point_image(1,1,CV_64FC2); - mat_point_image.at(0,0) = image_point.x; - mat_point_image.at(0,1) = image_point.y; - cv::Mat mat_point_world(1,1,CV_64FC2); - cv::perspectiveTransform(mat_point_image, mat_point_world, cam2world_); - cv::Point2f cam_point(mat_point_world.at(0,0),mat_point_world.at(0,1)); - - // todo: check if it is valid to assume the z depth if we used homography before - cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); - - geometry_msgs::PointStamped camera_point; - camera_point.header.frame_id = cam_model_.tfFrame(); - camera_point.point.x = cam_ray_point.x; - camera_point.point.y = cam_ray_point.y; - camera_point.point.z = cam_ray_point.z; - geometry_msgs::PointStamped geom_world_point; - - try { - ros::Duration timeout(1.0 / 30); - tf_listener_.waitForTransform(cam_model_.tfFrame(), "tf_front_axis_middle", - ros::Time::now(), timeout); - tf_listener_.transformPoint("tf_front_axis_middle", camera_point, geom_world_point); - } - catch (tf::TransformException& ex) { - ROS_WARN("[ImageToWorld] TF exception, unable to transform:\n%s", ex.what()); - return false; - } - world_point.x = geom_world_point.point.x; - world_point.y = geom_world_point.point.y; - return true; + catch (tf::TransformException& ex) { + ROS_WARN("[ImageToWorld] TF exception, unable to transform:\n%s", ex.what()); + return false; } + world_point.x = geom_world_point.point.x; + world_point.y = geom_world_point.point.y; + return true; + } - void setcam2worldMat(const cv::Mat& cam2world) { - cam2world_ = cam2world; - } + void setcam2worldMat(const cv::Mat& cam2world) { + cam2world_ = cam2world; + } - // to subscribe to camerainfo messages - image_geometry::PinholeCameraModel& getCameraModelReference() { - return cam_model_; - } + // to subscribe to camerainfo messages + image_geometry::PinholeCameraModel& getCameraModelReference() { + return cam_model_; + } #ifdef USE_WORLD2CAM_HOMOGRAPHY - void setworld2camMat(const cv::Mat& world2cam) { - world2cam_ = world2cam; - } + void setworld2camMat(const cv::Mat& world2cam) { + world2cam_ = world2cam; + } #endif private: - image_geometry::PinholeCameraModel cam_model_; - cv::Mat cam2world_; - tf::TransformListener tf_listener_; + image_geometry::PinholeCameraModel cam_model_; + cv::Mat cam2world_; + tf::TransformListener tf_listener_; #ifdef USE_WORLD2CAM_HOMOGRAPHY - cv::Mat world2cam_; + cv::Mat world2cam_; #endif - ros::Subscriber cam_info_sub_; + ros::Subscriber cam_info_sub_; }; // class TransformHelper class ImageOperator { public: - ImageOperator(): - helper_() - { - } + ImageOperator(): + helper_(), + config_() + { + config_.sobelThreshold = 50; + config_.brightness_threshold = 150; + } + + ImageOperator(ImageOperator& image_operator): + helper_(image_operator.helper_), + config_(image_operator.config_) + { + } + + ImageOperator& operator=(ImageOperator&& other) + { + std::swap(helper_, other.helper_); + std::swap(config_, other.config_); + return *this; + } + + ImageOperator(TransformHelper& helper): + helper_(helper), + config_() + { + config_.sobelThreshold = 50; + config_.brightness_threshold = 150; + } + + std::vector returnValidPoints(const SearchLine &sl, + const cv::Mat& current_image, + const float lineWidth, + const search_direction search_dir, + const search_method method) + { + std::vector imagePoints; + std::vector lineWidths; + std::vector foundPoints; + + float iDist = cv::norm(sl.iEnd - sl.iStart); + float wDist = cv::norm(sl.wEnd - sl.wStart); + float pxlPeakWidth = iDist/wDist*lineWidth; + findByLineSearch(sl, current_image, search_dir, method, imagePoints, lineWidths); - ImageOperator(ImageOperator& image_operator): - helper_(image_operator.helper_), - config_(image_operator.config_) - { - } - - ImageOperator& operator=(ImageOperator&& other) - { - std::swap(helper_, other.helper_); - std::swap(config_, other.config_); - return *this; - } + cv::Point2f wMid; - ImageOperator(TransformHelper& helper): - helper_(helper) - { + for (int i=0; i pxlPeakWidth*config_.minLineWidthMul && lineWidths[i] < pxlPeakWidth*config_.maxLineWidthMul) { + helper_.imageToWorld(imagePoints[i], wMid); + foundPoints.push_back(wMid); + } } - - std::vector returnValidPoints(const SearchLine &sl, - const cv::Mat& current_image, - const float lineWidth, - const search_direction search_dir, - const search_method method) - { - std::vector imagePoints; - std::vector lineWidths; - std::vector foundPoints; - - float iDist = cv::norm(sl.iEnd - sl.iStart); - float wDist = cv::norm(sl.wEnd - sl.wStart); - float pxlPeakWidth = iDist/wDist*lineWidth; - findByLineSearch(sl, current_image, search_dir, method, imagePoints, lineWidths); - - cv::Point2f wMid; - - for (int i=0; i pxlPeakWidth*config_.minLineWidthMul && lineWidths[i] < pxlPeakWidth*config_.maxLineWidthMul) { - helper_.imageToWorld(imagePoints[i], wMid); - foundPoints.push_back(wMid); - } - } - return foundPoints; + return foundPoints; + } + + void findByLineSearch(const SearchLine& sl, + const cv::Mat& current_image, + const search_direction search_dir, + const search_method search_meth, + std::vector& image_points, + std::vector& line_widths + ) + { + cv::Mat processed_image; + // step 0: cut image to line + processed_image = current_image(cv::Rect(sl.iStart, + cv::Point(sl.iEnd.x+(search_dir==search_direction::y), + sl.iEnd.y+(search_dir==search_direction::x)) + )); + + // step 1: threshold image with mat: + if (search_meth == search_method::brightness) { + cv::threshold(processed_image, processed_image, config_.brightness_threshold, 255, CV_THRESH_BINARY); + // testing removal of bright white blobs +// std::vector > image_contours; +// cv::findContours(processed_image, image_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); +// processed_image = cv::Mat::zeros(current_image.rows, current_image.cols, CV_8UC1); +// cv::Mat debug_exclude_image = cv::Mat::zeros(current_image.rows, current_image.cols, CV_8UC1); +// for (int cnt = 0; cnt < image_contours.size(); ++cnt) { +// if (cv::contourArea(image_contours[cnt]) < config_.blob_area_threshold) +// cv::drawContours(processed_image, image_contours, cnt, cv::Scalar(255), CV_FILLED); +// else if (cv::contourArea(image_contours[cnt])/cv::boundingRect(image_contours[cnt]).area()& image_points, - std::vector& line_widths - ) - { - - // step 1: threshold image with mat: - if (search_meth == search_method::brightness) - cv::threshold( current_image, current_image, config_.brightness_threshold, 255, CV_THRESH_BINARY ); - else if (search_meth == search_method::sobel) { - if (search_dir == search_direction::x) - cv::Sobel( current_image, current_image, CV_8UC1, 1, 0, 1, 1, 0, cv::BORDER_DEFAULT ); - else if(search_dir == search_direction::y) { - cv::Sobel( current_image, current_image, CV_8UC1, 0, 1, 1, 1, 0, cv::BORDER_DEFAULT); - } - } - -#ifdef DRAW_DEBUG - // display initial image - cv::Mat debug_image = current_image.clone(); - if (search_meth == search_method::brightness) { - cv::namedWindow("Thresholded image in brightness search", CV_WINDOW_NORMAL); - cv::imshow("Thresholded image in brightness search", debug_image); - } else if (search_meth == search_method::sobel) { - cv::namedWindow("Sobel image in line search", CV_WINDOW_NORMAL); - cv::imshow("Sobel image in line search", debug_image); - } -#endif - // step 2: line mask to get only relevant pixels in search area - cv::Mat line_mat(current_image.rows, current_image.cols, CV_8UC1, cv::Scalar(0)); - cv::line(line_mat, sl.iStart, sl.iEnd, cv::Scalar(255)); - - // step3: binary threshold with line mask - cv::Mat overlay_mat; - cv::bitwise_and(current_image, line_mat, overlay_mat); -// cv::Mat upper_thresh; -// cv::threshold(overlay_mat(cv::Rect(cv::Point(sl.iStart.x,sl.iStart.y),cv::Point(sl.iEnd.x,sl.iEnd.y+1))), -// upper_thresh, config_.sobelThreshold, 255, CV_THRESH_BINARY); -// cv::Mat lower_thresh; -// cv::threshold(overlay_mat(cv::Rect(cv::Point(sl.iStart.x,sl.iStart.y),cv::Point(sl.iEnd.x,sl.iEnd.y+1))), -// lower_thresh, -config_.sobelThreshold, 255, CV_THRESH_BINARY); -// lower_thresh = upper_thresh + lower_thresh; - #ifdef DRAW_DEBUG - // line thresholded image - cv::namedWindow("Line mask overlay", CV_WINDOW_NORMAL); - cv::imshow("Line mask overlay", overlay_mat); -// cv::threshold(overlay_mat, -// upper_thresh, config_.sobelThreshold, 255, CV_THRESH_BINARY); -// cv::imshow("Line mask overlay", overlay_mat(cv::Rect(cv::Point(sl.iStart.x,sl.iStart.y),cv::Point(sl.iEnd.x,sl.iEnd.y+1)))); -// cv::imshow("Line mask overlay",upper_thresh); + // display initial image + cv::Mat debug_image = processed_image.clone(); + if (search_meth == search_method::brightness) { + cv::namedWindow("Thresholded image in brightness search", CV_WINDOW_NORMAL); + cv::imshow("Thresholded image in brightness search", debug_image); + } else if (search_meth == search_method::sobel) { + cv::namedWindow("Sobel image in line search", CV_WINDOW_NORMAL); + cv::imshow("Sobel image in line search", debug_image); + } #endif - // step4: get connected components (can be multiple) and calculate midpoints - std::vector > line_contours; - std::vector hierarchy; - // todo: use some kind of connected contours based on threshold value - cv::findContours(overlay_mat, line_contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); + if (search_meth == search_method::brightness) { + // step3: get connected components (can be multiple) and calculate midpoints + std::vector > line_contours; + cv::findContours(processed_image, line_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); #ifdef DRAW_DEBUG - // line contours - cv::namedWindow("Line contours", CV_WINDOW_NORMAL); - cv::Mat line_contour_image = current_image.clone(); - for (int i=0; i 6) { - line_widths.push_back(bounds.br().x-bounds.tl().x); - image_points.push_back(bounds.br()+bounds.br()-bounds.tl()); + // step5: get midpoints of components + cv::Rect bounds; + cv::Point temp_point; + for (auto contour: line_contours) { + bounds = cv::boundingRect( cv::Mat(contour) ); + // todo: add parameter if we will use contours + if (bounds.area() > 6 && bounds.area() < 20) { + line_widths.push_back(bounds.br().x-bounds.tl().x); + temp_point = (bounds.tl()+bounds.br())/2; + if (search_dir == search_direction::x) { + temp_point.y = sl.iEnd.y; + } else { + temp_point.x = sl.iEnd.x; } + image_points.push_back(temp_point); } -// cv::Mat locations; -// cv::findNonZero(overlay_mat, locations); -// if (!locations.empty()) { -// double min, max; -// if (search_dir == search_direction::x) { -// cv::minMaxLoc(locations.col(0), &min, &max); -// } else if (search_dir == search_direction::y) { -// cv::minMaxLoc(locations.col(1), &min, &max); -// } - -// if (search_dir == search_direction::x) { -// line_widths.push_back(max-min); -// image_points.push_back(cv::Point(min+line_thickness*0.5, sl.iStart.y)); -// } else if (search_dir == search_direction::y) { -// line_widths.push_back(max-min); -// image_points.push_back(cv::Point(sl.iStart.x,min+line_thickness*0.5)); -// } -// } - return; - } - - // debug to perform line check - void debugPointsImage (const cv::Mat& current_image, - const search_direction search_dir, - const search_method search_meth - ) { - std::vector image_points; - std::vector image_widths; - cv::Mat debug_image; - cv::cvtColor(current_image, debug_image, CV_GRAY2RGB); - - int search_steps = 6; - int pixel_step = 50; - SearchLine lines[search_steps]; - for (int it = 3; it < search_steps; ++it) { - if (search_dir == search_direction::x) { - lines[it].iStart = cv::Point(0,it*pixel_step); - lines[it].iEnd = cv::Point(current_image.cols, it*pixel_step); - } else if (search_dir == search_direction::y) { - lines[it].iStart = cv::Point(it*pixel_step, 0); - lines[it].iEnd = cv::Point(it*pixel_step, current_image.rows); - } - cv::line(debug_image, lines[it].iStart, lines[it].iEnd, cv::Scalar(255,255,255)); - findByLineSearch(lines[it], - current_image, - search_dir, - search_meth, - image_points, - image_widths - ); - for (auto point: image_points) - cv::circle(debug_image,point,2,cv::Scalar(255,0,0),2); + } + } else if (search_meth == search_method::sobel) { + bool lowhigh_found = false; + int first = 0; + for(int i=0; i(i) > config_.sobelThreshold) { + lowhigh_found = true; + first = i; + } else if (lowhigh_found && processed_image.at(i) < -config_.sobelThreshold) { + line_widths.push_back(i-first); + if (search_dir == search_direction::x) { + image_points.push_back(cv::Point(sl.iStart.x+first+(i-first)/2,sl.iStart.y)); + } + else if (search_dir == search_direction::y) { + image_points.push_back(cv::Point(sl.iStart.x,sl.iStart.y+first+(i-first)/2)); + } + lowhigh_found = false; } - cv::namedWindow("Debug lineCheck", CV_WINDOW_NORMAL); - cv::imshow("Debug lineCheck", debug_image); - cv::waitKey(1); - return; + } } - void setConfig(const NewRoadDetectionConfig& config) { - config_ = config; + return; + } + + // debug to perform line check + void debugPointsImage (const cv::Mat& current_image, + const search_direction search_dir, + const search_method search_meth + ) { + std::vector image_points; + std::vector image_widths; + cv::Mat debug_image; + cv::cvtColor(current_image, debug_image, CV_GRAY2RGB); + + int search_steps = 6; + int pixel_step = 50; + SearchLine lines[search_steps]; + for (int it = 3; it < search_steps; ++it) { + if (search_dir == search_direction::x) { + lines[it].iStart = cv::Point(0,it*pixel_step); + lines[it].iEnd = cv::Point(current_image.cols, it*pixel_step); + } else if (search_dir == search_direction::y) { + lines[it].iStart = cv::Point(it*pixel_step, 0); + lines[it].iEnd = cv::Point(it*pixel_step, current_image.rows); + } + cv::line(debug_image, lines[it].iStart, lines[it].iEnd, cv::Scalar(255,255,255)); + findByLineSearch(lines[it], + current_image, + search_dir, + search_meth, + image_points, + image_widths + ); + for (auto point: image_points) { + cv::circle(debug_image,point,2,cv::Scalar(0,255,0),2); + } + } +#ifdef DRAW_DEBUG + if (search_meth == search_method::sobel) { + cv::namedWindow("Debug lineCheck sobel", CV_WINDOW_NORMAL); + cv::imshow("Debug lineCheck sobel", debug_image); + cv::waitKey(1); + } + if (search_meth == search_method::brightness) { + cv::namedWindow("Debug lineCheck brightness", CV_WINDOW_NORMAL); + cv::imshow("Debug lineCheck brightness", debug_image); + cv::waitKey(1); } +#endif + return; + } + + void setConfig(const LineDetectionConfig& config) { + config_ = config; + } private: - TransformHelper helper_; - NewRoadDetectionConfig config_; + TransformHelper helper_; + LineDetectionConfig config_; }; // class ImageOperator diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/new_road_detection.h index b4109f5..2bcc588 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/new_road_detection.h @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include #include #include @@ -32,7 +32,6 @@ class NewRoadDetection { float searchOffset_; float distanceBetweenSearchlines_; bool findPointsBySobel_; - bool renderDebugImage_; float minLineWidthMul_; float maxLineWidthMul_; int brightness_threshold_; @@ -40,7 +39,6 @@ class NewRoadDetection { bool translateEnvironment_; bool useWeights_; int sobelThreshold_; - int numThreads_; // 0 means single threaded // ported in the WarpImage class, ports the entire image already // lms::imaging::Homography homo; @@ -57,13 +55,6 @@ class NewRoadDetection { std::list lines_; - std::mutex mutex; - std::mutex debugAllPointsMutex; - std::mutex debugValidPointsMutex; - std::vector threads_; - std::condition_variable conditionNewLine_; - std::condition_variable conditionLineProcessed_; - bool threadsRunning_; int linesToProcess_; CvImagePtr current_image_; image_transport::ImageTransport it_; @@ -83,14 +74,13 @@ class NewRoadDetection { ros::NodeHandle pnh_; std::vector road_points_buffer_; - dynamic_reconfigure::Server dsrv_server_; - dynamic_reconfigure::Server::CallbackType dsrv_cb_; + dynamic_reconfigure::Server dsrv_server_; + dynamic_reconfigure::Server::CallbackType dsrv_cb_; void debugImageCallback(const sensor_msgs::ImageConstPtr& img_in); void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in); - void reconfigureCB(drive_ros_image_recognition::NewRoadDetectionConfig& config, uint32_t level); + void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); bool find(); void processSearchLine(const SearchLine &line); - void threadFunction(); TransformHelper transform_helper_; ImageOperator image_operator_; diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 77845f6..804fc19 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -7,60 +7,36 @@ #include #include #include +#include #include +#include +#include #include "drive_ros_image_recognition/RoadLane.h" -// todo: a lot of this stuff is used by new_road_detection too, so we should put this in a header / class -//typedef boost::shared_ptr CvImagePtr; - -//inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& imageIn) { -// CvImagePtr cvPtr; -// try -// { -// // hardcopies for now, might be possible to process on pointer if fast enough -// // todo: make prettier -// cv_bridge::CvImagePtr tmpPtr = cv_bridge::toCvCopy(*imageIn, ""); -// cvPtr.reset(new cv::Mat(tmpPtr->image) ); -// } -// catch(cv_bridge::Exception& e) -// { -// ROS_ERROR("cv_bridge exception: %s", e.what()); -// return NULL; -// } - -// if(!cvPtr->data) -// { -// ROS_WARN("Empty image received, skipping!"); -// return NULL; -// } -// return cvPtr; -//} - namespace drive_ros_image_recognition { class StreetCrossingDetection { private: // configs - int sobelThreshold; - float minLineWidthMul; - float maxLineWidthMul; - float lineWidth; // todo: should be in config + int sobelThreshold_; + float minLineWidthMul_; + float maxLineWidthMul_; + float lineWidth_; // variables - std::vector searchLines; - CvImagePtr currentImage; - CvImagePtr currentSobelImage; + std::vector searchLines_; + CvImagePtr currentImage_; // communication - image_transport::ImageTransport imageTransport; - image_transport::Subscriber imageSubscriber; + image_transport::ImageTransport imageTransport_; + image_transport::Subscriber imageSubscriber_; -// ros::Publisher linePublisher; + ros::Publisher line_output_pub_; - ros::NodeHandle nh; - ros::NodeHandle pnh; + ros::NodeHandle nh_; + ros::NodeHandle pnh_; void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); @@ -72,17 +48,28 @@ class StreetCrossingDetection { // methods bool findStartline(); - std::vector processSearchLine(SearchLine &sl); TransformHelper transform_helper_; ImageOperator image_operator_; + void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); + dynamic_reconfigure::Server dsrv_server_; + dynamic_reconfigure::Server::CallbackType dsrv_cb_; + public: StreetCrossingDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); ~StreetCrossingDetection(); bool init(); }; +class StreetCrossingDetectionNodelet: public nodelet::Nodelet { +public: + virtual void onInit(); +private: + std::unique_ptr street_crossing_detection_; +}; + + } // namepsace drive_ros_image_recognition #endif // STREET_CROSSING_H diff --git a/launch/new_road_detection.launch b/launch/new_road_detection.launch index 1193a37..ce14e0c 100644 --- a/launch/new_road_detection.launch +++ b/launch/new_road_detection.launch @@ -8,5 +8,7 @@ + + diff --git a/launch/new_road_detection_debug.launch b/launch/new_road_detection_debug.launch index 666a678..55d5003 100644 --- a/launch/new_road_detection_debug.launch +++ b/launch/new_road_detection_debug.launch @@ -3,11 +3,12 @@ - + + diff --git a/launch/new_road_detection_nodelet.launch b/launch/new_road_detection_nodelet.launch index 0efca20..fa43bf7 100644 --- a/launch/new_road_detection_nodelet.launch +++ b/launch/new_road_detection_nodelet.launch @@ -2,7 +2,7 @@ - + @@ -14,6 +14,8 @@ + + @@ -25,6 +27,8 @@ + + diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch index 4767eca..358c506 100644 --- a/launch/street_crossing.launch +++ b/launch/street_crossing.launch @@ -3,5 +3,7 @@ + + diff --git a/launch/street_crossing_nodelet.launch b/launch/street_crossing_nodelet.launch new file mode 100644 index 0000000..b11db2a --- /dev/null +++ b/launch/street_crossing_nodelet.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodelets.xml b/nodelets.xml index 9450a56..2d252dc 100644 --- a/nodelets.xml +++ b/nodelets.xml @@ -15,3 +15,12 @@ + + + + + + Street crossing detection nodelet. + + + diff --git a/src/new_road_detection.cpp b/src/new_road_detection.cpp index 1a496a9..51c9911 100644 --- a/src/new_road_detection.cpp +++ b/src/new_road_detection.cpp @@ -19,8 +19,6 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand laneWidthOffsetInMeter_(0.1), translateEnvironment_(false), useWeights_(false), - renderDebugImage_(false), - numThreads_(0), line_output_pub_(), it_(pnh), #ifdef PUBLISH_DEBUG @@ -31,11 +29,7 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand dsrv_server_(), dsrv_cb_(boost::bind(&NewRoadDetection::reconfigureCB, this, _1, _2)), lines_(), - threadsRunning_(false), linesToProcess_(0), - threads_(), - conditionNewLine_(), - conditionLineProcessed_(), current_image_(), road_points_buffer_(), transform_helper_(), @@ -86,13 +80,8 @@ bool NewRoadDetection::init() { ROS_WARN_STREAM("Unable to load 'useWeights' parameter, using default: "<("DEBUG_ALL_POINTS"); @@ -132,20 +119,15 @@ void NewRoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_ cv::Rect roi(cv::Point(current_image_->cols/2-(410/2),0),cv::Point(current_image_->cols/2+(410/2),current_image_->rows)); search_direction search_dir = search_direction::x; search_method search_meth = search_method::sobel; + search_method search_meth_brightness = search_method::brightness; image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth); + image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth_brightness); } void NewRoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in){ current_image_ = convertImageMessage(img_in); road_points_buffer_ = road_in->points; - // todo: adjust this time value -// if (img_in->header.stamp.toSec() - road_buffer_.header.stamp.toSec() > 0.1) { -// // todo: move to warn level, too spammy right now -// ROS_DEBUG("Outdated road callback in buffer, skipping incoming image"); -// return; -// } - //if we have a road(?), try to find the line if(road_in->points.size() > 1){ find(); //TODO use bool from find @@ -238,61 +220,16 @@ bool NewRoadDetection::find(){ l.wEnd = cv::Point2f(it_mid->x(),it_mid->y()); } - //transform them in image-coordinates - std::unique_lock lock(mutex); transform_helper_.worldToImage(l.wStart,l.iStart); transform_helper_.worldToImage(l.wEnd,l.iEnd); lines_.push_back(l); - linesToProcess_++; - - conditionNewLine_.notify_one(); - } - - if(numThreads_ == 0) { - // single threaded for(SearchLine &l:lines_){ processSearchLine(l); } - } else { - // multi threaded - - // initialize and start threads if not yet there - if(threads_.size() == 0) { - threadsRunning_ = true; - for(int i = 0; i < numThreads_; i++) { - threads_.emplace_back([this] () { - threadFunction(); - }); - } - } - - // wait till every search line was processed - { - std::unique_lock lock(mutex); - while(linesToProcess_ > 0) conditionNewLine_.wait(lock); - } } return true; } -void NewRoadDetection::threadFunction() { - while(threadsRunning_) { - SearchLine line; - { - std::unique_lock lock(mutex); - while(threadsRunning_ && lines_.empty()) conditionNewLine_.wait(lock); - if(lines_.size() > 0) { - line = lines_.front(); - lines_.pop_front(); - } - if(!threadsRunning_) { - break; - } - } - processSearchLine(line); - } -} - void NewRoadDetection::processSearchLine(const SearchLine &l) { // std::vector xv; // std::vector yv; @@ -322,8 +259,6 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { // draw unfiltered image points #if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) - if(renderDebugImage_) { - std::unique_lock lock(debugAllPointsMutex); cv::namedWindow("Unfiltered points", CV_WINDOW_NORMAL); cv::Mat found_points_mat = current_image_->clone(); for (auto point : foundPoints) { @@ -339,7 +274,6 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { #ifdef PUBLISH_DEBUG detected_points_pub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, found_points_mat).toImageMsg()); #endif - } #endif //filter @@ -384,9 +318,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { // draw filtered points #if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) - if(renderDebugImage_) { // todo: make some more efficient storage method here, will translate back and forth here for now - std::unique_lock lock(debugValidPointsMutex); cv::namedWindow("Filtered Points", CV_WINDOW_NORMAL); cv::Mat filtered_points_mat = current_image_->clone(); cv::Point img_point; @@ -400,7 +332,6 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { #ifdef PUBLISH_DEBUG filtered_points_pub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, filtered_points_mat).toImageMsg()); #endif - } #endif //Handle found points @@ -445,37 +376,21 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { } lane_out.roadStateType = drive_ros_image_recognition::RoadLane::UNKNOWN; line_output_pub_.publish(lane_out); - - std::unique_lock lock(mutex); - linesToProcess_--; - conditionLineProcessed_.notify_all(); } -void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::NewRoadDetectionConfig& config, uint32_t level){ +void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig &config, uint32_t level){ image_operator_.setConfig(config); searchOffset_ = config.searchOffset; findPointsBySobel_ = config.findBySobel; - renderDebugImage_ = config.renderDebugImage; minLineWidthMul_ = config.minLineWidthMul; maxLineWidthMul_ = config.maxLineWidthMul; brightness_threshold_ = config.brightness_threshold; laneWidthOffsetInMeter_ = config.laneWidthOffsetInMeter; useWeights_ = config.useWeights; sobelThreshold_ = config.sobelThreshold; - numThreads_ = config.threads; } NewRoadDetection::~NewRoadDetection() { - { - std::unique_lock lock(mutex); - threadsRunning_ = false; - conditionNewLine_.notify_all(); - } - for(auto &t : threads_) { - if(t.joinable()) { - t.join(); - } - } } void NewRoadDetectionNodelet::onInit() { diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index aa55c18..d8f89bc 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -1,21 +1,24 @@ #include #include #include -//#include "drive_ros_image_recognition/warp_image.h" #include "drive_ros_image_recognition/street_crossing.h" +#include namespace drive_ros_image_recognition { -StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh_, const ros::NodeHandle pnh_) - : nh(nh_) - , pnh(pnh_) - , imageTransport(nh_) - , sobelThreshold(50) - , minLineWidthMul(0.5) - , maxLineWidthMul(2.0) - , lineWidth(0.4) +StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh) + : nh_(nh) + , pnh_(pnh) + , imageTransport_(pnh) + , sobelThreshold_(50) + , minLineWidthMul_(0.5) + , maxLineWidthMul_(2.0) + , lineWidth_(0.4) + , line_output_pub_() , transform_helper_() , image_operator_() + , dsrv_server_() + , dsrv_cb_(boost::bind(&StreetCrossingDetection::reconfigureCB, this, _1, _2)) #ifdef DRAW_DEBUG , debugImage(0, 0, CV_8UC1) #endif @@ -23,28 +26,27 @@ StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh_, cons } StreetCrossingDetection::~StreetCrossingDetection() { - } bool StreetCrossingDetection::init() { - imageSubscriber = imageTransport.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); - // todo: advertise lines - -#ifdef DRAW_DEBUG - debugImagePublisher = imageTransport.advertise("/street_crossing/debug_image", 10); -#endif + dsrv_server_.setCallback(dsrv_cb_); - transform_helper_.init(pnh); + transform_helper_.init(pnh_); image_operator_ = ImageOperator(transform_helper_); + imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); + ROS_INFO_STREAM("Subscriber image transport with topic "<("line_out",10); + +#ifdef DRAW_DEBUG + debugImagePublisher = imageTransport_.advertise("/street_crossing/debug_image", 10); +#endif return true; } -// todo: -// dynamic reconfigure - void StreetCrossingDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { - currentImage = convertImageMessage(imgIn); + currentImage_ = convertImageMessage(imgIn); findStartline(); } @@ -52,115 +54,57 @@ bool StreetCrossingDetection::findStartline() { std::vector linePoints; SearchLine mySl; - // Sobel -> will leave this in for now, even though we actually sobel two times like this - currentSobelImage.reset(new cv::Mat(currentImage->rows, currentImage->cols, CV_8UC1)); - cv::Sobel(*currentImage, *currentSobelImage, -1, 0, 1); - // use search-line to find stop-line - mySl.iStart = cv::Point2f(currentImage->cols / 2, currentImage->rows * .8); - mySl.iEnd = cv::Point2f(currentImage->cols / 2, currentImage->rows * .4); + mySl.iStart = cv::Point2f(currentImage_->cols / 2, currentImage_->rows * .4); + mySl.iEnd = cv::Point2f(currentImage_->cols / 2, currentImage_->rows * .8); -// linePoints = processSearchLine(mySl); - - // test a single line - cv::Mat debug_image; - cv::cvtColor(*currentImage, debug_image, CV_GRAY2RGB); + // find line using unified header search_direction search_dir = search_direction::y; search_method search_meth = search_method::sobel; std::vector image_points; std::vector line_widths; - image_operator_.findByLineSearch(mySl,*currentImage,search_dir,search_meth,image_points,line_widths); - cv::line(debug_image, mySl.iStart, mySl.iEnd, cv::Scalar(0,255,0)); + image_operator_.findByLineSearch(mySl,*currentImage_,search_dir,search_meth,image_points,line_widths); +#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) + cv::Mat debug_image; + cv::cvtColor(*currentImage_, debug_image, CV_GRAY2RGB); + cv::line(debug_image, mySl.iStart, mySl.iEnd, cv::Scalar(255,255,255)); for (auto point: image_points) { - cv::circle(debug_image,point,2,cv::Scalar(0,0,255),2); + cv::circle(debug_image,point,2,cv::Scalar(0,255,0),2); } +#endif +#ifdef DRAW_DEBUG cv::namedWindow("Crossing Search debug",CV_WINDOW_NORMAL); cv::imshow("Crossing Search debug",debug_image); - cv::waitKey(0); + cv::waitKey(1); +#endif +#ifdef PUBLISH_DEBUG + debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debugImage).toImageMsg()); +#endif // test array of lines - image_operator_.debugPointsImage(*currentImage, search_dir, search_meth); + // image_operator_.debugPointsImage(*currentImage, search_dir, search_meth); +} - // Draw / publish points -#ifdef DRAW_DEBUG - // debugImage = currentSobelImage->clone(); - debugImage = currentImage->clone(); - // Image dimensions:1280x344 - for(auto point : linePoints) { - cv::line(debugImage, cv::Point(point.x - 40, point.y), cv::Point(point.x + 40, point.y), cv::Scalar(255)); - } - // draw searchline - cv::line(debugImage, mySl.iStart, mySl.iEnd, cv::Scalar(255)); - debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debugImage).toImageMsg()); -#endif +void StreetCrossingDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level){ + image_operator_.setConfig(config); + sobelThreshold_ = config.sobelThreshold; + minLineWidthMul_ = config.minLineWidthMul; + maxLineWidthMul_ = config.maxLineWidthMul; + lineWidth_ = config.lineWidth; } -std::vector StreetCrossingDetection::processSearchLine(SearchLine &sl) { - std::vector foundPoints; - - bool foundLowHigh = false; - int pxlCounter = 0; - cv::Point2f iStartPxlPeak; - - cv::LineIterator lineIt(*currentSobelImage, sl.iStart, sl.iEnd); - cv::Rect rect(0, 0, currentSobelImage->cols, currentSobelImage->rows); - // float iDist = cv::norm(sl.iEnd - sl.iStart); - // float wDist = cv::norm(sl.wEnd - sl.wStart); - - for(int i = 0; i < lineIt.count; i++, ++lineIt) { - // safety check : is the point inside the image - if(!rect.contains(lineIt.pos())){ - - ROS_WARN("Received an invalid point outside the image to check for Sobel (%i,%i)", lineIt.pos().x, lineIt.pos().y); - return foundPoints; - } - - // Search for a bright pixel. Searching from bottom to top - int sobel = currentSobelImage->at(lineIt.pos()); - - if(sobel > sobelThreshold) { - // found low-high pass - if(!foundLowHigh) { - foundLowHigh = true; - pxlCounter = 0; - iStartPxlPeak.x = lineIt.pos().x; - iStartPxlPeak.y = lineIt.pos().y; - } - } else if(sobel < -sobelThreshold) { - // found high-low pass - // check if we found a lowHigh + highLow border - if(foundLowHigh){ - //check if the points have the right distance - // TODO to bad, calculate for each road line (how should we use them for searching? - // float pxlPeakWidth = iDist / wDist * lineWidth; // todo: understand this - - //logger.debug("")<<"crossing found highLow: "< (pxlPeakWidth * minLineWidthMul)) && (pxlCounter < (pxlPeakWidth * maxLineWidthMul))) { - // return the middle of the line we found - // cv::Point2f iMid((iStartPxlPeak.x + lineIt.pos().x) *.5, (iStartPxlPeak.x + lineIt.pos().y) * .5); - - foundPoints.push_back(lineIt.pos()); - // cv::Point2f wMid; - // imageToWorld(iMid, wMid); - // foundPoints.push_back(wMid); - // ROS_DEBUG("Found a stop-line"); - // } - } - pxlCounter = 0; - foundLowHigh = false; - // if not, we dont have to do anything - } - - // for calculation of line width - if(foundLowHigh){ - pxlCounter++; - } - } - return foundPoints; +void StreetCrossingDetectionNodelet::onInit() { + street_crossing_detection_.reset(new StreetCrossingDetection(getNodeHandle(),getPrivateNodeHandle())); + if (!street_crossing_detection_->init()) { + ROS_ERROR("StreetCrossing nodelet failed to initialize"); + // nodelet failing will kill the entire loader anyway + ros::shutdown(); + } + else { + ROS_INFO("StreetCrossing detection nodelet succesfully initialized"); + } } -} //namespace drive_ros_image_recognition +} // namespace drive_ros_image_recognition + +PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::StreetCrossingDetectionNodelet, nodelet::Nodelet) From b4b6c404069efaa16d65704fbeadc1c48f48fe5a Mon Sep 17 00:00:00 2001 From: FaHa Date: Tue, 18 Jul 2017 14:39:12 +0200 Subject: [PATCH 35/94] changed default camera topic ns --- launch/warp_image.launch | 2 +- launch/warp_image_nodelet.launch | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/warp_image.launch b/launch/warp_image.launch index 26f0777..897dea7 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -1,5 +1,5 @@ - + diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch index 18cbfb3..15b5cd8 100644 --- a/launch/warp_image_nodelet.launch +++ b/launch/warp_image_nodelet.launch @@ -1,7 +1,7 @@ - + From 3fa7435ce82d3f7a13b3b4e206d1246bc37c931f Mon Sep 17 00:00:00 2001 From: FaHa Date: Tue, 18 Jul 2017 14:42:22 +0200 Subject: [PATCH 36/94] correct homography yaml path --- launch/warp_image.launch | 2 +- launch/warp_image_nodelet.launch | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/warp_image.launch b/launch/warp_image.launch index 897dea7..fc1b5f4 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -4,7 +4,7 @@ - + diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch index 15b5cd8..36239d3 100644 --- a/launch/warp_image_nodelet.launch +++ b/launch/warp_image_nodelet.launch @@ -10,7 +10,7 @@ - + @@ -21,7 +21,7 @@ - + From 5afcd17bf163aebba5d8e641eda801c6b65cee6f Mon Sep 17 00:00:00 2001 From: FaHa Date: Tue, 18 Jul 2017 14:45:27 +0200 Subject: [PATCH 37/94] updated yaml paths in launch files --- launch/new_road_detection.launch | 4 ++-- launch/new_road_detection_debug.launch | 4 ++-- launch/new_road_detection_nodelet.launch | 8 ++++---- launch/street_crossing.launch | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/launch/new_road_detection.launch b/launch/new_road_detection.launch index ce14e0c..c8ebdaf 100644 --- a/launch/new_road_detection.launch +++ b/launch/new_road_detection.launch @@ -7,8 +7,8 @@ - + - + diff --git a/launch/new_road_detection_debug.launch b/launch/new_road_detection_debug.launch index 55d5003..f2d74b7 100644 --- a/launch/new_road_detection_debug.launch +++ b/launch/new_road_detection_debug.launch @@ -7,8 +7,8 @@ - + - + diff --git a/launch/new_road_detection_nodelet.launch b/launch/new_road_detection_nodelet.launch index fa43bf7..bb3bd1c 100644 --- a/launch/new_road_detection_nodelet.launch +++ b/launch/new_road_detection_nodelet.launch @@ -13,9 +13,9 @@ - + - + @@ -26,9 +26,9 @@ - + - + diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch index 358c506..6527772 100644 --- a/launch/street_crossing.launch +++ b/launch/street_crossing.launch @@ -4,6 +4,6 @@ - + From e2dbeb6554a8d60ff1002286008030f2c3fb3f0b Mon Sep 17 00:00:00 2001 From: Fabian H Date: Tue, 18 Jul 2017 16:17:09 +0200 Subject: [PATCH 38/94] removed set matrix to [0] in getHomographyMatParam --- include/drive_ros_image_recognition/common_image_operations.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index af0414e..9fbf846 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -54,8 +54,8 @@ inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const ROS_ERROR("Retreived homography matrix %s does not have 9 values", mat_param.c_str()); return false; } + // update values for(unsigned i=0; i < temp_vals.size(); i++) { - mat = cv::Mat::zeros(3,3,CV_64F); mat.at(i) = temp_vals[i]; } ROS_DEBUG_STREAM("Paramater matrix loaded as: "< Date: Tue, 18 Jul 2017 16:52:44 +0200 Subject: [PATCH 39/94] added matrix in getHomographyMatParam to ensure correct dimensions --- include/drive_ros_image_recognition/common_image_operations.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 9fbf846..055e2fa 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -55,6 +55,7 @@ inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const return false; } // update values + mat = cv::Mat::zeros(3,3,CV_64F); for(unsigned i=0; i < temp_vals.size(); i++) { mat.at(i) = temp_vals[i]; } From d6bf6f6205aff94d56db8b4357415e6596692322 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Thu, 5 Oct 2017 10:59:50 +0200 Subject: [PATCH 40/94] change camera1 to camera in launch files --- launch/new_road_detection_debug.launch | 2 +- launch/new_road_detection_nodelet.launch | 2 +- launch/street_crossing.launch | 4 ++-- launch/street_crossing_nodelet.launch | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/launch/new_road_detection_debug.launch b/launch/new_road_detection_debug.launch index f2d74b7..b5f0b6d 100644 --- a/launch/new_road_detection_debug.launch +++ b/launch/new_road_detection_debug.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/new_road_detection_nodelet.launch b/launch/new_road_detection_nodelet.launch index bb3bd1c..a8d8ea3 100644 --- a/launch/new_road_detection_nodelet.launch +++ b/launch/new_road_detection_nodelet.launch @@ -2,7 +2,7 @@ - + diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch index 6527772..92d582d 100644 --- a/launch/street_crossing.launch +++ b/launch/street_crossing.launch @@ -1,8 +1,8 @@ - + - + diff --git a/launch/street_crossing_nodelet.launch b/launch/street_crossing_nodelet.launch index b11db2a..4f5b948 100644 --- a/launch/street_crossing_nodelet.launch +++ b/launch/street_crossing_nodelet.launch @@ -1,6 +1,6 @@ - + From 94fd9ef22fdd2ebaf203fc485f9ff54de9e506e4 Mon Sep 17 00:00:00 2001 From: FaHa Date: Thu, 5 Oct 2017 11:30:08 +0200 Subject: [PATCH 41/94] refactoring: changed naming from new_road_detection to road_detection --- CMakeLists.txt | 26 ++++----- ...oad_detection.yaml => road_detection.yaml} | 2 +- ...{new_road_detection.h => road_detection.h} | 18 +++--- ...detection.launch => road_detection.launch} | 4 +- ...bug.launch => road_detection_debug.launch} | 4 +- ...t.launch => road_detection_nodelet.launch} | 16 +++--- nodelets.xml | 8 +-- package.xml | 6 +- src/new_road_detection_node.cpp | 26 --------- ..._road_detection.cpp => road_detection.cpp} | 56 +++++++++---------- src/road_detection_node.cpp | 26 +++++++++ 11 files changed, 96 insertions(+), 96 deletions(-) rename config/{new_road_detection.yaml => road_detection.yaml} (93%) rename include/drive_ros_image_recognition/{new_road_detection.h => road_detection.h} (89%) rename launch/{new_road_detection.launch => road_detection.launch} (79%) rename launch/{new_road_detection_debug.launch => road_detection_debug.launch} (79%) rename launch/{new_road_detection_nodelet.launch => road_detection_nodelet.launch} (63%) delete mode 100644 src/new_road_detection_node.cpp rename src/{new_road_detection.cpp => road_detection.cpp} (86%) create mode 100644 src/road_detection_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fef719..130273b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,45 +62,45 @@ include_directories( ################################################################################ -# New road detection node (LMS port) +# road detection node (LMS port) ################################################################################ -add_executable(new_road_detection_node - src/new_road_detection.cpp - src/new_road_detection_node.cpp +add_executable(road_detection_node + src/road_detection.cpp + src/road_detection_node.cpp ) -add_dependencies(new_road_detection_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(road_detection_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(new_road_detection_node +target_link_libraries(road_detection_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ) -install(TARGETS new_road_detection_node +install(TARGETS road_detection_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) ################################################################################ -# New road detection nodelet (LMS port) +# road detection nodelet (LMS port) ################################################################################ -add_library(new_road_detection_nodelet - src/new_road_detection.cpp +add_library(road_detection_nodelet + src/road_detection.cpp ) -add_dependencies(new_road_detection_nodelet ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(road_detection_nodelet ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(new_road_detection_nodelet +target_link_libraries(road_detection_nodelet ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ) -install(TARGETS new_road_detection_nodelet +install(TARGETS road_detection_nodelet ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/config/new_road_detection.yaml b/config/road_detection.yaml similarity index 93% rename from config/new_road_detection.yaml rename to config/road_detection.yaml index bea791b..9009a35 100644 --- a/config/new_road_detection.yaml +++ b/config/road_detection.yaml @@ -1,4 +1,4 @@ -new_road_detection: +road_detection: searchOffset: 0.15 distanceBetweenSearchlines: 0.1 minLineWidthMul: 0.5 diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/road_detection.h similarity index 89% rename from include/drive_ros_image_recognition/new_road_detection.h rename to include/drive_ros_image_recognition/road_detection.h index 2bcc588..dc8ee81 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/road_detection.h @@ -1,5 +1,5 @@ -#ifndef NEW_ROAD_DETECTION_H -#define NEW_ROAD_DETECTION_H +#ifndef ROAD_DETECTION_H +#define ROAD_DETECTION_H #include #include @@ -25,9 +25,9 @@ namespace drive_ros_image_recognition { /** - * @brief Port of LMS module new_road_detection to ROS + * @brief Port of LMS module road_detection to ROS **/ -class NewRoadDetection { +class RoadDetection { // configs float searchOffset_; float distanceBetweenSearchlines_; @@ -85,17 +85,17 @@ class NewRoadDetection { ImageOperator image_operator_; public: - NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); - ~NewRoadDetection(); + RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); + ~RoadDetection(); bool init(); }; -class NewRoadDetectionNodelet: public nodelet::Nodelet { +class RoadDetectionNodelet: public nodelet::Nodelet { public: virtual void onInit(); private: - std::unique_ptr new_road_detection_; + std::unique_ptr road_detection_; }; } -#endif // NEW_ROAD_DETECTION_H +#endif // ROAD_DETECTION_H diff --git a/launch/new_road_detection.launch b/launch/road_detection.launch similarity index 79% rename from launch/new_road_detection.launch rename to launch/road_detection.launch index c8ebdaf..a0b233c 100644 --- a/launch/new_road_detection.launch +++ b/launch/road_detection.launch @@ -2,12 +2,12 @@ - + - + diff --git a/launch/new_road_detection_debug.launch b/launch/road_detection_debug.launch similarity index 79% rename from launch/new_road_detection_debug.launch rename to launch/road_detection_debug.launch index b5f0b6d..ed02187 100644 --- a/launch/new_road_detection_debug.launch +++ b/launch/road_detection_debug.launch @@ -2,12 +2,12 @@ - + - + diff --git a/launch/new_road_detection_nodelet.launch b/launch/road_detection_nodelet.launch similarity index 63% rename from launch/new_road_detection_nodelet.launch rename to launch/road_detection_nodelet.launch index a8d8ea3..fc3c271 100644 --- a/launch/new_road_detection_nodelet.launch +++ b/launch/road_detection_nodelet.launch @@ -1,32 +1,32 @@ - + - - + + - + - + - - + + - + diff --git a/nodelets.xml b/nodelets.xml index 2d252dc..66a8106 100644 --- a/nodelets.xml +++ b/nodelets.xml @@ -7,16 +7,16 @@ - - - + + + Image warper nodelet. - + diff --git a/package.xml b/package.xml index 8647986..8a87e87 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ 0.0.0 Phoenix Drive image processing. - + mykyta @@ -51,7 +51,7 @@ geometry_msgs tf message_filters - + message_generation cv_bridge @@ -65,7 +65,7 @@ geometry_msgs tf message_filters - + message_runtime diff --git a/src/new_road_detection_node.cpp b/src/new_road_detection_node.cpp deleted file mode 100644 index c05f2ab..0000000 --- a/src/new_road_detection_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "new_road_detection"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - -#ifndef NDEBUG - // give GDB time to attach - ros::Duration(2.0).sleep(); -#endif - - drive_ros_image_recognition::NewRoadDetection new_road_detection(nh,pnh); - if (!new_road_detection.init()) { - return 1; - } - else { - ROS_INFO("New road detection node succesfully initialized"); - } - - while (ros::ok()) { - ros::spin(); - } - return 0; -} diff --git a/src/new_road_detection.cpp b/src/road_detection.cpp similarity index 86% rename from src/new_road_detection.cpp rename to src/road_detection.cpp index 51c9911..a46876c 100644 --- a/src/new_road_detection.cpp +++ b/src/road_detection.cpp @@ -1,9 +1,9 @@ -#include "drive_ros_image_recognition/new_road_detection.h" +#include "drive_ros_image_recognition/road_detection.h" #include namespace drive_ros_image_recognition { -NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh): +RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh): nh_(nh), pnh_(pnh), // img_sub_(), @@ -27,7 +27,7 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand filtered_points_pub_(), #endif dsrv_server_(), - dsrv_cb_(boost::bind(&NewRoadDetection::reconfigureCB, this, _1, _2)), + dsrv_cb_(boost::bind(&RoadDetection::reconfigureCB, this, _1, _2)), lines_(), linesToProcess_(0), current_image_(), @@ -38,45 +38,45 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand { } -bool NewRoadDetection::init() { +bool RoadDetection::init() { // load parameters - if (!pnh_.getParam("new_road_detection/searchOffset", searchOffset_)) { + if (!pnh_.getParam("road_detection/searchOffset", searchOffset_)) { ROS_WARN_STREAM("Unable to load 'searchOffset' parameter, using default: "<(pnh_,"road_in", 1000)); // sync_.reset(new message_filters::Synchronizer(SyncImageToRoad(100), *img_sub_, *road_sub_)); // sync_->setAgePenalty(1.0); -// sync_->registerCallback(boost::bind(&NewRoadDetection::syncCallback, this, _1, _2)); +// sync_->registerCallback(boost::bind(&RoadDetection::syncCallback, this, _1, _2)); - img_sub_debug_ = it_.subscribe("img_in", 1000, &NewRoadDetection::debugImageCallback, this); + img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); line_output_pub_ = pnh_.advertise("line_out",10); @@ -113,7 +113,7 @@ bool NewRoadDetection::init() { return true; } -void NewRoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) { +void RoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) { current_image_ = convertImageMessage(img_in); // crop image to 410 width cv::Rect roi(cv::Point(current_image_->cols/2-(410/2),0),cv::Point(current_image_->cols/2+(410/2),current_image_->rows)); @@ -124,7 +124,7 @@ void NewRoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_ image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth_brightness); } -void NewRoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in){ +void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in){ current_image_ = convertImageMessage(img_in); road_points_buffer_ = road_in->points; @@ -169,7 +169,7 @@ void NewRoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, co // } } -bool NewRoadDetection::find(){ +bool RoadDetection::find(){ //clear old lines ROS_INFO("Creating new lines"); lines_.clear(); @@ -230,7 +230,7 @@ bool NewRoadDetection::find(){ return true; } -void NewRoadDetection::processSearchLine(const SearchLine &l) { +void RoadDetection::processSearchLine(const SearchLine &l) { // std::vector xv; // std::vector yv; @@ -378,7 +378,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { line_output_pub_.publish(lane_out); } -void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig &config, uint32_t level){ +void RoadDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig &config, uint32_t level){ image_operator_.setConfig(config); searchOffset_ = config.searchOffset; findPointsBySobel_ = config.findBySobel; @@ -390,21 +390,21 @@ void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionC sobelThreshold_ = config.sobelThreshold; } -NewRoadDetection::~NewRoadDetection() { +RoadDetection::~RoadDetection() { } -void NewRoadDetectionNodelet::onInit() { - new_road_detection_.reset(new NewRoadDetection(getNodeHandle(),getPrivateNodeHandle())); - if (!new_road_detection_->init()) { - ROS_ERROR("New_road_detection nodelet failed to initialize"); +void RoadDetectionNodelet::onInit() { + road_detection_.reset(new RoadDetection(getNodeHandle(),getPrivateNodeHandle())); + if (!road_detection_->init()) { + ROS_ERROR("road_detection nodelet failed to initialize"); // nodelet failing will kill the entire loader anyway ros::shutdown(); } else { - ROS_INFO("New road detection nodelet succesfully initialized"); + ROS_INFO("road detection nodelet succesfully initialized"); } } } // namespace drive_ros_image_recognition -PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::NewRoadDetectionNodelet, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::RoadDetectionNodelet, nodelet::Nodelet) diff --git a/src/road_detection_node.cpp b/src/road_detection_node.cpp new file mode 100644 index 0000000..90ff1b3 --- /dev/null +++ b/src/road_detection_node.cpp @@ -0,0 +1,26 @@ +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "road_detection"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + +#ifndef NDEBUG + // give GDB time to attach + ros::Duration(2.0).sleep(); +#endif + + drive_ros_image_recognition::RoadDetection road_detection(nh,pnh); + if (!road_detection.init()) { + return 1; + } + else { + ROS_INFO("road detection node succesfully initialized"); + } + + while (ros::ok()) { + ros::spin(); + } + return 0; +} From b17da1d2a842f607d247f8e172156fb0c8adf614 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 7 Oct 2017 13:48:42 +0200 Subject: [PATCH 42/94] added first unittest test --- CMakeLists.txt | 8 ++++++++ test/common_image_operations_test.cpp | 21 +++++++++++++++++++++ test/common_image_operations_test.test | 3 +++ 3 files changed, 32 insertions(+) create mode 100644 test/common_image_operations_test.cpp create mode 100644 test/common_image_operations_test.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 130273b..a39f737 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,9 @@ find_package(catkin REQUIRED COMPONENTS #include Boost for lane line processing find_package(Boost REQUIRED) +# include test suite +find_package(rostest REQUIRED) + add_message_files( FILES RoadLane.msg @@ -215,3 +218,8 @@ install(DIRECTORY config/ FILES_MATCHING PATTERN "*.yaml" ) +################################################################################ +# test header files +################################################################################ +add_rostest_gtest(common_image_operations_test test/common_image_operations_test.test test/common_image_operations_test.cpp) +target_link_libraries(common_image_operations_test ${catkin_LIBRARIES}) diff --git a/test/common_image_operations_test.cpp b/test/common_image_operations_test.cpp new file mode 100644 index 0000000..b15f678 --- /dev/null +++ b/test/common_image_operations_test.cpp @@ -0,0 +1,21 @@ +#include +#include "drive_ros_image_recognition/common_image_operations.h" + + +// Declare test +TEST(TestSuite, testCase0) +{ + EXPECT_EQ(1,1); +} + +// Declare test +TEST(TestSuite, testCase1) +{ + EXPECT_EQ(1,0); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/common_image_operations_test.test b/test/common_image_operations_test.test new file mode 100644 index 0000000..47d1ed9 --- /dev/null +++ b/test/common_image_operations_test.test @@ -0,0 +1,3 @@ + + + From cd4cce59912c13217a5d90eeaecb6c4aa8ccaf2a Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 7 Oct 2017 14:23:52 +0200 Subject: [PATCH 43/94] add if() clause to cmake test --- CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a39f737..1fa7146 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,9 +31,6 @@ find_package(catkin REQUIRED COMPONENTS #include Boost for lane line processing find_package(Boost REQUIRED) -# include test suite -find_package(rostest REQUIRED) - add_message_files( FILES RoadLane.msg @@ -221,5 +218,8 @@ install(DIRECTORY config/ ################################################################################ # test header files ################################################################################ -add_rostest_gtest(common_image_operations_test test/common_image_operations_test.test test/common_image_operations_test.cpp) -target_link_libraries(common_image_operations_test ${catkin_LIBRARIES}) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(common_image_operations_test test/common_image_operations_test.test test/common_image_operations_test.cpp) + target_link_libraries(common_image_operations_test ${catkin_LIBRARIES}) +endif() From dbc15131d43ce87484f86d62bb70fd759a538944 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 7 Oct 2017 14:46:39 +0200 Subject: [PATCH 44/94] removed failing testcase --- test/common_image_operations_test.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/test/common_image_operations_test.cpp b/test/common_image_operations_test.cpp index b15f678..5534929 100644 --- a/test/common_image_operations_test.cpp +++ b/test/common_image_operations_test.cpp @@ -8,12 +8,6 @@ TEST(TestSuite, testCase0) EXPECT_EQ(1,1); } -// Declare test -TEST(TestSuite, testCase1) -{ - EXPECT_EQ(1,0); -} - // Run all the tests that were declared with TEST() int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); From fd489876a0969aa619acb12f9d0fe28d1cfd3455 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sun, 8 Oct 2017 00:07:32 +0200 Subject: [PATCH 45/94] added more testing --- CMakeLists.txt | 22 ++++++++------- launch/road_detection.launch | 2 +- launch/road_detection.test | 3 +++ test/common_image_operations_test.cpp | 8 +----- test/common_image_operations_test.test | 3 --- test/geometry_common_test.cpp | 37 ++++++++++++++++++++++++++ test/road_detection_test.cpp | 15 +++++++++++ 7 files changed, 69 insertions(+), 21 deletions(-) create mode 100644 launch/road_detection.test delete mode 100644 test/common_image_operations_test.test create mode 100644 test/geometry_common_test.cpp create mode 100644 test/road_detection_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1fa7146..7e0524b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,6 +84,17 @@ install(TARGETS road_detection_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(road_detection_node_test launch/road_detection.test + test/common_image_operations_test.cpp + test/road_detection_test.cpp + test/geometry_common_test.cpp + ) + target_link_libraries(road_detection_node_test ${catkin_LIBRARIES}) +endif() + + ################################################################################ # road detection nodelet (LMS port) ################################################################################ @@ -148,7 +159,7 @@ install(TARGETS warp_image_nodelet ) ################################################################################ -# Street crossing detection node (LMS port) +# Street crossing detection nodelet (LMS port) ################################################################################ add_library(street_crossing_nodelet @@ -214,12 +225,3 @@ install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} FILES_MATCHING PATTERN "*.yaml" ) - -################################################################################ -# test header files -################################################################################ -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(common_image_operations_test test/common_image_operations_test.test test/common_image_operations_test.cpp) - target_link_libraries(common_image_operations_test ${catkin_LIBRARIES}) -endif() diff --git a/launch/road_detection.launch b/launch/road_detection.launch index a0b233c..ed02187 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/road_detection.test b/launch/road_detection.test new file mode 100644 index 0000000..c4b1250 --- /dev/null +++ b/launch/road_detection.test @@ -0,0 +1,3 @@ + + + diff --git a/test/common_image_operations_test.cpp b/test/common_image_operations_test.cpp index 5534929..5924139 100644 --- a/test/common_image_operations_test.cpp +++ b/test/common_image_operations_test.cpp @@ -3,13 +3,7 @@ // Declare test -TEST(TestSuite, testCase0) +TEST(CommonImageOperations, testCase0) { EXPECT_EQ(1,1); } - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv){ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/common_image_operations_test.test b/test/common_image_operations_test.test deleted file mode 100644 index 47d1ed9..0000000 --- a/test/common_image_operations_test.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/test/geometry_common_test.cpp b/test/geometry_common_test.cpp new file mode 100644 index 0000000..b2af582 --- /dev/null +++ b/test/geometry_common_test.cpp @@ -0,0 +1,37 @@ +#include +#include "drive_ros_image_recognition/geometry_common.h" + + +// Declare test +TEST(GeometryCommon, moveOrtho) +{ + linestring line1, line2, line3; + // 2 points on a vertical line (easier to test) + point_xy p1(56.567, 37.5989); + point_xy p2(56.567, 1985.4); + line1.push_back(p1); + line1.push_back(p2); + + const double dis=78.5569; // distance to move + const double err=0.0000001; // allowed error + + // move line to the right + drive_ros_geometry_common::moveOrthogonal(line1, line2, dis); + EXPECT_NEAR(line1.at(0).x()+dis, line2.at(0).x(), err); + EXPECT_NEAR(line1.at(1).x()+dis, line2.at(1).x(), err); + EXPECT_NEAR(line1.at(0).y(), line2.at(0).y(), err); + EXPECT_NEAR(line1.at(1).y(), line2.at(1).y(), err); + + // move line back to the left + drive_ros_geometry_common::moveOrthogonal(line2, line3, -dis); + EXPECT_NEAR(line2.at(0).x()-dis, line3.at(0).x(), err); + EXPECT_NEAR(line2.at(1).x()-dis, line3.at(1).x(), err); + EXPECT_NEAR(line2.at(0).y(), line3.at(0).y(), err); + EXPECT_NEAR(line2.at(1).y(), line3.at(1).y(), err); + + EXPECT_NEAR(line1.at(0).x(), line3.at(0).x(), err); + EXPECT_NEAR(line1.at(1).x(), line3.at(1).x(), err); + EXPECT_NEAR(line1.at(0).y(), line3.at(0).y(), err); + EXPECT_NEAR(line1.at(1).y(), line3.at(1).y(), err); + +} diff --git a/test/road_detection_test.cpp b/test/road_detection_test.cpp new file mode 100644 index 0000000..016afcb --- /dev/null +++ b/test/road_detection_test.cpp @@ -0,0 +1,15 @@ +#include +#include "drive_ros_image_recognition/road_detection.h" + + +// Declare test +TEST(RoadDetection, testCase0) +{ + EXPECT_EQ(1,1); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 81eeceb2097e2cbc651c2b60b7572d1652fda9c8 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sun, 8 Oct 2017 09:59:19 +0200 Subject: [PATCH 46/94] added some more geometry common tests and fixed move orthogonal with same points --- .../geometry_common.h | 24 ++++++++-------- test/geometry_common_test.cpp | 28 +++++++++++++++++++ 2 files changed, 41 insertions(+), 11 deletions(-) diff --git a/include/drive_ros_image_recognition/geometry_common.h b/include/drive_ros_image_recognition/geometry_common.h index 782eb92..a735722 100644 --- a/include/drive_ros_image_recognition/geometry_common.h +++ b/include/drive_ros_image_recognition/geometry_common.h @@ -22,18 +22,20 @@ struct orthogonalHelper{ orthogonalHelper(double distance) : distance_(distance){} inline void operator()(Segment &s){ - // offset 90 degrees - segment_hc rot; - boost::geometry::transform(s, rot, rotate); - - // normalize double length = boost::geometry::distance(s.first, s.second); - trans::scale_transformer scale(distance_/length); - boost::geometry::transform(rot, rot, scale); - - // offset original segment - trans::translate_transformer translate(bg::get<0>(rot.second)-bg::get<0>(rot.first), bg::get<1>(rot.second)-bg::get<1>(rot.first)); - boost::geometry::transform(s,s,translate); + if(length > 0){ + // offset 90 degrees + segment_hc rot; + boost::geometry::transform(s, rot, rotate); + + // normalize + trans::scale_transformer scale(distance_/length); + boost::geometry::transform(rot, rot, scale); + + // offset original segment + trans::translate_transformer translate(bg::get<0>(rot.second)-bg::get<0>(rot.first), bg::get<1>(rot.second)-bg::get<1>(rot.first)); + boost::geometry::transform(s,s,translate); + } } double distance_; diff --git a/test/geometry_common_test.cpp b/test/geometry_common_test.cpp index b2af582..4bcc405 100644 --- a/test/geometry_common_test.cpp +++ b/test/geometry_common_test.cpp @@ -1,5 +1,6 @@ #include #include "drive_ros_image_recognition/geometry_common.h" +#include // Declare test @@ -15,6 +16,13 @@ TEST(GeometryCommon, moveOrtho) const double dis=78.5569; // distance to move const double err=0.0000001; // allowed error + // don't move + drive_ros_geometry_common::moveOrthogonal(line1, line2, 0); + EXPECT_NEAR(line1.at(0).x(), line2.at(0).x(), err); + EXPECT_NEAR(line1.at(1).x(), line2.at(1).x(), err); + EXPECT_NEAR(line1.at(0).y(), line2.at(0).y(), err); + EXPECT_NEAR(line1.at(1).y(), line2.at(1).y(), err); + // move line to the right drive_ros_geometry_common::moveOrthogonal(line1, line2, dis); EXPECT_NEAR(line1.at(0).x()+dis, line2.at(0).x(), err); @@ -34,4 +42,24 @@ TEST(GeometryCommon, moveOrtho) EXPECT_NEAR(line1.at(0).y(), line3.at(0).y(), err); EXPECT_NEAR(line1.at(1).y(), line3.at(1).y(), err); + + linestring line4, line5; + point_xy p3(0,0), p4(0,0); + line4.push_back(p3); + + // line with only 1 point + drive_ros_geometry_common::moveOrthogonal(line4, line5, dis); + EXPECT_NEAR(line4.at(0).x(), line5.at(0).x(), err); + EXPECT_NEAR(line4.at(0).y(), line5.at(0).y(), err); + + // two points at 0 + line4.push_back(p4); + drive_ros_geometry_common::moveOrthogonal(line4, line5, dis); + EXPECT_NEAR(line4.at(0).x(), line5.at(0).x(), err); + EXPECT_NEAR(line4.at(1).x(), line5.at(1).x(), err); + EXPECT_NEAR(line4.at(0).y(), line5.at(0).y(), err); + EXPECT_NEAR(line4.at(1).y(), line5.at(1).y(), err); + + + } From 90805514e4808e6bc46793735d9f1375f9a8568c Mon Sep 17 00:00:00 2001 From: FaHa Date: Tue, 10 Oct 2017 10:20:20 +0200 Subject: [PATCH 47/94] moved RoadLane.msg to drive_ros_msgs package --- CMakeLists.txt | 14 ++------------ .../drive_ros_image_recognition/road_detection.h | 4 ++-- .../drive_ros_image_recognition/street_crossing.h | 3 +-- msg/RoadLane.msg | 10 ---------- package.xml | 6 ++---- scripts/publish_road.sh | 2 +- src/road_detection.cpp | 8 ++++---- src/street_crossing.cpp | 2 +- 8 files changed, 13 insertions(+), 36 deletions(-) delete mode 100644 msg/RoadLane.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e0524b..d0a21c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs + drive_ros_msgs dynamic_reconfigure geometry_msgs message_generation @@ -31,24 +32,13 @@ find_package(catkin REQUIRED COMPONENTS #include Boost for lane line processing find_package(Boost REQUIRED) -add_message_files( - FILES - RoadLane.msg - ) - -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs - ) - generate_dynamic_reconfigure_options( cfg/LineDetection.cfg ) catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs message_runtime + CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs drive_ros_msgs ) find_package(OpenCV REQUIRED) diff --git a/include/drive_ros_image_recognition/road_detection.h b/include/drive_ros_image_recognition/road_detection.h index dc8ee81..75fac14 100644 --- a/include/drive_ros_image_recognition/road_detection.h +++ b/include/drive_ros_image_recognition/road_detection.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include #include @@ -77,7 +77,7 @@ class RoadDetection { dynamic_reconfigure::Server dsrv_server_; dynamic_reconfigure::Server::CallbackType dsrv_cb_; void debugImageCallback(const sensor_msgs::ImageConstPtr& img_in); - void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in); + void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in); void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); bool find(); void processSearchLine(const SearchLine &line); diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 804fc19..1971ec5 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -11,8 +11,7 @@ #include #include #include - -#include "drive_ros_image_recognition/RoadLane.h" +#include "drive_ros_msgs/RoadLane.h" namespace drive_ros_image_recognition { diff --git a/msg/RoadLane.msg b/msg/RoadLane.msg deleted file mode 100644 index c995c7c..0000000 --- a/msg/RoadLane.msg +++ /dev/null @@ -1,10 +0,0 @@ -#Port of RoadLane from LMS: -Header header -# lane points -geometry_msgs/Point32[] points -# type of lane -uint8 UNKNOWN=0 -uint8 STRAIGHT=1 -uint8 STRAIGHT_CURVE=2 -uint8 CURVE = 3 -uint8 roadStateType diff --git a/package.xml b/package.xml index 8a87e87..b10de34 100644 --- a/package.xml +++ b/package.xml @@ -51,8 +51,7 @@ geometry_msgs tf message_filters - - message_generation + drive_ros_msgs cv_bridge image_transport @@ -65,8 +64,7 @@ geometry_msgs tf message_filters - - message_runtime + drive_ros_msgs diff --git a/scripts/publish_road.sh b/scripts/publish_road.sh index 32602b9..257575f 100755 --- a/scripts/publish_road.sh +++ b/scripts/publish_road.sh @@ -1,5 +1,5 @@ # simple publisher for road messages to be able to view what happens during processing -rostopic pub /road_trajectory drive_ros_image_recognition/RoadLane "header: +rostopic pub /road_trajectory drive_ros_msgs/RoadLane "header: seq: 0 stamp: secs: 0 diff --git a/src/road_detection.cpp b/src/road_detection.cpp index a46876c..4dac63e 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -94,7 +94,7 @@ bool RoadDetection::init() { img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); - line_output_pub_ = pnh_.advertise("line_out",10); + line_output_pub_ = pnh_.advertise("line_out",10); #ifdef PUBLISH_DEBUG debug_img_pub_ = it_.advertise("debug_image_out", 10); @@ -124,7 +124,7 @@ void RoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth_brightness); } -void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in){ +void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in){ current_image_ = convertImageMessage(img_in); road_points_buffer_ = road_in->points; @@ -365,7 +365,7 @@ void RoadDetection::processSearchLine(const SearchLine &l) { } // todo: check how this gets handled and adjust the interface - drive_ros_image_recognition::RoadLane lane_out; + drive_ros_msgs::RoadLane lane_out; lane_out.header.stamp = ros::Time::now(); geometry_msgs::Point32 point_temp; point_temp.z = 0.f; @@ -374,7 +374,7 @@ void RoadDetection::processSearchLine(const SearchLine &l) { point_temp.y = point.y; lane_out.points.push_back(point_temp); } - lane_out.roadStateType = drive_ros_image_recognition::RoadLane::UNKNOWN; + lane_out.roadStateType = drive_ros_msgs::RoadLane::UNKNOWN; line_output_pub_.publish(lane_out); } diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index d8f89bc..28e66eb 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -37,7 +37,7 @@ bool StreetCrossingDetection::init() { imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); ROS_INFO_STREAM("Subscriber image transport with topic "<("line_out",10); + line_output_pub_ = pnh_.advertise("line_out",10); #ifdef DRAW_DEBUG debugImagePublisher = imageTransport_.advertise("/street_crossing/debug_image", 10); From 64c7cc50236196b0d7573734f7e236e94a6fe353 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Tue, 10 Oct 2017 12:18:46 +0200 Subject: [PATCH 48/94] modify the search-line and fix publishing the debug image --- .../street_crossing.h | 6 ++- src/street_crossing.cpp | 48 ++++++++++++++----- 2 files changed, 39 insertions(+), 15 deletions(-) diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 804fc19..779ef42 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -29,6 +29,9 @@ class StreetCrossingDetection { std::vector searchLines_; CvImagePtr currentImage_; + int startLineCounter_; + cv::Point lastStartLinePos_; + // communication image_transport::ImageTransport imageTransport_; image_transport::Subscriber imageSubscriber_; @@ -41,9 +44,8 @@ class StreetCrossingDetection { void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG image_transport::Publisher debugImagePublisher; - cv::Mat debugImage; #endif // methods diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index d8f89bc..9fba52c 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -19,9 +19,7 @@ StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh, const , image_operator_() , dsrv_server_() , dsrv_cb_(boost::bind(&StreetCrossingDetection::reconfigureCB, this, _1, _2)) - #ifdef DRAW_DEBUG - , debugImage(0, 0, CV_8UC1) - #endif + , startLineCounter_(0) { } @@ -39,8 +37,12 @@ bool StreetCrossingDetection::init() { line_output_pub_ = pnh_.advertise("line_out",10); -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG debugImagePublisher = imageTransport_.advertise("/street_crossing/debug_image", 10); + ROS_INFO("Will publish debug image to /street_crossing/debug_image"); +#endif +#ifdef DRAW_DEBUG + ROS_INFO("Draw debug image"); #endif return true; } @@ -51,25 +53,45 @@ void StreetCrossingDetection::imageCallback(const sensor_msgs::ImageConstPtr &im } bool StreetCrossingDetection::findStartline() { - std::vector linePoints; - SearchLine mySl; + SearchLine slRightLane; +// SearchLine slLeftLane; // use search-line to find stop-line - mySl.iStart = cv::Point2f(currentImage_->cols / 2, currentImage_->rows * .4); - mySl.iEnd = cv::Point2f(currentImage_->cols / 2, currentImage_->rows * .8); + slRightLane.iStart = cv::Point2f(currentImage_->cols / 2, currentImage_->rows * .4); + slRightLane.iEnd = cv::Point2f(currentImage_->cols / 2, currentImage_->rows * .8); + +// slLeftLane.iStart = cv::Point2f(currentImage_->cols / 8 * 3, currentImage_->rows * .4); +// slLeftLane.iEnd = cv::Point2f(currentImage_->cols / 4, currentImage_->rows * .8); +// slRightLane.iStart = cv::Point2f(currentImage_->cols * .55, currentImage_->rows * .4); +// slRightLane.iEnd = cv::Point2f(currentImage_->cols * .55, currentImage_->rows * .8); + +// slLeftLane.iStart = cv::Point2f(currentImage_->cols * .45, currentImage_->rows * .4); +// slLeftLane.iEnd = cv::Point2f(currentImage_->cols * .45, currentImage_->rows * .8); // find line using unified header search_direction search_dir = search_direction::y; search_method search_meth = search_method::sobel; std::vector image_points; std::vector line_widths; - image_operator_.findByLineSearch(mySl,*currentImage_,search_dir,search_meth,image_points,line_widths); + image_operator_.findByLineSearch(slRightLane,*currentImage_,search_dir,search_meth,image_points,line_widths); +// image_operator_.findByLineSearch(slLeftLane,*currentImage_,search_dir,search_meth,image_points,line_widths); + if(image_points.empty()) { + startLineCounter_ = 0; + } else { + startLineCounter_++; + if(image_points.size() > 1) { + ROS_INFO("Found more than one start/stop line"); + } + } #if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) cv::Mat debug_image; cv::cvtColor(*currentImage_, debug_image, CV_GRAY2RGB); - cv::line(debug_image, mySl.iStart, mySl.iEnd, cv::Scalar(255,255,255)); - for (auto point: image_points) { - cv::circle(debug_image,point,2,cv::Scalar(0,255,0),2); + cv::line(debug_image, slRightLane.iStart, slRightLane.iEnd, cv::Scalar(255, 255, 255), 2); +// cv::line(debug_image, slLeftLane.iStart, slLeftLane.iEnd, cv::Scalar(255, 255, 255), 2); + if(startLineCounter_ > 0) { + for (auto point: image_points) { + cv::circle(debug_image,point,2,cv::Scalar(0,255,0),3); + } } #endif #ifdef DRAW_DEBUG @@ -78,7 +100,7 @@ bool StreetCrossingDetection::findStartline() { cv::waitKey(1); #endif #ifdef PUBLISH_DEBUG - debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debugImage).toImageMsg()); + debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debug_image).toImageMsg()); #endif // test array of lines From 4c104d8a97c614a758da06a070709ac026f698a5 Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Wed, 11 Oct 2017 10:22:46 +0200 Subject: [PATCH 49/94] Add homography subscription to warp node, some minor compile flags in line processing operations --- CMakeLists.txt | 4 +- .../common_image_operations.h | 7 ++++ .../drive_ros_image_recognition/warp_image.h | 10 +++-- src/warp_image.cpp | 41 +++++++++++++++++-- 4 files changed, 53 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d0a21c1..0c10e1b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -115,7 +115,7 @@ add_executable(warp_image_node src/warp_image.cpp ) -add_dependencies(warp_image_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(warp_image_node ${drive_ros_msgs_generate_messages_cpp} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(warp_image_node ${catkin_LIBRARIES} @@ -135,7 +135,7 @@ add_library(warp_image_nodelet src/warp_image.cpp ) -add_dependencies(warp_image_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(warp_image_nodelet ${drive_ros_msgs_generate_messages_cpp} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(warp_image_nodelet ${catkin_LIBRARIES} diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 055e2fa..44e0a14 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -8,6 +8,7 @@ #include #include #include +#include typedef boost::shared_ptr CvImagePtr; @@ -425,8 +426,10 @@ class ImageOperator { ) { std::vector image_points; std::vector image_widths; +#ifdef DRAW_DEBUG cv::Mat debug_image; cv::cvtColor(current_image, debug_image, CV_GRAY2RGB); +#endif int search_steps = 6; int pixel_step = 50; @@ -439,7 +442,9 @@ class ImageOperator { lines[it].iStart = cv::Point(it*pixel_step, 0); lines[it].iEnd = cv::Point(it*pixel_step, current_image.rows); } +#ifdef DRAW_DEBUG cv::line(debug_image, lines[it].iStart, lines[it].iEnd, cv::Scalar(255,255,255)); +#endif findByLineSearch(lines[it], current_image, search_dir, @@ -447,9 +452,11 @@ class ImageOperator { image_points, image_widths ); +#ifdef DRAW_DEBUG for (auto point: image_points) { cv::circle(debug_image,point,2,cv::Scalar(0,255,0),2); } +#endif } #ifdef DRAW_DEBUG if (search_meth == search_method::sobel) { diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 1ccbbae..e1dcfb1 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -18,15 +18,13 @@ class WarpContent { WarpContent(const ros::NodeHandle &nh, const ros::NodeHandle& pnh); ~WarpContent(); bool init(); + void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in); private: void world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg); ros::NodeHandle pnh_; ros::NodeHandle nh_; cv::Mat current_image_; // needs two components: camera model and homography for transformation - // homography matrices for transformation - cv::Mat world2cam_; - cv::Mat cam2world_; // camera model matrix and distortion coefficients cv::Mat cam_mat_; cv::Mat dist_coeffs_; @@ -39,6 +37,12 @@ class WarpContent { ros::ServiceServer imageToWorldServer_; // todo: moving to camera model subscription image_geometry::PinholeCameraModel cam_model_; + + // homography components + ros::Subscriber homography_params_sub_; + cv::Mat world2cam_; + cv::Mat cam2world_; + bool homo_received_; }; class WarpImageNodelet : public nodelet::Nodelet { diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 7773989..7a80257 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -7,15 +7,17 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): nh_(nh), pnh_(pnh), current_image_(), - world2cam_(3,3,CV_64F,cv::Scalar(0.0)), - cam2world_(3,3,CV_64F,cv::Scalar(0.0)), cam_mat_(3,3,CV_64F,cv::Scalar(0.0)), dist_coeffs_(8,1,CV_64F,cv::Scalar(0.0)), it_(pnh), cam_sub_(), worldToImageServer_(), imageToWorldServer_(), - cam_model_() + cam_model_(), + homography_params_sub_(), + cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), + world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), + homo_received_(false) { img_pub_ = it_.advertise("warped_out", 1); undistort_pub_ = it_.advertise("undistort_out", 1); @@ -33,11 +35,18 @@ bool WarpContent::init() { // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); + + homography_params_sub_ = pnh_.subscribe("homography_in", 1, &WarpContent::homography_callback, this); return true; } void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) { + if (!homo_received_) { + ROS_WARN_THROTTLE(10, "Homography callback not yet received, waiting"); + return; + } + try { // copy @@ -66,10 +75,34 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); // flag ensures that we directly use the matrix, as it is done in LMS -// cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); + cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); // img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } +// bind cannot resolve to references +void WarpContent::homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in) { + if (!homo_received_) + homo_received_ = true; + + ROS_ASSERT(homo_in->cam2world.layout.dim[0].size == 3 && homo_in->cam2world.layout.dim[1].size == 3 ); + cam2world_ = cv::Mat::zeros(3,3,CV_64FC1); + int k=0; + for (int i=0; icam2world.layout.dim[0].size; i++){ + for (int j=0; jcam2world.layout.dim[1].size; j++){ + cam2world_.at(i,j) = homo_in->cam2world.data[k++]; + } + } + + ROS_ASSERT(homo_in->world2cam.layout.dim[0].size == 3 && homo_in->world2cam.layout.dim[1].size == 3 ); + world2cam_ = cv::Mat::zeros(3,3,CV_64FC1); + k=0; + for (int i=0; iworld2cam.layout.dim[0].size; i++){ + for (int j=0; jworld2cam.layout.dim[1].size; j++){ + world2cam_.at(i,j) = homo_in->world2cam.data[k++]; + } + } +} + void WarpImageNodelet::onInit() { my_content_.reset(new WarpContent(getNodeHandle(),ros::NodeHandle(getPrivateNodeHandle()))); From ce001cd13413ad4fae2a69302a66cd0445574db1 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Fri, 13 Oct 2017 02:00:12 +0200 Subject: [PATCH 50/94] Fix launch files (install paths and removed homography yaml references) --- launch/road_detection.launch | 4 +--- launch/road_detection_debug.launch | 4 +--- launch/road_detection_nodelet.launch | 8 ++------ launch/street_crossing.launch | 2 -- launch/street_crossing_nodelet.launch | 4 ---- launch/warp_image.launch | 1 - launch/warp_image_nodelet.launch | 2 -- 7 files changed, 4 insertions(+), 21 deletions(-) diff --git a/launch/road_detection.launch b/launch/road_detection.launch index ed02187..bc41ed7 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -7,8 +7,6 @@ - - - + diff --git a/launch/road_detection_debug.launch b/launch/road_detection_debug.launch index ed02187..bc41ed7 100644 --- a/launch/road_detection_debug.launch +++ b/launch/road_detection_debug.launch @@ -7,8 +7,6 @@ - - - + diff --git a/launch/road_detection_nodelet.launch b/launch/road_detection_nodelet.launch index fc3c271..2b08469 100644 --- a/launch/road_detection_nodelet.launch +++ b/launch/road_detection_nodelet.launch @@ -13,9 +13,7 @@ - - - + @@ -26,9 +24,7 @@ - - - + diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch index 92d582d..10a7269 100644 --- a/launch/street_crossing.launch +++ b/launch/street_crossing.launch @@ -3,7 +3,5 @@ - - diff --git a/launch/street_crossing_nodelet.launch b/launch/street_crossing_nodelet.launch index 4f5b948..761270c 100644 --- a/launch/street_crossing_nodelet.launch +++ b/launch/street_crossing_nodelet.launch @@ -8,8 +8,6 @@ - - @@ -18,8 +16,6 @@ - - diff --git a/launch/warp_image.launch b/launch/warp_image.launch index fc1b5f4..eea7d51 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -4,7 +4,6 @@ - diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch index 36239d3..87b711b 100644 --- a/launch/warp_image_nodelet.launch +++ b/launch/warp_image_nodelet.launch @@ -10,7 +10,6 @@ - @@ -21,7 +20,6 @@ - From 98dd4cbb7071134eb3e50d566ff64c7f7e508923 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sat, 14 Oct 2017 05:51:02 +0200 Subject: [PATCH 51/94] Remove yaml homography loading, add homography subscription to imaging nodes. Todo: wait for first callback, launch files are still broken probably, also does not compute cam2world correctly yet --- .../common_image_operations.h | 146 +++++++++--------- .../drive_ros_image_recognition/warp_image.h | 1 - launch/road_detection.launch | 2 + src/road_detection.cpp | 2 +- src/street_crossing.cpp | 2 +- src/warp_image.cpp | 35 +---- 6 files changed, 77 insertions(+), 111 deletions(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 44e0a14..9b460c0 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -36,32 +36,34 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) return cv_ptr; } - namespace drive_ros_image_recognition { -inline void cam_info_sub(const sensor_msgs::CameraInfoConstPtr& incoming_cam_info, image_geometry::PinholeCameraModel& cam_model) { - cam_model.fromCameraInfo(incoming_cam_info); -} - -inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const std::string mat_param) { - // retrieve world2map and map2world homography matrices - std::vector temp_vals(9); - - if (!pnh.getParam("homography_matrix/"+mat_param, temp_vals)) { - ROS_ERROR("Unable to load homography matrix %s from configuration file!", mat_param.c_str()); - return false; - } - if (temp_vals.size() != 9) { - ROS_ERROR("Retreived homography matrix %s does not have 9 values", mat_param.c_str()); - return false; +inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in, cv::Mat& cam2world, cv::Mat& world2cam, + bool& homography_received) { + if (!homography_received) + homography_received = true; + + ROS_ASSERT(homo_in->cam2world.layout.dim[0].size == 3 && homo_in->cam2world.layout.dim[1].size == 3 ); + cam2world = cv::Mat::zeros(3,3,CV_64FC1); + int k=0; + for (int i=0; icam2world.layout.dim[0].size; i++){ + for (int j=0; jcam2world.layout.dim[1].size; j++){ + cam2world.at(i,j) = homo_in->cam2world.data[k++]; + } } - // update values - mat = cv::Mat::zeros(3,3,CV_64F); - for(unsigned i=0; i < temp_vals.size(); i++) { - mat.at(i) = temp_vals[i]; + + ROS_ASSERT(homo_in->world2cam.layout.dim[0].size == 3 && homo_in->world2cam.layout.dim[1].size == 3 ); + world2cam = cv::Mat::zeros(3,3,CV_64FC1); + k=0; + for (int i=0; iworld2cam.layout.dim[0].size; i++){ + for (int j=0; jworld2cam.layout.dim[1].size; j++){ + world2cam.at(i,j) = homo_in->world2cam.data[k++]; + } } - ROS_DEBUG_STREAM("Paramater matrix loaded as: "<("/camera1/camera_info", 1, + cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); + + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + cam2world_, world2cam_, homography_received_)); other.cam_info_sub_.shutdown(); return *this; } @@ -122,25 +126,19 @@ class TransformHelper { tf_listener_() { cam2world_ = helper.cam2world_; -#ifdef USE_WORLD2CAM_HOMOGRAPHY world2cam_ = helper.world2cam_; -#endif cam_model_ = helper.cam_model_; - ros::NodeHandle pnh("~"); - cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, + cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + cam2world_, world2cam_, homography_received_)); } - TransformHelper(const cv::Mat& cam2world, const image_geometry::PinholeCameraModel& cam_model - #ifdef USE_WORLD2CAM_HOMOGRAPHY - const cv::Mat& world2cam - #endif - ): - cam2world_(cam2world), - #ifdef USE_WORLD2CAM_HOMOGRAPHY - world2cam_(world2cam), - #endif + TransformHelper(const image_geometry::PinholeCameraModel& cam_model): + cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), + world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), cam_model_(cam_model), tf_listener_(), cam_info_sub_() @@ -150,19 +148,14 @@ class TransformHelper { ~TransformHelper() { } - bool init(ros::NodeHandle& pnh) { -#ifdef USE_WORLD2CAM_HOMOGRAPHY - if (!getHomographyMatParam(pnh, world2cam_, "world2cam")) - return false; -#endif - - if (!getHomographyMatParam(pnh, cam2world_, "cam2world")) - return false; - + bool init() { // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated - cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, + cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + cam2world_, world2cam_, homography_received_)); } void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { @@ -171,16 +164,15 @@ class TransformHelper { bool worldToImage(const cv::Point3f& world_point, cv::Point& image_point) { -#ifdef USE_WORLD2CAM_HOMOGRAPHY - // using homography instead of camera model - cv::perspectiveTransform(world_point, image_point, world2cam_); - if (point_world.rows != 1 || point_world.cols != 1) { - ROS_WARN("Point transformed to image dimensions has invalid dimensions"); - return false; - } -#else - image_point = cam_model_.project3dToPixel(world_point); -#endif + // transform to image point and then repeat homography transform + cv::Point2d pixel_coords = cam_model_.project3dToPixel(world_point); + cv::Mat mat_pixel_coords(1,1,CV_64FC2,cv::Scalar(0.0)); + mat_pixel_coords.at(0,0) = pixel_coords.x; + mat_pixel_coords.at(0,1) = pixel_coords.y; + cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); + cv::perspectiveTransform(mat_pixel_coords, mat_image_point, world2cam_); + // todo: check if we lose precision here + image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); return true; } @@ -189,14 +181,16 @@ class TransformHelper { } bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point) { - cv::Mat mat_point_image(1,1,CV_64FC2); + cv::Mat mat_point_image(1,1,CV_64FC2,cv::Scalar(0.0)); mat_point_image.at(0,0) = image_point.x; mat_point_image.at(0,1) = image_point.y; - cv::Mat mat_point_world(1,1,CV_64FC2); + cv::Mat mat_point_world(1,1,CV_64FC2,cv::Scalar(0.0)); cv::perspectiveTransform(mat_point_image, mat_point_world, cam2world_); cv::Point2f cam_point(mat_point_world.at(0,0),mat_point_world.at(0,1)); - // todo: check if it is valid to assume the z depth if we used homography before + // this returns a point with z=1, the pixel is on a ray from origin to this point + // todo: set correct camera height and use it to calculate x and y of the world point + double cam_height = 10.0; cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); geometry_msgs::PointStamped camera_point; @@ -225,25 +219,23 @@ class TransformHelper { cam2world_ = cam2world; } + void setworld2camMat(const cv::Mat& world2cam) { + world2cam_ = world2cam; + } + // to subscribe to camerainfo messages image_geometry::PinholeCameraModel& getCameraModelReference() { return cam_model_; } -#ifdef USE_WORLD2CAM_HOMOGRAPHY - void setworld2camMat(const cv::Mat& world2cam) { - world2cam_ = world2cam; - } -#endif - private: image_geometry::PinholeCameraModel cam_model_; + bool homography_received_; cv::Mat cam2world_; - tf::TransformListener tf_listener_; -#ifdef USE_WORLD2CAM_HOMOGRAPHY cv::Mat world2cam_; -#endif + tf::TransformListener tf_listener_; ros::Subscriber cam_info_sub_; + ros::Subscriber homography_sub_; }; // class TransformHelper class ImageOperator { diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index e1dcfb1..52d134f 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -18,7 +18,6 @@ class WarpContent { WarpContent(const ros::NodeHandle &nh, const ros::NodeHandle& pnh); ~WarpContent(); bool init(); - void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in); private: void world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg); ros::NodeHandle pnh_; diff --git a/launch/road_detection.launch b/launch/road_detection.launch index bc41ed7..1dd61cd 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -1,10 +1,12 @@ + + diff --git a/src/road_detection.cpp b/src/road_detection.cpp index 4dac63e..59102a4 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -80,7 +80,7 @@ bool RoadDetection::init() { ROS_WARN_STREAM("Unable to load 'useWeights' parameter, using default: "<("homography_in", 1, + boost::bind(homography_callback, _1, + cam2world_, world2cam_, homo_received_)); return true; } @@ -79,30 +76,6 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, // img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } -// bind cannot resolve to references -void WarpContent::homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in) { - if (!homo_received_) - homo_received_ = true; - - ROS_ASSERT(homo_in->cam2world.layout.dim[0].size == 3 && homo_in->cam2world.layout.dim[1].size == 3 ); - cam2world_ = cv::Mat::zeros(3,3,CV_64FC1); - int k=0; - for (int i=0; icam2world.layout.dim[0].size; i++){ - for (int j=0; jcam2world.layout.dim[1].size; j++){ - cam2world_.at(i,j) = homo_in->cam2world.data[k++]; - } - } - - ROS_ASSERT(homo_in->world2cam.layout.dim[0].size == 3 && homo_in->world2cam.layout.dim[1].size == 3 ); - world2cam_ = cv::Mat::zeros(3,3,CV_64FC1); - k=0; - for (int i=0; iworld2cam.layout.dim[0].size; i++){ - for (int j=0; jworld2cam.layout.dim[1].size; j++){ - world2cam_.at(i,j) = homo_in->world2cam.data[k++]; - } - } -} - void WarpImageNodelet::onInit() { my_content_.reset(new WarpContent(getNodeHandle(),ros::NodeHandle(getPrivateNodeHandle()))); From 8b10a7f2e4fff25f437bade08ad8147b883d030b Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 14 Oct 2017 12:13:34 +0200 Subject: [PATCH 52/94] updated camera_info topic name; please redownload your bags to get updated bags with new naming --- launch/road_detection.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/road_detection.launch b/launch/road_detection.launch index 1dd61cd..9d84c72 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -1,7 +1,7 @@ - + From 92340cc31edc92e113e230a3be1347f2f92dc2a5 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 14 Oct 2017 12:17:16 +0200 Subject: [PATCH 53/94] fix launch file --- launch/road_detection.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/road_detection.launch b/launch/road_detection.launch index 9d84c72..e7868b1 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -6,7 +6,7 @@ - + From 8d5b5c435c23ae4346b5be7f73c506aa93cc26b2 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Mon, 16 Oct 2017 01:17:15 +0200 Subject: [PATCH 54/94] Reorganized and fixed launch files, fixed missing references in subscription, homography scaling added --- CMakeLists.txt | 4 +- .../common_image_operations.h | 5 +- launch/road_detection.launch | 41 ++++++++-- launch/road_detection_debug.launch | 12 --- launch/road_detection_nodelet.launch | 31 ------- launch/street_crossing.launch | 24 +++++- launch/street_crossing_nodelet.launch | 22 ----- launch/warp_image.launch | 36 +++++++-- launch/warp_image_nodelet.launch | 29 ------- src/road_detection.cpp | 2 +- src/warp_image.cpp | 80 ++++++++++++++++--- 11 files changed, 161 insertions(+), 125 deletions(-) delete mode 100644 launch/road_detection_debug.launch delete mode 100644 launch/road_detection_nodelet.launch delete mode 100644 launch/street_crossing_nodelet.launch delete mode 100644 launch/warp_image_nodelet.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c10e1b..10d9dad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -207,11 +207,11 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch FILES_MATCHING PATTERN "*.launch" ) install(DIRECTORY config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config FILES_MATCHING PATTERN "*.yaml" ) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 9b460c0..06e2570 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -116,7 +116,7 @@ class TransformHelper { homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, - cam2world_, world2cam_, homography_received_)); + std::ref(cam2world_), std::ref(world2cam_), std::ref(homography_received_))); other.cam_info_sub_.shutdown(); return *this; } @@ -192,6 +192,9 @@ class TransformHelper { // todo: set correct camera height and use it to calculate x and y of the world point double cam_height = 10.0; cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); + cam_ray_point.x = cam_ray_point.x*cam_height; + cam_ray_point.y = cam_ray_point.y*cam_height; + cam_ray_point.z = cam_height; geometry_msgs::PointStamped camera_point; camera_point.header.frame_id = cam_model_.tfFrame(); diff --git a/launch/road_detection.launch b/launch/road_detection.launch index e7868b1..712fef6 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -2,13 +2,38 @@ + + + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/road_detection_debug.launch b/launch/road_detection_debug.launch deleted file mode 100644 index bc41ed7..0000000 --- a/launch/road_detection_debug.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - diff --git a/launch/road_detection_nodelet.launch b/launch/road_detection_nodelet.launch deleted file mode 100644 index 2b08469..0000000 --- a/launch/road_detection_nodelet.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch index 10a7269..58778b5 100644 --- a/launch/street_crossing.launch +++ b/launch/street_crossing.launch @@ -1,7 +1,25 @@ + + + - - - + + + + + + + + + + + + + + + + + + diff --git a/launch/street_crossing_nodelet.launch b/launch/street_crossing_nodelet.launch deleted file mode 100644 index 761270c..0000000 --- a/launch/street_crossing_nodelet.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/warp_image.launch b/launch/warp_image.launch index eea7d51..50d6d82 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -2,11 +2,35 @@ + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch deleted file mode 100644 index 87b711b..0000000 --- a/launch/warp_image_nodelet.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/road_detection.cpp b/src/road_detection.cpp index 59102a4..0c4d2b1 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -20,7 +20,7 @@ RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh translateEnvironment_(false), useWeights_(false), line_output_pub_(), - it_(pnh), + it_(nh), #ifdef PUBLISH_DEBUG debug_img_pub_(), detected_points_pub_(), diff --git a/src/warp_image.cpp b/src/warp_image.cpp index caf923b..4b0972a 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -9,7 +9,7 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): current_image_(), cam_mat_(3,3,CV_64F,cv::Scalar(0.0)), dist_coeffs_(8,1,CV_64F,cv::Scalar(0.0)), - it_(pnh), + it_(nh), cam_sub_(), worldToImageServer_(), imageToWorldServer_(), @@ -27,13 +27,11 @@ WarpContent::~WarpContent() { } bool WarpContent::init() { + homography_params_sub_ = nh_.subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + std::ref(cam2world_), std::ref(world2cam_), std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); - -// homography_params_sub_ = pnh_.subscribe("homography_in", 1, &WarpContent::homography_callback, this); - homography_params_sub_ = pnh_.subscribe("homography_in", 1, - boost::bind(homography_callback, _1, - cam2world_, world2cam_, homo_received_)); return true; } @@ -58,9 +56,11 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, cam_model_.fromCameraInfo(info_msg); // optionally: scale homography, as image is too small -// cv::Rect roi(cv::Point(current_image_.cols/2-(410/2),0),cv::Point(current_image_.cols/2+(410/2),current_image_.rows)); +// cv::Rect roi(cv::Point(current_image_.cols/2-(512/2),0),cv::Point(current_image_.cols/2+(512/2),current_image_.rows)); // current_image_ = current_image_(roi); -// cv::Mat S = cv::Mat::eye(3,3,CV_64F); + cv::Mat S = cv::Mat::eye(3,3,CV_64F); + S.at(0,0) = 0.007; + S.at(1,1) = 0.007; // S.at(0,0) = 410/current_image_.rows; // S.at(1,1) = 752/current_image_.cols; @@ -72,8 +72,68 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); // flag ensures that we directly use the matrix, as it is done in LMS - cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); -// img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); +// cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); +// cv::Mat output_mat; +// // Choose top-view image size +// cv::Size topViewSize = cv::Size(512, 512); + +// // Choose corner points (in real-world coordinates) +// std::vector coordinates; +// coordinates.emplace_back(0, -1.500); +// coordinates.emplace_back(0, 1.500); +// coordinates.emplace_back(3.000, -1.500); +// coordinates.emplace_back(3.000, 1.500); + +// std::vector pixels; +// pixels.emplace_back(0, topViewSize.height); +// pixels.emplace_back(0, 0); +// pixels.emplace_back(topViewSize.width, topViewSize.height); +// pixels.emplace_back(topViewSize.width, 0); + +// cv::Mat H = cv::findHomography(pixels, coordinates); +// cv::Mat trafo_mat = world2cam_ * H; + +// cv::Size topViewSize = current_image_.size(); + +// std::vector coordinates; +// coordinates.emplace_back(0, -1.500); +// coordinates.emplace_back(0, 1.500); +// coordinates.emplace_back(3.000, -1.500); +// coordinates.emplace_back(3.000, 1.500); + +// std::vector pixels; +// pixels.emplace_back(0, topViewSize.height); +// pixels.emplace_back(0, 0); +// pixels.emplace_back(topViewSize.width, topViewSize.height); +// pixels.emplace_back(topViewSize.width, 0); + +// cv::Mat H = cv::findHomography(pixels, coordinates); + + +// cv::Mat S = cv::Mat::eye(3,3,CV_64F); +// S.at(0,0) = 410/current_image_.rows; +// S.at(1,1) = 752/current_image_.cols; + cv::Mat output_mat(current_image_.size(),CV_8UC1,cv::Scalar(0)); + +// for(int i = 0; i < current_image_.rows; i++) +// { +// const double* Mi = current_image_.ptr(i); +// for(int j = 0; j < current_image_.cols; j++) { +// float a = i * cam2world_.data[0] + j * cam2world_.data[1] + cam2world_.data[2]; +// float b = i * cam2world_.data[3] + j * cam2world_.data[4] + cam2world_.data[5]; +// float c = i * cam2world_.data[6] + j * cam2world_.data[7] + cam2world_.data[8]; +// int x = a / c; +// int y = b / c; +//// ROS_INFO_STREAM("x: "<(x,y) = Mi[j]; +// } +// } + + cv::warpPerspective(undistorted_mat, output_mat, world2cam_*S, current_image_.size(), cv::WARP_INVERSE_MAP); + cv::namedWindow("Homographied",CV_WINDOW_NORMAL); + cv::imshow("Homographied",output_mat); + cv::waitKey(1); + img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, output_mat).toImageMsg()); } void WarpImageNodelet::onInit() From 30c0dacb6cf5e3f3a8b562f5d84496f17e0ac899 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sun, 22 Oct 2017 23:46:47 +0200 Subject: [PATCH 55/94] Fixed homography scaling, removed unneeded dependencies --- CMakeLists.txt | 4 +- config/homography.yaml | 2 + config/homography_matrix.yaml | 3 - .../common_image_operations.h | 40 ++++++- .../drive_ros_image_recognition/warp_image.h | 2 + launch/road_detection.launch | 5 + launch/warp_image.launch | 2 + package.xml | 2 - scripts/publish_road.sh | 0 src/road_detection.cpp | 2 +- src/warp_image.cpp | 105 ++++++------------ 11 files changed, 81 insertions(+), 86 deletions(-) create mode 100644 config/homography.yaml delete mode 100644 config/homography_matrix.yaml mode change 100755 => 100644 scripts/publish_road.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 10d9dad..4d408a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(drive_ros_image_recognition) SET(CMAKE_BUILD_TYPE Debug) # enable to compile with OpenCV window displays -add_definitions(-DDRAW_DEBUG) +#add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images #add_definitions(-DPUBLISH_DEBUG) # enable to use World2Cam homography @@ -23,10 +23,8 @@ find_package(catkin REQUIRED COMPONENTS drive_ros_msgs dynamic_reconfigure geometry_msgs - message_generation nodelet tf - message_filters ) #include Boost for lane line processing diff --git a/config/homography.yaml b/config/homography.yaml new file mode 100644 index 0000000..66b2425 --- /dev/null +++ b/config/homography.yaml @@ -0,0 +1,2 @@ +world_size: [5.0, 5.0] +image_size: [512, 512] diff --git a/config/homography_matrix.yaml b/config/homography_matrix.yaml deleted file mode 100644 index 19963a5..0000000 --- a/config/homography_matrix.yaml +++ /dev/null @@ -1,3 +0,0 @@ -homography_matrix: - world2cam: [-1076.85,677.839,-97.8148,-543.74,26.5117,-240.371,-1.65943,0.0135395,-0.154656] - cam2world: [-4.01363e-06,0.00049125,-0.760979,0.00149399,2.0045e-05,-0.976056,0.000173859,-0.00526928,1.61375] diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 06e2570..682c7bd 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -39,7 +39,7 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) namespace drive_ros_image_recognition { inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in, cv::Mat& cam2world, cv::Mat& world2cam, - bool& homography_received) { + cv::Mat& scaling_mat, bool& homography_received) { if (!homography_received) homography_received = true; @@ -60,6 +60,8 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i world2cam.at(i,j) = homo_in->world2cam.data[k++]; } } + + scaling_mat = world2cam*scaling_mat; } inline void cam_info_sub(const sensor_msgs::CameraInfoConstPtr& incoming_cam_info, image_geometry::PinholeCameraModel& cam_model) { @@ -114,9 +116,10 @@ class TransformHelper { boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); - homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, - boost::bind(homography_callback, _1, - std::ref(cam2world_), std::ref(world2cam_), std::ref(homography_received_))); +// homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, +// boost::bind(homography_callback, _1, +// std::ref(cam2world_), std::ref(world2cam_), +// std::ref(scaling_mat_), std::ref(homography_received_))); other.cam_info_sub_.shutdown(); return *this; } @@ -133,12 +136,15 @@ class TransformHelper { getCameraModelReference()) ); homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, - cam2world_, world2cam_, homography_received_)); + std::ref(cam2world_), std::ref(world2cam_), + std::ref(scaling_mat_), std::ref(homography_received_))); } TransformHelper(const image_geometry::PinholeCameraModel& cam_model): cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), + scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), + transformed_size_(0,0), cam_model_(cam_model), tf_listener_(), cam_info_sub_() @@ -149,13 +155,33 @@ class TransformHelper { } bool init() { + ros::NodeHandle pnh = ros::NodeHandle("~"); + std::vector world_size; + if(!pnh.getParam("world_size", world_size)) { + ROS_ERROR("Unable to load parameter world_size!"); + return false; + } + ROS_ASSERT(world_size.size() == 2); + std::vector image_size; + if(!pnh.getParam("image_size", image_size)) { + ROS_ERROR("Unable to load parameter world_size!"); + return false; + } + ROS_ASSERT(image_size.size() == 2); + transformed_size_ = cv::Size(image_size[0],image_size[1]); + scaling_mat_.at(0,0) = world_size[0]/image_size[0]; + scaling_mat_.at(1,1) = -world_size[1]/image_size[1]; + scaling_mat_.at(1,2) = world_size[1]/2; + scaling_mat_.at(2,2) = 1.0; + // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, - cam2world_, world2cam_, homography_received_)); + std::ref(cam2world_), std::ref(world2cam_), + std::ref(scaling_mat_), std::ref(homography_received_))); } void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { @@ -236,6 +262,8 @@ class TransformHelper { bool homography_received_; cv::Mat cam2world_; cv::Mat world2cam_; + cv::Mat scaling_mat_; + cv::Size transformed_size_; tf::TransformListener tf_listener_; ros::Subscriber cam_info_sub_; ros::Subscriber homography_sub_; diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 52d134f..6e702a8 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -41,6 +41,8 @@ class WarpContent { ros::Subscriber homography_params_sub_; cv::Mat world2cam_; cv::Mat cam2world_; + cv::Mat scaling_mat_; + cv::Size transformed_size_; bool homo_received_; }; diff --git a/launch/road_detection.launch b/launch/road_detection.launch index 712fef6..cb4d739 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -3,6 +3,7 @@ + @@ -16,9 +17,11 @@ + + @@ -31,9 +34,11 @@ + + diff --git a/launch/warp_image.launch b/launch/warp_image.launch index 50d6d82..b1a8e8a 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -17,6 +17,7 @@ + @@ -31,6 +32,7 @@ + diff --git a/package.xml b/package.xml index b10de34..1de3705 100644 --- a/package.xml +++ b/package.xml @@ -50,7 +50,6 @@ nodelet geometry_msgs tf - message_filters drive_ros_msgs cv_bridge @@ -63,7 +62,6 @@ nodelet geometry_msgs tf - message_filters drive_ros_msgs diff --git a/scripts/publish_road.sh b/scripts/publish_road.sh old mode 100755 new mode 100644 diff --git a/src/road_detection.cpp b/src/road_detection.cpp index 0c4d2b1..67d57e6 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -94,7 +94,7 @@ bool RoadDetection::init() { img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); - line_output_pub_ = pnh_.advertise("line_out",10); + line_output_pub_ = nh_.advertise("line_out",10); #ifdef PUBLISH_DEBUG debug_img_pub_ = it_.advertise("debug_image_out", 10); diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 4b0972a..2ec3d6d 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -17,6 +17,8 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): homography_params_sub_(), cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), + scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), + transformed_size_(0,0), homo_received_(false) { img_pub_ = it_.advertise("warped_out", 1); @@ -27,9 +29,33 @@ WarpContent::~WarpContent() { } bool WarpContent::init() { + std::vector world_size; + double test; + ROS_INFO_STREAM("PNH namespace: "< image_size; + if(!pnh_.getParam("image_size", image_size)) { + ROS_ERROR("Unable to load parameter image_size!"); + return false; + } + ROS_ASSERT(image_size.size() == 2); + transformed_size_ = cv::Size(image_size[0],image_size[1]); + scaling_mat_.at(0,0) = world_size[0]/image_size[0]; + scaling_mat_.at(1,1) = -world_size[1]/image_size[1]; + scaling_mat_.at(1,2) = world_size[1]/2; + scaling_mat_.at(2,2) = 1.0; + ROS_INFO_STREAM("Calculated world2mat scaling matrix: "<("homography_in", 1, boost::bind(homography_callback, _1, - std::ref(cam2world_), std::ref(world2cam_), std::ref(homo_received_))); + std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), + std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); return true; @@ -53,86 +79,23 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, return; } - cam_model_.fromCameraInfo(info_msg); - - // optionally: scale homography, as image is too small -// cv::Rect roi(cv::Point(current_image_.cols/2-(512/2),0),cv::Point(current_image_.cols/2+(512/2),current_image_.rows)); -// current_image_ = current_image_(roi); - cv::Mat S = cv::Mat::eye(3,3,CV_64F); - S.at(0,0) = 0.007; - S.at(1,1) = 0.007; -// S.at(0,0) = 410/current_image_.rows; -// S.at(1,1) = 752/current_image_.cols; + cam_model_.fromCameraInfo(info_msg); // undistort and apply homography transformation cv::Mat undistorted_mat; cv::undistort(current_image_, undistorted_mat, cam_model_.fullIntrinsicMatrix(), cam_model_.distortionCoeffs()); undistort_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); - // opionally: apply scaled homography - // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); - - // flag ensures that we directly use the matrix, as it is done in LMS -// cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); -// cv::Mat output_mat; -// // Choose top-view image size -// cv::Size topViewSize = cv::Size(512, 512); - -// // Choose corner points (in real-world coordinates) -// std::vector coordinates; -// coordinates.emplace_back(0, -1.500); -// coordinates.emplace_back(0, 1.500); -// coordinates.emplace_back(3.000, -1.500); -// coordinates.emplace_back(3.000, 1.500); - -// std::vector pixels; -// pixels.emplace_back(0, topViewSize.height); -// pixels.emplace_back(0, 0); -// pixels.emplace_back(topViewSize.width, topViewSize.height); -// pixels.emplace_back(topViewSize.width, 0); -// cv::Mat H = cv::findHomography(pixels, coordinates); -// cv::Mat trafo_mat = world2cam_ * H; +// cv::Mat topView2cam = world2cam_ * pixels_to_meters; -// cv::Size topViewSize = current_image_.size(); - -// std::vector coordinates; -// coordinates.emplace_back(0, -1.500); -// coordinates.emplace_back(0, 1.500); -// coordinates.emplace_back(3.000, -1.500); -// coordinates.emplace_back(3.000, 1.500); - -// std::vector pixels; -// pixels.emplace_back(0, topViewSize.height); -// pixels.emplace_back(0, 0); -// pixels.emplace_back(topViewSize.width, topViewSize.height); -// pixels.emplace_back(topViewSize.width, 0); - -// cv::Mat H = cv::findHomography(pixels, coordinates); - - -// cv::Mat S = cv::Mat::eye(3,3,CV_64F); -// S.at(0,0) = 410/current_image_.rows; -// S.at(1,1) = 752/current_image_.cols; - cv::Mat output_mat(current_image_.size(),CV_8UC1,cv::Scalar(0)); - -// for(int i = 0; i < current_image_.rows; i++) -// { -// const double* Mi = current_image_.ptr(i); -// for(int j = 0; j < current_image_.cols; j++) { -// float a = i * cam2world_.data[0] + j * cam2world_.data[1] + cam2world_.data[2]; -// float b = i * cam2world_.data[3] + j * cam2world_.data[4] + cam2world_.data[5]; -// float c = i * cam2world_.data[6] + j * cam2world_.data[7] + cam2world_.data[8]; -// int x = a / c; -// int y = b / c; -//// ROS_INFO_STREAM("x: "<(x,y) = Mi[j]; -// } -// } - - cv::warpPerspective(undistorted_mat, output_mat, world2cam_*S, current_image_.size(), cv::WARP_INVERSE_MAP); + // flag ensures that we directly use the matrix, as it is done in LMS + cv::Mat output_mat; + cv::warpPerspective(undistorted_mat, output_mat, scaling_mat_, transformed_size_, cv::WARP_INVERSE_MAP); +#ifdef DRAW_DEBUG cv::namedWindow("Homographied",CV_WINDOW_NORMAL); cv::imshow("Homographied",output_mat); cv::waitKey(1); +#endif img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, output_mat).toImageMsg()); } From c8b1a4eb8df4d657386661d64c51dec87cb0cc46 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Thu, 2 Nov 2017 00:55:26 +0100 Subject: [PATCH 56/94] Fixed transform callbacks, fixed duplicate callbacks due to helper setup, added test for transform callbacks, added some debug callbacks --- CMakeLists.txt | 4 +- .../common_image_operations.h | 203 +++++++++++++----- .../road_detection.h | 7 +- .../drive_ros_image_recognition/warp_image.h | 1 + launch/road_detection.launch | 10 +- src/road_detection.cpp | 73 ++++++- src/warp_image.cpp | 3 +- test/common_image_operations_test.cpp | 37 +++- 8 files changed, 267 insertions(+), 71 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d408a1..18d7f97 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,11 +5,13 @@ project(drive_ros_image_recognition) SET(CMAKE_BUILD_TYPE Debug) # enable to compile with OpenCV window displays -#add_definitions(-DDRAW_DEBUG) +add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images #add_definitions(-DPUBLISH_DEBUG) # enable to use World2Cam homography #add_definitions(-DUSE_WORLD2CAM_HOMOGRAPHY) +# enable publishing of detected points (for rviz display) +#add_definitions(-DPUBLISH_WORLD_POINTS) add_definitions(-std=c++11) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 682c7bd..09e93d7 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -39,7 +39,7 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) namespace drive_ros_image_recognition { inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in, cv::Mat& cam2world, cv::Mat& world2cam, - cv::Mat& scaling_mat, bool& homography_received) { + cv::Mat& scaling_mat, cv::Mat& scaling_mat_inv, bool& homography_received) { if (!homography_received) homography_received = true; @@ -61,11 +61,23 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i } } + ROS_INFO_STREAM("Scaling mat: "<("camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); + boost::bind(&camInfo_callback, _1, + std::ref(cam_model_), + std::ref(camera_model_received_) ) ); + + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + std::ref(cam2world_), std::ref(world2cam_), + std::ref(scaling_mat_), std::ref(scaling_mat_inv_), + std::ref(homography_received_))); -// homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, -// boost::bind(homography_callback, _1, -// std::ref(cam2world_), std::ref(world2cam_), -// std::ref(scaling_mat_), std::ref(homography_received_))); other.cam_info_sub_.shutdown(); + other.homography_sub_.shutdown(); return *this; } @@ -131,23 +157,32 @@ class TransformHelper { cam2world_ = helper.cam2world_; world2cam_ = helper.world2cam_; cam_model_ = helper.cam_model_; + world_frame_ = helper.world_frame_; + camera_frame_ = helper.camera_frame_; cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); + boost::bind(&camInfo_callback, _1, + std::ref(cam_model_), + std::ref(camera_model_received_))); homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), - std::ref(scaling_mat_), std::ref(homography_received_))); + std::ref(scaling_mat_), std::ref(scaling_mat_inv_), + std::ref(homography_received_))); } TransformHelper(const image_geometry::PinholeCameraModel& cam_model): cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), + scaling_mat_inv_(3,3,CV_64FC1,cv::Scalar(0.0)), transformed_size_(0,0), cam_model_(cam_model), tf_listener_(), - cam_info_sub_() + cam_info_sub_(), + camera_model_received_(false), + // temporary solution until have embedded correct frame in camerainfo messages (including the bag files) + camera_frame_(""), + world_frame_("") { } @@ -176,28 +211,71 @@ class TransformHelper { // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); + boost::bind(&camInfo_callback, _1, + std::ref(cam_model_), + std::ref(camera_model_received_) ) ); homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), - std::ref(scaling_mat_), std::ref(homography_received_))); + std::ref(scaling_mat_), std::ref(scaling_mat_inv_), + std::ref(homography_received_))); } void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { cam_model_ = cam_model; } + void setWorldFrame(const std::string& frame) { + if (camera_frame_ != std::string("")) { + try { + ros::Duration timeout(1.0); + tf_listener_.waitForTransform(camera_frame_, frame, + ros::Time::now(), timeout); + } + catch (tf::TransformException& ex) { + ROS_WARN("[TransformHelper] TF exception, unable to find transform to new camera frame %s, will not set it, reason:\n%s", frame.c_str(), ex.what()); + return; + } + } + world_frame_ = frame; + } + + // placeholder until we get updated bags with camera messages + // cannot access PinHoleCameraModel after it has been created, so just use a string instead + void setCameraFrame(const std::string& frame) { + if (world_frame_ != std::string("")) { + try { + ros::Duration timeout(1.0); + tf_listener_.waitForTransform(world_frame_, frame, + ros::Time::now(), timeout); + } + catch (tf::TransformException& ex) { + ROS_WARN("[TransformHelper] TF exception, unable to find transform to new world frame %s, will not set it, reason:\n%s", frame.c_str(), ex.what()); + return; + } + } + camera_frame_ = frame; + } + bool worldToImage(const cv::Point3f& world_point, cv::Point& image_point) { + + if(!homography_received_ || !camera_model_received_) { + if (!homography_received_) + ROS_WARN_STREAM("[WorldToImage] Homography not received yet, skipping world to image transformation requested for world point "<(0,0) = pixel_coords.x; mat_pixel_coords.at(0,1) = pixel_coords.y; cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); - cv::perspectiveTransform(mat_pixel_coords, mat_image_point, world2cam_); - // todo: check if we lose precision here + cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_inv_); image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); return true; } @@ -206,42 +284,61 @@ class TransformHelper { return worldToImage(cv::Point3f(world_point.x, world_point.y, 0.0f), image_point); } - bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point) { - cv::Mat mat_point_image(1,1,CV_64FC2,cv::Scalar(0.0)); - mat_point_image.at(0,0) = image_point.x; - mat_point_image.at(0,1) = image_point.y; - cv::Mat mat_point_world(1,1,CV_64FC2,cv::Scalar(0.0)); - cv::perspectiveTransform(mat_point_image, mat_point_world, cam2world_); - cv::Point2f cam_point(mat_point_world.at(0,0),mat_point_world.at(0,1)); + bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point, std::string target_frame = std::string("/rear_axis_middle_ground"), cv::Mat test_image = cv::Mat()) { + if(!homography_received_ || !camera_model_received_) { + if (!homography_received_) + ROS_WARN_STREAM("[ImageToWorld] Homography not received yet, skipping image to world transformation requested for image point "< obj_corners(1); + obj_corners[0] = cv::Point2f(image_point.x, image_point.y); + std::vector scene_corners(1); + cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_); // this returns a point with z=1, the pixel is on a ray from origin to this point - // todo: set correct camera height and use it to calculate x and y of the world point - double cam_height = 10.0; - cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); - cam_ray_point.x = cam_ray_point.x*cam_height; - cam_ray_point.y = cam_ray_point.y*cam_height; - cam_ray_point.z = cam_height; + cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(scene_corners[0]); + // this gives us a ray defined by two points, now we transform to the fixed frame + // where we can easily define the road plane and intersect the ray with it to get the world point geometry_msgs::PointStamped camera_point; - camera_point.header.frame_id = cam_model_.tfFrame(); + camera_point.header.frame_id = camera_frame_; camera_point.point.x = cam_ray_point.x; camera_point.point.y = cam_ray_point.y; camera_point.point.z = cam_ray_point.z; - geometry_msgs::PointStamped geom_world_point; - + geometry_msgs::PointStamped camera_point_world; + tf::StampedTransform transform; try { - ros::Duration timeout(1.0 / 30); - tf_listener_.waitForTransform(cam_model_.tfFrame(), "tf_front_axis_middle", - ros::Time::now(), timeout); - tf_listener_.transformPoint("tf_front_axis_middle", camera_point, geom_world_point); + tf_listener_.transformPoint(target_frame, camera_point, camera_point_world); + tf_listener_.lookupTransform(target_frame, camera_frame_, + ros::Time::now(), transform); } catch (tf::TransformException& ex) { ROS_WARN("[ImageToWorld] TF exception, unable to transform:\n%s", ex.what()); return false; } - world_point.x = geom_world_point.point.x; - world_point.y = geom_world_point.point.y; - return true; + + // ray-plane intersection (plane is defined by P=[0 0 0] and n=[0 0 1] + tf::Point ray_point_tf; + tf::Vector3 plane_normal(0,0,1); + tf::Vector3 plane_point(0,0,0); + tf::pointMsgToTF(camera_point_world.point, ray_point_tf); + tf::Vector3 ray_dir = ray_point_tf - transform.getOrigin(); + double denom = plane_normal.dot(ray_dir); + + if (std::abs(denom) > 1e-9) { + double scale_axis = (plane_point-transform.getOrigin()).dot(plane_normal)/denom; + tf::Vector3 return_point = transform.getOrigin()+scale_axis*ray_dir; + world_point.x = return_point.x(); + world_point.y = return_point.y(); + return true; + } + else { + ROS_WARN_STREAM("[imageToWorld] Camera ray and ground plane are parallel, unable to transform image point "< pxlPeakWidth*config_.minLineWidthMul && lineWidths[i] < pxlPeakWidth*config_.maxLineWidthMul) { - helper_.imageToWorld(imagePoints[i], wMid); + imageToWorld(imagePoints[i], wMid); foundPoints.push_back(wMid); } } @@ -501,7 +597,6 @@ class ImageOperator { } private: - TransformHelper helper_; LineDetectionConfig config_; }; // class ImageOperator diff --git a/include/drive_ros_image_recognition/road_detection.h b/include/drive_ros_image_recognition/road_detection.h index 75fac14..f6124bc 100644 --- a/include/drive_ros_image_recognition/road_detection.h +++ b/include/drive_ros_image_recognition/road_detection.h @@ -77,11 +77,16 @@ class RoadDetection { dynamic_reconfigure::Server dsrv_server_; dynamic_reconfigure::Server::CallbackType dsrv_cb_; void debugImageCallback(const sensor_msgs::ImageConstPtr& img_in); + void debugDrawFrameCallback(const sensor_msgs::ImageConstPtr& img_in, + const std::string camera_frame, + const std::string draw_frame); void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in); void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); bool find(); void processSearchLine(const SearchLine &line); - TransformHelper transform_helper_; +#ifdef PUBLISH_WORLD_POINTS + ros::Publisher world_point_pub; +#endif ImageOperator image_operator_; public: diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 6e702a8..b15520f 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -42,6 +42,7 @@ class WarpContent { cv::Mat world2cam_; cv::Mat cam2world_; cv::Mat scaling_mat_; + cv::Mat scaling_mat_inv_; cv::Size transformed_size_; bool homo_received_; }; diff --git a/launch/road_detection.launch b/launch/road_detection.launch index cb4d739..839f54b 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -4,6 +4,8 @@ + + @@ -16,8 +18,10 @@ - + + + @@ -33,8 +37,10 @@ - + + + diff --git a/src/road_detection.cpp b/src/road_detection.cpp index 67d57e6..bae5921 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -32,9 +32,11 @@ RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh linesToProcess_(0), current_image_(), road_points_buffer_(), - transform_helper_(), image_operator_(), img_sub_debug_() +#ifdef PUBLISH_WORLD_POINTS + ,world_point_pub() +#endif { } @@ -80,11 +82,28 @@ bool RoadDetection::init() { ROS_WARN_STREAM("Unable to load 'useWeights' parameter, using default: "<("worldPoints", 1000); +#endif + // to synchronize incoming images and the road, we use message filters // img_sub_.reset(new image_transport::SubscriberFilter(it_,"img_in", 1000)); // road_sub_.reset(new message_filters::Subscriber(pnh_,"road_in", 1000)); @@ -93,6 +112,8 @@ bool RoadDetection::init() { // sync_->registerCallback(boost::bind(&RoadDetection::syncCallback, this, _1, _2)); img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); +// img_sub_debug_ = it_.subscribe("img_in", 1000, boost::bind(&RoadDetection::debugDrawFrameCallback, this, +// _1, std::string("/camera_optical"), std::string("/front_axis_middle"))); line_output_pub_ = nh_.advertise("line_out",10); @@ -102,7 +123,6 @@ bool RoadDetection::init() { filtered_points_pub_ = it_.advertise("filtered_points_out", 10); #endif - // todo: we have not decided on an interface for these debug channels yet // debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); // debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); @@ -124,6 +144,39 @@ void RoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth_brightness); } +// draws draw_frame in camera image (if visible) +void RoadDetection::debugDrawFrameCallback(const sensor_msgs::ImageConstPtr& img_in, + const std::string camera_frame, + const std::string draw_frame) { + current_image_ = convertImageMessage(img_in); + + tf::TransformListener tf_listener_; + tf::StampedTransform transform; + try { + ros::Duration timeout(1); + tf_listener_.waitForTransform(camera_frame, draw_frame, + ros::Time::now(), timeout); + tf_listener_.lookupTransform(camera_frame, draw_frame, + ros::Time::now(), transform); + } + catch (tf::TransformException& ex) { + ROS_WARN("[debugDrawFrameCallback] TF exception:\n%s", ex.what()); + return; + } + + tf::Point pt = transform.getOrigin(); + cv::Point3f pt_cv(pt.x(), pt.y(), pt.z()); + cv::Point uv; + image_operator_.worldToImage(pt_cv, uv); + + cv::Mat draw_image; + cv::cvtColor(*current_image_, draw_image, cv::COLOR_GRAY2BGR); + cv::circle(draw_image, uv, 3, CV_RGB(255,0,0), -1); + cv::namedWindow("frame in image", CV_WINDOW_NORMAL); + cv::imshow("frame in image", draw_image); + cv::waitKey(1); +} + void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in){ current_image_ = convertImageMessage(img_in); road_points_buffer_ = road_in->points; @@ -204,14 +257,14 @@ bool RoadDetection::find(){ //check if the part is valid (middle should be always visible) cv::Point l_w_start, l_w_end; - transform_helper_.worldToImage(l.wStart, l.iStart); - transform_helper_.worldToImage(l.wEnd, l.iEnd); + image_operator_.worldToImage(l.wStart, l.iStart); + image_operator_.worldToImage(l.wEnd, l.iEnd); cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); // if the left side is out of the current view, use middle instead if(!img_rect.contains(l_w_start)){ // try middle lane -> should always be in image l.wStart = cv::Point2f(it_mid->x(),it_mid->y()); - transform_helper_.worldToImage(l.wMid, l.iStart); + image_operator_.worldToImage(l.wMid, l.iStart); if(img_rect.contains(l_w_start)){ continue; } @@ -220,8 +273,8 @@ bool RoadDetection::find(){ l.wEnd = cv::Point2f(it_mid->x(),it_mid->y()); } - transform_helper_.worldToImage(l.wStart,l.iStart); - transform_helper_.worldToImage(l.wEnd,l.iEnd); + image_operator_.worldToImage(l.wStart,l.iStart); + image_operator_.worldToImage(l.wEnd,l.iEnd); lines_.push_back(l); for(SearchLine &l:lines_){ processSearchLine(l); @@ -323,7 +376,7 @@ void RoadDetection::processSearchLine(const SearchLine &l) { cv::Mat filtered_points_mat = current_image_->clone(); cv::Point img_point; for (auto point : validPoints) { - transform_helper_.worldToImage(point, img_point); + image_operator_.worldToImage(point, img_point); cv::circle(filtered_points_mat, img_point, 2, cv::Scalar(255)); } #ifdef DRAW_DEBUG diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 2ec3d6d..efeb04a 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -18,6 +18,7 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), + scaling_mat_inv_(3,3,CV_64FC1,cv::Scalar(0.0)), transformed_size_(0,0), homo_received_(false) { @@ -55,7 +56,7 @@ bool WarpContent::init() { homography_params_sub_ = nh_.subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), - std::ref(homo_received_))); + std::ref(scaling_mat_inv_), std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); return true; diff --git a/test/common_image_operations_test.cpp b/test/common_image_operations_test.cpp index 5924139..89cbdf1 100644 --- a/test/common_image_operations_test.cpp +++ b/test/common_image_operations_test.cpp @@ -1,9 +1,42 @@ #include #include "drive_ros_image_recognition/common_image_operations.h" - +#include // Declare test TEST(CommonImageOperations, testCase0) { - EXPECT_EQ(1,1); + ros::NodeHandle pnh("~"); + std::string world_frame("/rear_axis_middle_ground"); + if (!pnh.getParam("world_frame", world_frame)) + ROS_WARN_STREAM("Parameter 'world_frame' not found, using default: "< Date: Fri, 3 Nov 2017 15:31:42 +0100 Subject: [PATCH 57/94] Added option to transform from unwarped image, move to PointStamped in RoadLane callback --- .../common_image_operations.h | 24 +++++++++---- .../road_detection.h | 14 ++++---- src/road_detection.cpp | 36 ++++++++++--------- 3 files changed, 45 insertions(+), 29 deletions(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 09e93d7..c281532 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -105,7 +105,7 @@ class TransformHelper { public: - TransformHelper(): + TransformHelper(bool use_homography = true): tf_listener_(), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), cam_model_(), @@ -119,7 +119,8 @@ class TransformHelper { camera_model_received_(false), // temporary solution until have embedded correct frame in camerainfo messages (including the bag files) camera_frame_(""), - world_frame_("") + world_frame_(""), + use_homography_(use_homography) { } @@ -182,7 +183,8 @@ class TransformHelper { camera_model_received_(false), // temporary solution until have embedded correct frame in camerainfo messages (including the bag files) camera_frame_(""), - world_frame_("") + world_frame_(""), + use_homography_(true) { } @@ -275,8 +277,14 @@ class TransformHelper { mat_pixel_coords.at(0,0) = pixel_coords.x; mat_pixel_coords.at(0,1) = pixel_coords.y; cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); - cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_inv_); - image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); + if (use_homography_) { + cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_inv_); + image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); + } + else { + // nothing to transform if homography has not been applied + image_point = cv::Point(pixel_coords.x, pixel_coords.y); + } return true; } @@ -297,7 +305,10 @@ class TransformHelper { std::vector obj_corners(1); obj_corners[0] = cv::Point2f(image_point.x, image_point.y); std::vector scene_corners(1); - cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_); + if (homography_received_) + cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_); + else + scene_corners[0] = obj_corners[0]; // no homography transforms if we use camera image directly // this returns a point with z=1, the pixel is on a ray from origin to this point cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(scene_corners[0]); @@ -351,6 +362,7 @@ class TransformHelper { private: image_geometry::PinholeCameraModel cam_model_; + bool use_homography_; bool homography_received_; bool camera_model_received_; std::string world_frame_; diff --git a/include/drive_ros_image_recognition/road_detection.h b/include/drive_ros_image_recognition/road_detection.h index f6124bc..f6f2a7d 100644 --- a/include/drive_ros_image_recognition/road_detection.h +++ b/include/drive_ros_image_recognition/road_detection.h @@ -16,6 +16,8 @@ #include #include #include +#include +#include // for multithreading #include @@ -60,10 +62,10 @@ class RoadDetection { image_transport::ImageTransport it_; // road inputs and outputs image_transport::Subscriber img_sub_debug_; -// std::unique_ptr img_sub_; -// std::unique_ptr > road_sub_; -// typedef message_filters::sync_policies::ApproximateTime SyncImageToRoad; -// std::unique_ptr > sync_; + std::unique_ptr img_sub_; + std::unique_ptr > road_sub_; + typedef message_filters::sync_policies::ApproximateTime SyncImageToRoad; + std::unique_ptr > sync_; ros::Publisher line_output_pub_; #ifdef PUBLISH_DEBUG image_transport::Publisher debug_img_pub_; @@ -72,7 +74,7 @@ class RoadDetection { #endif ros::NodeHandle nh_; ros::NodeHandle pnh_; - std::vector road_points_buffer_; + std::vector road_points_buffer_; dynamic_reconfigure::Server dsrv_server_; dynamic_reconfigure::Server::CallbackType dsrv_cb_; @@ -85,7 +87,7 @@ class RoadDetection { bool find(); void processSearchLine(const SearchLine &line); #ifdef PUBLISH_WORLD_POINTS - ros::Publisher world_point_pub; + ros::Publisher world_point_pub_; #endif ImageOperator image_operator_; diff --git a/src/road_detection.cpp b/src/road_detection.cpp index bae5921..848503e 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -32,10 +32,10 @@ RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh linesToProcess_(0), current_image_(), road_points_buffer_(), - image_operator_(), - img_sub_debug_() + image_operator_() +, img_sub_debug_() #ifdef PUBLISH_WORLD_POINTS - ,world_point_pub() + ,world_point_pub_() #endif { } @@ -101,19 +101,20 @@ bool RoadDetection::init() { dsrv_server_.setCallback(dsrv_cb_); #ifdef PUBLISH_WORLD_POINTS - world_point_pub = pnh_.advertise("worldPoints", 1000); + world_point_pub_ = pnh_.advertise("worldPoints", 1000); #endif // to synchronize incoming images and the road, we use message filters -// img_sub_.reset(new image_transport::SubscriberFilter(it_,"img_in", 1000)); -// road_sub_.reset(new message_filters::Subscriber(pnh_,"road_in", 1000)); -// sync_.reset(new message_filters::Synchronizer(SyncImageToRoad(100), *img_sub_, *road_sub_)); -// sync_->setAgePenalty(1.0); + img_sub_.reset(new image_transport::SubscriberFilter(it_,"img_in", 1000)); + road_sub_.reset(new message_filters::Subscriber(pnh_,"road_in", 1000)); + sync_.reset(new message_filters::Synchronizer(SyncImageToRoad(100), *img_sub_, *road_sub_)); + sync_->setAgePenalty(1.0); // sync_->registerCallback(boost::bind(&RoadDetection::syncCallback, this, _1, _2)); - img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); -// img_sub_debug_ = it_.subscribe("img_in", 1000, boost::bind(&RoadDetection::debugDrawFrameCallback, this, -// _1, std::string("/camera_optical"), std::string("/front_axis_middle"))); + // Debug callbacks for testing +// img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); + img_sub_debug_ = it_.subscribe("img_in", 1000, boost::bind(&RoadDetection::debugDrawFrameCallback, this, + _1, std::string("/camera_optical"), std::string("/front_axis_middle"))); line_output_pub_ = nh_.advertise("line_out",10); @@ -230,8 +231,8 @@ bool RoadDetection::find(){ //create left/mid/right lane linestring mid, left, right; - for (auto point : road_points_buffer_) { - boost::geometry::append(mid,point_xy(point.x, point.y)); + for (auto point_stamped : road_points_buffer_) { + boost::geometry::append(mid,point_xy(point_stamped.point.x, point_stamped.point.y)); } // simplify mid (previously done with distance-based algorithm // skip this for now, can completely clear the line @@ -420,11 +421,12 @@ void RoadDetection::processSearchLine(const SearchLine &l) { // todo: check how this gets handled and adjust the interface drive_ros_msgs::RoadLane lane_out; lane_out.header.stamp = ros::Time::now(); - geometry_msgs::Point32 point_temp; - point_temp.z = 0.f; + geometry_msgs::PointStamped point_temp; + point_temp.header.frame_id = std::string("/rear_axis_middle"); + point_temp.point.z = 0.f; for (auto point: validPoints) { - point_temp.x = point.x; - point_temp.y = point.y; + point_temp.point.x = point.x; + point_temp.point.y = point.y; lane_out.points.push_back(point_temp); } lane_out.roadStateType = drive_ros_msgs::RoadLane::UNKNOWN; From c718c81d22587d1b1986f50fbd92752956a19320 Mon Sep 17 00:00:00 2001 From: FaHa Date: Thu, 5 Oct 2017 11:30:08 +0200 Subject: [PATCH 58/94] refactoring: changed naming from new_road_detection to road_detection --- CMakeLists.txt | 26 ++++----- ...oad_detection.yaml => road_detection.yaml} | 2 +- ...{new_road_detection.h => road_detection.h} | 18 +++--- ...detection.launch => road_detection.launch} | 4 +- ...bug.launch => road_detection_debug.launch} | 4 +- ...t.launch => road_detection_nodelet.launch} | 16 +++--- nodelets.xml | 8 +-- package.xml | 6 +- src/new_road_detection_node.cpp | 26 --------- ..._road_detection.cpp => road_detection.cpp} | 56 +++++++++---------- src/road_detection_node.cpp | 26 +++++++++ 11 files changed, 96 insertions(+), 96 deletions(-) rename config/{new_road_detection.yaml => road_detection.yaml} (93%) rename include/drive_ros_image_recognition/{new_road_detection.h => road_detection.h} (89%) rename launch/{new_road_detection.launch => road_detection.launch} (79%) rename launch/{new_road_detection_debug.launch => road_detection_debug.launch} (79%) rename launch/{new_road_detection_nodelet.launch => road_detection_nodelet.launch} (63%) delete mode 100644 src/new_road_detection_node.cpp rename src/{new_road_detection.cpp => road_detection.cpp} (86%) create mode 100644 src/road_detection_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fef719..130273b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,45 +62,45 @@ include_directories( ################################################################################ -# New road detection node (LMS port) +# road detection node (LMS port) ################################################################################ -add_executable(new_road_detection_node - src/new_road_detection.cpp - src/new_road_detection_node.cpp +add_executable(road_detection_node + src/road_detection.cpp + src/road_detection_node.cpp ) -add_dependencies(new_road_detection_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(road_detection_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(new_road_detection_node +target_link_libraries(road_detection_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ) -install(TARGETS new_road_detection_node +install(TARGETS road_detection_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) ################################################################################ -# New road detection nodelet (LMS port) +# road detection nodelet (LMS port) ################################################################################ -add_library(new_road_detection_nodelet - src/new_road_detection.cpp +add_library(road_detection_nodelet + src/road_detection.cpp ) -add_dependencies(new_road_detection_nodelet ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(road_detection_nodelet ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(new_road_detection_nodelet +target_link_libraries(road_detection_nodelet ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ) -install(TARGETS new_road_detection_nodelet +install(TARGETS road_detection_nodelet ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/config/new_road_detection.yaml b/config/road_detection.yaml similarity index 93% rename from config/new_road_detection.yaml rename to config/road_detection.yaml index bea791b..9009a35 100644 --- a/config/new_road_detection.yaml +++ b/config/road_detection.yaml @@ -1,4 +1,4 @@ -new_road_detection: +road_detection: searchOffset: 0.15 distanceBetweenSearchlines: 0.1 minLineWidthMul: 0.5 diff --git a/include/drive_ros_image_recognition/new_road_detection.h b/include/drive_ros_image_recognition/road_detection.h similarity index 89% rename from include/drive_ros_image_recognition/new_road_detection.h rename to include/drive_ros_image_recognition/road_detection.h index 2bcc588..dc8ee81 100644 --- a/include/drive_ros_image_recognition/new_road_detection.h +++ b/include/drive_ros_image_recognition/road_detection.h @@ -1,5 +1,5 @@ -#ifndef NEW_ROAD_DETECTION_H -#define NEW_ROAD_DETECTION_H +#ifndef ROAD_DETECTION_H +#define ROAD_DETECTION_H #include #include @@ -25,9 +25,9 @@ namespace drive_ros_image_recognition { /** - * @brief Port of LMS module new_road_detection to ROS + * @brief Port of LMS module road_detection to ROS **/ -class NewRoadDetection { +class RoadDetection { // configs float searchOffset_; float distanceBetweenSearchlines_; @@ -85,17 +85,17 @@ class NewRoadDetection { ImageOperator image_operator_; public: - NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); - ~NewRoadDetection(); + RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); + ~RoadDetection(); bool init(); }; -class NewRoadDetectionNodelet: public nodelet::Nodelet { +class RoadDetectionNodelet: public nodelet::Nodelet { public: virtual void onInit(); private: - std::unique_ptr new_road_detection_; + std::unique_ptr road_detection_; }; } -#endif // NEW_ROAD_DETECTION_H +#endif // ROAD_DETECTION_H diff --git a/launch/new_road_detection.launch b/launch/road_detection.launch similarity index 79% rename from launch/new_road_detection.launch rename to launch/road_detection.launch index c8ebdaf..a0b233c 100644 --- a/launch/new_road_detection.launch +++ b/launch/road_detection.launch @@ -2,12 +2,12 @@ - + - + diff --git a/launch/new_road_detection_debug.launch b/launch/road_detection_debug.launch similarity index 79% rename from launch/new_road_detection_debug.launch rename to launch/road_detection_debug.launch index b5f0b6d..ed02187 100644 --- a/launch/new_road_detection_debug.launch +++ b/launch/road_detection_debug.launch @@ -2,12 +2,12 @@ - + - + diff --git a/launch/new_road_detection_nodelet.launch b/launch/road_detection_nodelet.launch similarity index 63% rename from launch/new_road_detection_nodelet.launch rename to launch/road_detection_nodelet.launch index a8d8ea3..fc3c271 100644 --- a/launch/new_road_detection_nodelet.launch +++ b/launch/road_detection_nodelet.launch @@ -1,32 +1,32 @@ - + - - + + - + - + - - + + - + diff --git a/nodelets.xml b/nodelets.xml index 2d252dc..66a8106 100644 --- a/nodelets.xml +++ b/nodelets.xml @@ -7,16 +7,16 @@ - - - + + + Image warper nodelet. - + diff --git a/package.xml b/package.xml index 8647986..8a87e87 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ 0.0.0 Phoenix Drive image processing. - + mykyta @@ -51,7 +51,7 @@ geometry_msgs tf message_filters - + message_generation cv_bridge @@ -65,7 +65,7 @@ geometry_msgs tf message_filters - + message_runtime diff --git a/src/new_road_detection_node.cpp b/src/new_road_detection_node.cpp deleted file mode 100644 index c05f2ab..0000000 --- a/src/new_road_detection_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "new_road_detection"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - -#ifndef NDEBUG - // give GDB time to attach - ros::Duration(2.0).sleep(); -#endif - - drive_ros_image_recognition::NewRoadDetection new_road_detection(nh,pnh); - if (!new_road_detection.init()) { - return 1; - } - else { - ROS_INFO("New road detection node succesfully initialized"); - } - - while (ros::ok()) { - ros::spin(); - } - return 0; -} diff --git a/src/new_road_detection.cpp b/src/road_detection.cpp similarity index 86% rename from src/new_road_detection.cpp rename to src/road_detection.cpp index 51c9911..a46876c 100644 --- a/src/new_road_detection.cpp +++ b/src/road_detection.cpp @@ -1,9 +1,9 @@ -#include "drive_ros_image_recognition/new_road_detection.h" +#include "drive_ros_image_recognition/road_detection.h" #include namespace drive_ros_image_recognition { -NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh): +RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh): nh_(nh), pnh_(pnh), // img_sub_(), @@ -27,7 +27,7 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand filtered_points_pub_(), #endif dsrv_server_(), - dsrv_cb_(boost::bind(&NewRoadDetection::reconfigureCB, this, _1, _2)), + dsrv_cb_(boost::bind(&RoadDetection::reconfigureCB, this, _1, _2)), lines_(), linesToProcess_(0), current_image_(), @@ -38,45 +38,45 @@ NewRoadDetection::NewRoadDetection(const ros::NodeHandle nh, const ros::NodeHand { } -bool NewRoadDetection::init() { +bool RoadDetection::init() { // load parameters - if (!pnh_.getParam("new_road_detection/searchOffset", searchOffset_)) { + if (!pnh_.getParam("road_detection/searchOffset", searchOffset_)) { ROS_WARN_STREAM("Unable to load 'searchOffset' parameter, using default: "<(pnh_,"road_in", 1000)); // sync_.reset(new message_filters::Synchronizer(SyncImageToRoad(100), *img_sub_, *road_sub_)); // sync_->setAgePenalty(1.0); -// sync_->registerCallback(boost::bind(&NewRoadDetection::syncCallback, this, _1, _2)); +// sync_->registerCallback(boost::bind(&RoadDetection::syncCallback, this, _1, _2)); - img_sub_debug_ = it_.subscribe("img_in", 1000, &NewRoadDetection::debugImageCallback, this); + img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); line_output_pub_ = pnh_.advertise("line_out",10); @@ -113,7 +113,7 @@ bool NewRoadDetection::init() { return true; } -void NewRoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) { +void RoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) { current_image_ = convertImageMessage(img_in); // crop image to 410 width cv::Rect roi(cv::Point(current_image_->cols/2-(410/2),0),cv::Point(current_image_->cols/2+(410/2),current_image_->rows)); @@ -124,7 +124,7 @@ void NewRoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_ image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth_brightness); } -void NewRoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in){ +void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in){ current_image_ = convertImageMessage(img_in); road_points_buffer_ = road_in->points; @@ -169,7 +169,7 @@ void NewRoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, co // } } -bool NewRoadDetection::find(){ +bool RoadDetection::find(){ //clear old lines ROS_INFO("Creating new lines"); lines_.clear(); @@ -230,7 +230,7 @@ bool NewRoadDetection::find(){ return true; } -void NewRoadDetection::processSearchLine(const SearchLine &l) { +void RoadDetection::processSearchLine(const SearchLine &l) { // std::vector xv; // std::vector yv; @@ -378,7 +378,7 @@ void NewRoadDetection::processSearchLine(const SearchLine &l) { line_output_pub_.publish(lane_out); } -void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig &config, uint32_t level){ +void RoadDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig &config, uint32_t level){ image_operator_.setConfig(config); searchOffset_ = config.searchOffset; findPointsBySobel_ = config.findBySobel; @@ -390,21 +390,21 @@ void NewRoadDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionC sobelThreshold_ = config.sobelThreshold; } -NewRoadDetection::~NewRoadDetection() { +RoadDetection::~RoadDetection() { } -void NewRoadDetectionNodelet::onInit() { - new_road_detection_.reset(new NewRoadDetection(getNodeHandle(),getPrivateNodeHandle())); - if (!new_road_detection_->init()) { - ROS_ERROR("New_road_detection nodelet failed to initialize"); +void RoadDetectionNodelet::onInit() { + road_detection_.reset(new RoadDetection(getNodeHandle(),getPrivateNodeHandle())); + if (!road_detection_->init()) { + ROS_ERROR("road_detection nodelet failed to initialize"); // nodelet failing will kill the entire loader anyway ros::shutdown(); } else { - ROS_INFO("New road detection nodelet succesfully initialized"); + ROS_INFO("road detection nodelet succesfully initialized"); } } } // namespace drive_ros_image_recognition -PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::NewRoadDetectionNodelet, nodelet::Nodelet) +PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::RoadDetectionNodelet, nodelet::Nodelet) diff --git a/src/road_detection_node.cpp b/src/road_detection_node.cpp new file mode 100644 index 0000000..90ff1b3 --- /dev/null +++ b/src/road_detection_node.cpp @@ -0,0 +1,26 @@ +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "road_detection"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + +#ifndef NDEBUG + // give GDB time to attach + ros::Duration(2.0).sleep(); +#endif + + drive_ros_image_recognition::RoadDetection road_detection(nh,pnh); + if (!road_detection.init()) { + return 1; + } + else { + ROS_INFO("road detection node succesfully initialized"); + } + + while (ros::ok()) { + ros::spin(); + } + return 0; +} From 2b8e0884e355a99b1384b337ff87a4f636d7759a Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 7 Oct 2017 13:48:42 +0200 Subject: [PATCH 59/94] added first unittest test --- CMakeLists.txt | 8 ++++++++ test/common_image_operations_test.cpp | 21 +++++++++++++++++++++ test/common_image_operations_test.test | 3 +++ 3 files changed, 32 insertions(+) create mode 100644 test/common_image_operations_test.cpp create mode 100644 test/common_image_operations_test.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 130273b..a39f737 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,9 @@ find_package(catkin REQUIRED COMPONENTS #include Boost for lane line processing find_package(Boost REQUIRED) +# include test suite +find_package(rostest REQUIRED) + add_message_files( FILES RoadLane.msg @@ -215,3 +218,8 @@ install(DIRECTORY config/ FILES_MATCHING PATTERN "*.yaml" ) +################################################################################ +# test header files +################################################################################ +add_rostest_gtest(common_image_operations_test test/common_image_operations_test.test test/common_image_operations_test.cpp) +target_link_libraries(common_image_operations_test ${catkin_LIBRARIES}) diff --git a/test/common_image_operations_test.cpp b/test/common_image_operations_test.cpp new file mode 100644 index 0000000..b15f678 --- /dev/null +++ b/test/common_image_operations_test.cpp @@ -0,0 +1,21 @@ +#include +#include "drive_ros_image_recognition/common_image_operations.h" + + +// Declare test +TEST(TestSuite, testCase0) +{ + EXPECT_EQ(1,1); +} + +// Declare test +TEST(TestSuite, testCase1) +{ + EXPECT_EQ(1,0); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/common_image_operations_test.test b/test/common_image_operations_test.test new file mode 100644 index 0000000..47d1ed9 --- /dev/null +++ b/test/common_image_operations_test.test @@ -0,0 +1,3 @@ + + + From b17fa6c16c618bedfab0e58052fc25e385518d69 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 7 Oct 2017 14:23:52 +0200 Subject: [PATCH 60/94] add if() clause to cmake test --- CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a39f737..1fa7146 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,9 +31,6 @@ find_package(catkin REQUIRED COMPONENTS #include Boost for lane line processing find_package(Boost REQUIRED) -# include test suite -find_package(rostest REQUIRED) - add_message_files( FILES RoadLane.msg @@ -221,5 +218,8 @@ install(DIRECTORY config/ ################################################################################ # test header files ################################################################################ -add_rostest_gtest(common_image_operations_test test/common_image_operations_test.test test/common_image_operations_test.cpp) -target_link_libraries(common_image_operations_test ${catkin_LIBRARIES}) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(common_image_operations_test test/common_image_operations_test.test test/common_image_operations_test.cpp) + target_link_libraries(common_image_operations_test ${catkin_LIBRARIES}) +endif() From cad57bab53aac9030119604eb28f42b9c37b1581 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 7 Oct 2017 14:46:39 +0200 Subject: [PATCH 61/94] removed failing testcase --- test/common_image_operations_test.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/test/common_image_operations_test.cpp b/test/common_image_operations_test.cpp index b15f678..5534929 100644 --- a/test/common_image_operations_test.cpp +++ b/test/common_image_operations_test.cpp @@ -8,12 +8,6 @@ TEST(TestSuite, testCase0) EXPECT_EQ(1,1); } -// Declare test -TEST(TestSuite, testCase1) -{ - EXPECT_EQ(1,0); -} - // Run all the tests that were declared with TEST() int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); From d25c34377fde79f8fec975a8d43729ceddd0d9f9 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sun, 8 Oct 2017 00:07:32 +0200 Subject: [PATCH 62/94] added more testing --- CMakeLists.txt | 22 ++++++++------- launch/road_detection.launch | 2 +- launch/road_detection.test | 3 +++ test/common_image_operations_test.cpp | 8 +----- test/common_image_operations_test.test | 3 --- test/geometry_common_test.cpp | 37 ++++++++++++++++++++++++++ test/road_detection_test.cpp | 15 +++++++++++ 7 files changed, 69 insertions(+), 21 deletions(-) create mode 100644 launch/road_detection.test delete mode 100644 test/common_image_operations_test.test create mode 100644 test/geometry_common_test.cpp create mode 100644 test/road_detection_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1fa7146..7e0524b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,6 +84,17 @@ install(TARGETS road_detection_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(road_detection_node_test launch/road_detection.test + test/common_image_operations_test.cpp + test/road_detection_test.cpp + test/geometry_common_test.cpp + ) + target_link_libraries(road_detection_node_test ${catkin_LIBRARIES}) +endif() + + ################################################################################ # road detection nodelet (LMS port) ################################################################################ @@ -148,7 +159,7 @@ install(TARGETS warp_image_nodelet ) ################################################################################ -# Street crossing detection node (LMS port) +# Street crossing detection nodelet (LMS port) ################################################################################ add_library(street_crossing_nodelet @@ -214,12 +225,3 @@ install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} FILES_MATCHING PATTERN "*.yaml" ) - -################################################################################ -# test header files -################################################################################ -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(common_image_operations_test test/common_image_operations_test.test test/common_image_operations_test.cpp) - target_link_libraries(common_image_operations_test ${catkin_LIBRARIES}) -endif() diff --git a/launch/road_detection.launch b/launch/road_detection.launch index a0b233c..ed02187 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/road_detection.test b/launch/road_detection.test new file mode 100644 index 0000000..c4b1250 --- /dev/null +++ b/launch/road_detection.test @@ -0,0 +1,3 @@ + + + diff --git a/test/common_image_operations_test.cpp b/test/common_image_operations_test.cpp index 5534929..5924139 100644 --- a/test/common_image_operations_test.cpp +++ b/test/common_image_operations_test.cpp @@ -3,13 +3,7 @@ // Declare test -TEST(TestSuite, testCase0) +TEST(CommonImageOperations, testCase0) { EXPECT_EQ(1,1); } - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv){ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/common_image_operations_test.test b/test/common_image_operations_test.test deleted file mode 100644 index 47d1ed9..0000000 --- a/test/common_image_operations_test.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/test/geometry_common_test.cpp b/test/geometry_common_test.cpp new file mode 100644 index 0000000..b2af582 --- /dev/null +++ b/test/geometry_common_test.cpp @@ -0,0 +1,37 @@ +#include +#include "drive_ros_image_recognition/geometry_common.h" + + +// Declare test +TEST(GeometryCommon, moveOrtho) +{ + linestring line1, line2, line3; + // 2 points on a vertical line (easier to test) + point_xy p1(56.567, 37.5989); + point_xy p2(56.567, 1985.4); + line1.push_back(p1); + line1.push_back(p2); + + const double dis=78.5569; // distance to move + const double err=0.0000001; // allowed error + + // move line to the right + drive_ros_geometry_common::moveOrthogonal(line1, line2, dis); + EXPECT_NEAR(line1.at(0).x()+dis, line2.at(0).x(), err); + EXPECT_NEAR(line1.at(1).x()+dis, line2.at(1).x(), err); + EXPECT_NEAR(line1.at(0).y(), line2.at(0).y(), err); + EXPECT_NEAR(line1.at(1).y(), line2.at(1).y(), err); + + // move line back to the left + drive_ros_geometry_common::moveOrthogonal(line2, line3, -dis); + EXPECT_NEAR(line2.at(0).x()-dis, line3.at(0).x(), err); + EXPECT_NEAR(line2.at(1).x()-dis, line3.at(1).x(), err); + EXPECT_NEAR(line2.at(0).y(), line3.at(0).y(), err); + EXPECT_NEAR(line2.at(1).y(), line3.at(1).y(), err); + + EXPECT_NEAR(line1.at(0).x(), line3.at(0).x(), err); + EXPECT_NEAR(line1.at(1).x(), line3.at(1).x(), err); + EXPECT_NEAR(line1.at(0).y(), line3.at(0).y(), err); + EXPECT_NEAR(line1.at(1).y(), line3.at(1).y(), err); + +} diff --git a/test/road_detection_test.cpp b/test/road_detection_test.cpp new file mode 100644 index 0000000..016afcb --- /dev/null +++ b/test/road_detection_test.cpp @@ -0,0 +1,15 @@ +#include +#include "drive_ros_image_recognition/road_detection.h" + + +// Declare test +TEST(RoadDetection, testCase0) +{ + EXPECT_EQ(1,1); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From b1bc764fa8f2c364afcf220116bcb03f9bb7bc0e Mon Sep 17 00:00:00 2001 From: FaHa Date: Sun, 8 Oct 2017 09:59:19 +0200 Subject: [PATCH 63/94] added some more geometry common tests and fixed move orthogonal with same points --- .../geometry_common.h | 24 ++++++++-------- test/geometry_common_test.cpp | 28 +++++++++++++++++++ 2 files changed, 41 insertions(+), 11 deletions(-) diff --git a/include/drive_ros_image_recognition/geometry_common.h b/include/drive_ros_image_recognition/geometry_common.h index 782eb92..a735722 100644 --- a/include/drive_ros_image_recognition/geometry_common.h +++ b/include/drive_ros_image_recognition/geometry_common.h @@ -22,18 +22,20 @@ struct orthogonalHelper{ orthogonalHelper(double distance) : distance_(distance){} inline void operator()(Segment &s){ - // offset 90 degrees - segment_hc rot; - boost::geometry::transform(s, rot, rotate); - - // normalize double length = boost::geometry::distance(s.first, s.second); - trans::scale_transformer scale(distance_/length); - boost::geometry::transform(rot, rot, scale); - - // offset original segment - trans::translate_transformer translate(bg::get<0>(rot.second)-bg::get<0>(rot.first), bg::get<1>(rot.second)-bg::get<1>(rot.first)); - boost::geometry::transform(s,s,translate); + if(length > 0){ + // offset 90 degrees + segment_hc rot; + boost::geometry::transform(s, rot, rotate); + + // normalize + trans::scale_transformer scale(distance_/length); + boost::geometry::transform(rot, rot, scale); + + // offset original segment + trans::translate_transformer translate(bg::get<0>(rot.second)-bg::get<0>(rot.first), bg::get<1>(rot.second)-bg::get<1>(rot.first)); + boost::geometry::transform(s,s,translate); + } } double distance_; diff --git a/test/geometry_common_test.cpp b/test/geometry_common_test.cpp index b2af582..4bcc405 100644 --- a/test/geometry_common_test.cpp +++ b/test/geometry_common_test.cpp @@ -1,5 +1,6 @@ #include #include "drive_ros_image_recognition/geometry_common.h" +#include // Declare test @@ -15,6 +16,13 @@ TEST(GeometryCommon, moveOrtho) const double dis=78.5569; // distance to move const double err=0.0000001; // allowed error + // don't move + drive_ros_geometry_common::moveOrthogonal(line1, line2, 0); + EXPECT_NEAR(line1.at(0).x(), line2.at(0).x(), err); + EXPECT_NEAR(line1.at(1).x(), line2.at(1).x(), err); + EXPECT_NEAR(line1.at(0).y(), line2.at(0).y(), err); + EXPECT_NEAR(line1.at(1).y(), line2.at(1).y(), err); + // move line to the right drive_ros_geometry_common::moveOrthogonal(line1, line2, dis); EXPECT_NEAR(line1.at(0).x()+dis, line2.at(0).x(), err); @@ -34,4 +42,24 @@ TEST(GeometryCommon, moveOrtho) EXPECT_NEAR(line1.at(0).y(), line3.at(0).y(), err); EXPECT_NEAR(line1.at(1).y(), line3.at(1).y(), err); + + linestring line4, line5; + point_xy p3(0,0), p4(0,0); + line4.push_back(p3); + + // line with only 1 point + drive_ros_geometry_common::moveOrthogonal(line4, line5, dis); + EXPECT_NEAR(line4.at(0).x(), line5.at(0).x(), err); + EXPECT_NEAR(line4.at(0).y(), line5.at(0).y(), err); + + // two points at 0 + line4.push_back(p4); + drive_ros_geometry_common::moveOrthogonal(line4, line5, dis); + EXPECT_NEAR(line4.at(0).x(), line5.at(0).x(), err); + EXPECT_NEAR(line4.at(1).x(), line5.at(1).x(), err); + EXPECT_NEAR(line4.at(0).y(), line5.at(0).y(), err); + EXPECT_NEAR(line4.at(1).y(), line5.at(1).y(), err); + + + } From 3affde1693294edf79b3cbce17f6ef47cd2128ce Mon Sep 17 00:00:00 2001 From: FaHa Date: Tue, 10 Oct 2017 10:20:20 +0200 Subject: [PATCH 64/94] moved RoadLane.msg to drive_ros_msgs package --- CMakeLists.txt | 14 ++------------ .../drive_ros_image_recognition/road_detection.h | 4 ++-- .../drive_ros_image_recognition/street_crossing.h | 3 +-- msg/RoadLane.msg | 10 ---------- package.xml | 6 ++---- scripts/publish_road.sh | 2 +- src/road_detection.cpp | 8 ++++---- src/street_crossing.cpp | 2 +- 8 files changed, 13 insertions(+), 36 deletions(-) delete mode 100644 msg/RoadLane.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e0524b..d0a21c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs + drive_ros_msgs dynamic_reconfigure geometry_msgs message_generation @@ -31,24 +32,13 @@ find_package(catkin REQUIRED COMPONENTS #include Boost for lane line processing find_package(Boost REQUIRED) -add_message_files( - FILES - RoadLane.msg - ) - -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs - ) - generate_dynamic_reconfigure_options( cfg/LineDetection.cfg ) catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs message_runtime + CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs drive_ros_msgs ) find_package(OpenCV REQUIRED) diff --git a/include/drive_ros_image_recognition/road_detection.h b/include/drive_ros_image_recognition/road_detection.h index dc8ee81..75fac14 100644 --- a/include/drive_ros_image_recognition/road_detection.h +++ b/include/drive_ros_image_recognition/road_detection.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include #include @@ -77,7 +77,7 @@ class RoadDetection { dynamic_reconfigure::Server dsrv_server_; dynamic_reconfigure::Server::CallbackType dsrv_cb_; void debugImageCallback(const sensor_msgs::ImageConstPtr& img_in); - void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in); + void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in); void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); bool find(); void processSearchLine(const SearchLine &line); diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index 779ef42..dd8a0e3 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -11,8 +11,7 @@ #include #include #include - -#include "drive_ros_image_recognition/RoadLane.h" +#include "drive_ros_msgs/RoadLane.h" namespace drive_ros_image_recognition { diff --git a/msg/RoadLane.msg b/msg/RoadLane.msg deleted file mode 100644 index c995c7c..0000000 --- a/msg/RoadLane.msg +++ /dev/null @@ -1,10 +0,0 @@ -#Port of RoadLane from LMS: -Header header -# lane points -geometry_msgs/Point32[] points -# type of lane -uint8 UNKNOWN=0 -uint8 STRAIGHT=1 -uint8 STRAIGHT_CURVE=2 -uint8 CURVE = 3 -uint8 roadStateType diff --git a/package.xml b/package.xml index 8a87e87..b10de34 100644 --- a/package.xml +++ b/package.xml @@ -51,8 +51,7 @@ geometry_msgs tf message_filters - - message_generation + drive_ros_msgs cv_bridge image_transport @@ -65,8 +64,7 @@ geometry_msgs tf message_filters - - message_runtime + drive_ros_msgs diff --git a/scripts/publish_road.sh b/scripts/publish_road.sh index 32602b9..257575f 100755 --- a/scripts/publish_road.sh +++ b/scripts/publish_road.sh @@ -1,5 +1,5 @@ # simple publisher for road messages to be able to view what happens during processing -rostopic pub /road_trajectory drive_ros_image_recognition/RoadLane "header: +rostopic pub /road_trajectory drive_ros_msgs/RoadLane "header: seq: 0 stamp: secs: 0 diff --git a/src/road_detection.cpp b/src/road_detection.cpp index a46876c..4dac63e 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -94,7 +94,7 @@ bool RoadDetection::init() { img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); - line_output_pub_ = pnh_.advertise("line_out",10); + line_output_pub_ = pnh_.advertise("line_out",10); #ifdef PUBLISH_DEBUG debug_img_pub_ = it_.advertise("debug_image_out", 10); @@ -124,7 +124,7 @@ void RoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth_brightness); } -void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const RoadLaneConstPtr& road_in){ +void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in){ current_image_ = convertImageMessage(img_in); road_points_buffer_ = road_in->points; @@ -365,7 +365,7 @@ void RoadDetection::processSearchLine(const SearchLine &l) { } // todo: check how this gets handled and adjust the interface - drive_ros_image_recognition::RoadLane lane_out; + drive_ros_msgs::RoadLane lane_out; lane_out.header.stamp = ros::Time::now(); geometry_msgs::Point32 point_temp; point_temp.z = 0.f; @@ -374,7 +374,7 @@ void RoadDetection::processSearchLine(const SearchLine &l) { point_temp.y = point.y; lane_out.points.push_back(point_temp); } - lane_out.roadStateType = drive_ros_image_recognition::RoadLane::UNKNOWN; + lane_out.roadStateType = drive_ros_msgs::RoadLane::UNKNOWN; line_output_pub_.publish(lane_out); } diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index 9fba52c..9823e15 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -35,7 +35,7 @@ bool StreetCrossingDetection::init() { imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); ROS_INFO_STREAM("Subscriber image transport with topic "<("line_out",10); + line_output_pub_ = pnh_.advertise("line_out",10); #ifdef PUBLISH_DEBUG debugImagePublisher = imageTransport_.advertise("/street_crossing/debug_image", 10); From cf7163e5062338bb459011a34672753a4414904c Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Wed, 11 Oct 2017 10:22:46 +0200 Subject: [PATCH 65/94] Add homography subscription to warp node, some minor compile flags in line processing operations --- CMakeLists.txt | 4 +- .../common_image_operations.h | 7 ++++ .../drive_ros_image_recognition/warp_image.h | 10 +++-- src/warp_image.cpp | 41 +++++++++++++++++-- 4 files changed, 53 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d0a21c1..0c10e1b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -115,7 +115,7 @@ add_executable(warp_image_node src/warp_image.cpp ) -add_dependencies(warp_image_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(warp_image_node ${drive_ros_msgs_generate_messages_cpp} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(warp_image_node ${catkin_LIBRARIES} @@ -135,7 +135,7 @@ add_library(warp_image_nodelet src/warp_image.cpp ) -add_dependencies(warp_image_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(warp_image_nodelet ${drive_ros_msgs_generate_messages_cpp} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(warp_image_nodelet ${catkin_LIBRARIES} diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 055e2fa..44e0a14 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -8,6 +8,7 @@ #include #include #include +#include typedef boost::shared_ptr CvImagePtr; @@ -425,8 +426,10 @@ class ImageOperator { ) { std::vector image_points; std::vector image_widths; +#ifdef DRAW_DEBUG cv::Mat debug_image; cv::cvtColor(current_image, debug_image, CV_GRAY2RGB); +#endif int search_steps = 6; int pixel_step = 50; @@ -439,7 +442,9 @@ class ImageOperator { lines[it].iStart = cv::Point(it*pixel_step, 0); lines[it].iEnd = cv::Point(it*pixel_step, current_image.rows); } +#ifdef DRAW_DEBUG cv::line(debug_image, lines[it].iStart, lines[it].iEnd, cv::Scalar(255,255,255)); +#endif findByLineSearch(lines[it], current_image, search_dir, @@ -447,9 +452,11 @@ class ImageOperator { image_points, image_widths ); +#ifdef DRAW_DEBUG for (auto point: image_points) { cv::circle(debug_image,point,2,cv::Scalar(0,255,0),2); } +#endif } #ifdef DRAW_DEBUG if (search_meth == search_method::sobel) { diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 1ccbbae..e1dcfb1 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -18,15 +18,13 @@ class WarpContent { WarpContent(const ros::NodeHandle &nh, const ros::NodeHandle& pnh); ~WarpContent(); bool init(); + void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in); private: void world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg); ros::NodeHandle pnh_; ros::NodeHandle nh_; cv::Mat current_image_; // needs two components: camera model and homography for transformation - // homography matrices for transformation - cv::Mat world2cam_; - cv::Mat cam2world_; // camera model matrix and distortion coefficients cv::Mat cam_mat_; cv::Mat dist_coeffs_; @@ -39,6 +37,12 @@ class WarpContent { ros::ServiceServer imageToWorldServer_; // todo: moving to camera model subscription image_geometry::PinholeCameraModel cam_model_; + + // homography components + ros::Subscriber homography_params_sub_; + cv::Mat world2cam_; + cv::Mat cam2world_; + bool homo_received_; }; class WarpImageNodelet : public nodelet::Nodelet { diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 7773989..7a80257 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -7,15 +7,17 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): nh_(nh), pnh_(pnh), current_image_(), - world2cam_(3,3,CV_64F,cv::Scalar(0.0)), - cam2world_(3,3,CV_64F,cv::Scalar(0.0)), cam_mat_(3,3,CV_64F,cv::Scalar(0.0)), dist_coeffs_(8,1,CV_64F,cv::Scalar(0.0)), it_(pnh), cam_sub_(), worldToImageServer_(), imageToWorldServer_(), - cam_model_() + cam_model_(), + homography_params_sub_(), + cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), + world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), + homo_received_(false) { img_pub_ = it_.advertise("warped_out", 1); undistort_pub_ = it_.advertise("undistort_out", 1); @@ -33,11 +35,18 @@ bool WarpContent::init() { // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); + + homography_params_sub_ = pnh_.subscribe("homography_in", 1, &WarpContent::homography_callback, this); return true; } void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) { + if (!homo_received_) { + ROS_WARN_THROTTLE(10, "Homography callback not yet received, waiting"); + return; + } + try { // copy @@ -66,10 +75,34 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); // flag ensures that we directly use the matrix, as it is done in LMS -// cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); + cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); // img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } +// bind cannot resolve to references +void WarpContent::homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in) { + if (!homo_received_) + homo_received_ = true; + + ROS_ASSERT(homo_in->cam2world.layout.dim[0].size == 3 && homo_in->cam2world.layout.dim[1].size == 3 ); + cam2world_ = cv::Mat::zeros(3,3,CV_64FC1); + int k=0; + for (int i=0; icam2world.layout.dim[0].size; i++){ + for (int j=0; jcam2world.layout.dim[1].size; j++){ + cam2world_.at(i,j) = homo_in->cam2world.data[k++]; + } + } + + ROS_ASSERT(homo_in->world2cam.layout.dim[0].size == 3 && homo_in->world2cam.layout.dim[1].size == 3 ); + world2cam_ = cv::Mat::zeros(3,3,CV_64FC1); + k=0; + for (int i=0; iworld2cam.layout.dim[0].size; i++){ + for (int j=0; jworld2cam.layout.dim[1].size; j++){ + world2cam_.at(i,j) = homo_in->world2cam.data[k++]; + } + } +} + void WarpImageNodelet::onInit() { my_content_.reset(new WarpContent(getNodeHandle(),ros::NodeHandle(getPrivateNodeHandle()))); From 42a6cfcf9b6e7f2f449c18ddc589f7b807d5850c Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Fri, 13 Oct 2017 02:00:12 +0200 Subject: [PATCH 66/94] Fix launch files (install paths and removed homography yaml references) --- launch/road_detection.launch | 4 +--- launch/road_detection_debug.launch | 4 +--- launch/road_detection_nodelet.launch | 8 ++------ launch/street_crossing.launch | 2 -- launch/street_crossing_nodelet.launch | 4 ---- launch/warp_image.launch | 1 - launch/warp_image_nodelet.launch | 2 -- 7 files changed, 4 insertions(+), 21 deletions(-) diff --git a/launch/road_detection.launch b/launch/road_detection.launch index ed02187..bc41ed7 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -7,8 +7,6 @@ - - - + diff --git a/launch/road_detection_debug.launch b/launch/road_detection_debug.launch index ed02187..bc41ed7 100644 --- a/launch/road_detection_debug.launch +++ b/launch/road_detection_debug.launch @@ -7,8 +7,6 @@ - - - + diff --git a/launch/road_detection_nodelet.launch b/launch/road_detection_nodelet.launch index fc3c271..2b08469 100644 --- a/launch/road_detection_nodelet.launch +++ b/launch/road_detection_nodelet.launch @@ -13,9 +13,7 @@ - - - + @@ -26,9 +24,7 @@ - - - + diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch index 92d582d..10a7269 100644 --- a/launch/street_crossing.launch +++ b/launch/street_crossing.launch @@ -3,7 +3,5 @@ - - diff --git a/launch/street_crossing_nodelet.launch b/launch/street_crossing_nodelet.launch index 4f5b948..761270c 100644 --- a/launch/street_crossing_nodelet.launch +++ b/launch/street_crossing_nodelet.launch @@ -8,8 +8,6 @@ - - @@ -18,8 +16,6 @@ - - diff --git a/launch/warp_image.launch b/launch/warp_image.launch index fc1b5f4..eea7d51 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -4,7 +4,6 @@ - diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch index 36239d3..87b711b 100644 --- a/launch/warp_image_nodelet.launch +++ b/launch/warp_image_nodelet.launch @@ -10,7 +10,6 @@ - @@ -21,7 +20,6 @@ - From f61b6210acdfa6fbde3eae374dd63fe47893c312 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sat, 14 Oct 2017 05:51:02 +0200 Subject: [PATCH 67/94] Remove yaml homography loading, add homography subscription to imaging nodes. Todo: wait for first callback, launch files are still broken probably, also does not compute cam2world correctly yet --- .../common_image_operations.h | 146 +++++++++--------- .../drive_ros_image_recognition/warp_image.h | 1 - launch/road_detection.launch | 2 + src/road_detection.cpp | 2 +- src/street_crossing.cpp | 2 +- src/warp_image.cpp | 35 +---- 6 files changed, 77 insertions(+), 111 deletions(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 44e0a14..9b460c0 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -36,32 +36,34 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) return cv_ptr; } - namespace drive_ros_image_recognition { -inline void cam_info_sub(const sensor_msgs::CameraInfoConstPtr& incoming_cam_info, image_geometry::PinholeCameraModel& cam_model) { - cam_model.fromCameraInfo(incoming_cam_info); -} - -inline bool getHomographyMatParam(const ros::NodeHandle& pnh, cv::Mat mat, const std::string mat_param) { - // retrieve world2map and map2world homography matrices - std::vector temp_vals(9); - - if (!pnh.getParam("homography_matrix/"+mat_param, temp_vals)) { - ROS_ERROR("Unable to load homography matrix %s from configuration file!", mat_param.c_str()); - return false; - } - if (temp_vals.size() != 9) { - ROS_ERROR("Retreived homography matrix %s does not have 9 values", mat_param.c_str()); - return false; +inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in, cv::Mat& cam2world, cv::Mat& world2cam, + bool& homography_received) { + if (!homography_received) + homography_received = true; + + ROS_ASSERT(homo_in->cam2world.layout.dim[0].size == 3 && homo_in->cam2world.layout.dim[1].size == 3 ); + cam2world = cv::Mat::zeros(3,3,CV_64FC1); + int k=0; + for (int i=0; icam2world.layout.dim[0].size; i++){ + for (int j=0; jcam2world.layout.dim[1].size; j++){ + cam2world.at(i,j) = homo_in->cam2world.data[k++]; + } } - // update values - mat = cv::Mat::zeros(3,3,CV_64F); - for(unsigned i=0; i < temp_vals.size(); i++) { - mat.at(i) = temp_vals[i]; + + ROS_ASSERT(homo_in->world2cam.layout.dim[0].size == 3 && homo_in->world2cam.layout.dim[1].size == 3 ); + world2cam = cv::Mat::zeros(3,3,CV_64FC1); + k=0; + for (int i=0; iworld2cam.layout.dim[0].size; i++){ + for (int j=0; jworld2cam.layout.dim[1].size; j++){ + world2cam.at(i,j) = homo_in->world2cam.data[k++]; + } } - ROS_DEBUG_STREAM("Paramater matrix loaded as: "<("/camera1/camera_info", 1, + cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); + + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + cam2world_, world2cam_, homography_received_)); other.cam_info_sub_.shutdown(); return *this; } @@ -122,25 +126,19 @@ class TransformHelper { tf_listener_() { cam2world_ = helper.cam2world_; -#ifdef USE_WORLD2CAM_HOMOGRAPHY world2cam_ = helper.world2cam_; -#endif cam_model_ = helper.cam_model_; - ros::NodeHandle pnh("~"); - cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, + cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + cam2world_, world2cam_, homography_received_)); } - TransformHelper(const cv::Mat& cam2world, const image_geometry::PinholeCameraModel& cam_model - #ifdef USE_WORLD2CAM_HOMOGRAPHY - const cv::Mat& world2cam - #endif - ): - cam2world_(cam2world), - #ifdef USE_WORLD2CAM_HOMOGRAPHY - world2cam_(world2cam), - #endif + TransformHelper(const image_geometry::PinholeCameraModel& cam_model): + cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), + world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), cam_model_(cam_model), tf_listener_(), cam_info_sub_() @@ -150,19 +148,14 @@ class TransformHelper { ~TransformHelper() { } - bool init(ros::NodeHandle& pnh) { -#ifdef USE_WORLD2CAM_HOMOGRAPHY - if (!getHomographyMatParam(pnh, world2cam_, "world2cam")) - return false; -#endif - - if (!getHomographyMatParam(pnh, cam2world_, "cam2world")) - return false; - + bool init() { // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated - cam_info_sub_ = pnh.subscribe("/camera1/camera_info", 1, + cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + cam2world_, world2cam_, homography_received_)); } void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { @@ -171,16 +164,15 @@ class TransformHelper { bool worldToImage(const cv::Point3f& world_point, cv::Point& image_point) { -#ifdef USE_WORLD2CAM_HOMOGRAPHY - // using homography instead of camera model - cv::perspectiveTransform(world_point, image_point, world2cam_); - if (point_world.rows != 1 || point_world.cols != 1) { - ROS_WARN("Point transformed to image dimensions has invalid dimensions"); - return false; - } -#else - image_point = cam_model_.project3dToPixel(world_point); -#endif + // transform to image point and then repeat homography transform + cv::Point2d pixel_coords = cam_model_.project3dToPixel(world_point); + cv::Mat mat_pixel_coords(1,1,CV_64FC2,cv::Scalar(0.0)); + mat_pixel_coords.at(0,0) = pixel_coords.x; + mat_pixel_coords.at(0,1) = pixel_coords.y; + cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); + cv::perspectiveTransform(mat_pixel_coords, mat_image_point, world2cam_); + // todo: check if we lose precision here + image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); return true; } @@ -189,14 +181,16 @@ class TransformHelper { } bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point) { - cv::Mat mat_point_image(1,1,CV_64FC2); + cv::Mat mat_point_image(1,1,CV_64FC2,cv::Scalar(0.0)); mat_point_image.at(0,0) = image_point.x; mat_point_image.at(0,1) = image_point.y; - cv::Mat mat_point_world(1,1,CV_64FC2); + cv::Mat mat_point_world(1,1,CV_64FC2,cv::Scalar(0.0)); cv::perspectiveTransform(mat_point_image, mat_point_world, cam2world_); cv::Point2f cam_point(mat_point_world.at(0,0),mat_point_world.at(0,1)); - // todo: check if it is valid to assume the z depth if we used homography before + // this returns a point with z=1, the pixel is on a ray from origin to this point + // todo: set correct camera height and use it to calculate x and y of the world point + double cam_height = 10.0; cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); geometry_msgs::PointStamped camera_point; @@ -225,25 +219,23 @@ class TransformHelper { cam2world_ = cam2world; } + void setworld2camMat(const cv::Mat& world2cam) { + world2cam_ = world2cam; + } + // to subscribe to camerainfo messages image_geometry::PinholeCameraModel& getCameraModelReference() { return cam_model_; } -#ifdef USE_WORLD2CAM_HOMOGRAPHY - void setworld2camMat(const cv::Mat& world2cam) { - world2cam_ = world2cam; - } -#endif - private: image_geometry::PinholeCameraModel cam_model_; + bool homography_received_; cv::Mat cam2world_; - tf::TransformListener tf_listener_; -#ifdef USE_WORLD2CAM_HOMOGRAPHY cv::Mat world2cam_; -#endif + tf::TransformListener tf_listener_; ros::Subscriber cam_info_sub_; + ros::Subscriber homography_sub_; }; // class TransformHelper class ImageOperator { diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index e1dcfb1..52d134f 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -18,7 +18,6 @@ class WarpContent { WarpContent(const ros::NodeHandle &nh, const ros::NodeHandle& pnh); ~WarpContent(); bool init(); - void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in); private: void world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg); ros::NodeHandle pnh_; diff --git a/launch/road_detection.launch b/launch/road_detection.launch index bc41ed7..1dd61cd 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -1,10 +1,12 @@ + + diff --git a/src/road_detection.cpp b/src/road_detection.cpp index 4dac63e..59102a4 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -80,7 +80,7 @@ bool RoadDetection::init() { ROS_WARN_STREAM("Unable to load 'useWeights' parameter, using default: "<("homography_in", 1, + boost::bind(homography_callback, _1, + cam2world_, world2cam_, homo_received_)); return true; } @@ -79,30 +76,6 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, // img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); } -// bind cannot resolve to references -void WarpContent::homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in) { - if (!homo_received_) - homo_received_ = true; - - ROS_ASSERT(homo_in->cam2world.layout.dim[0].size == 3 && homo_in->cam2world.layout.dim[1].size == 3 ); - cam2world_ = cv::Mat::zeros(3,3,CV_64FC1); - int k=0; - for (int i=0; icam2world.layout.dim[0].size; i++){ - for (int j=0; jcam2world.layout.dim[1].size; j++){ - cam2world_.at(i,j) = homo_in->cam2world.data[k++]; - } - } - - ROS_ASSERT(homo_in->world2cam.layout.dim[0].size == 3 && homo_in->world2cam.layout.dim[1].size == 3 ); - world2cam_ = cv::Mat::zeros(3,3,CV_64FC1); - k=0; - for (int i=0; iworld2cam.layout.dim[0].size; i++){ - for (int j=0; jworld2cam.layout.dim[1].size; j++){ - world2cam_.at(i,j) = homo_in->world2cam.data[k++]; - } - } -} - void WarpImageNodelet::onInit() { my_content_.reset(new WarpContent(getNodeHandle(),ros::NodeHandle(getPrivateNodeHandle()))); From 4052f84fafd8e7c535b99ae97ac1a53afca385fc Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 14 Oct 2017 12:13:34 +0200 Subject: [PATCH 68/94] updated camera_info topic name; please redownload your bags to get updated bags with new naming --- launch/road_detection.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/road_detection.launch b/launch/road_detection.launch index 1dd61cd..9d84c72 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -1,7 +1,7 @@ - + From f02d1e185804291472cf1177b1f94e96b21ab49a Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 14 Oct 2017 12:17:16 +0200 Subject: [PATCH 69/94] fix launch file --- launch/road_detection.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/road_detection.launch b/launch/road_detection.launch index 9d84c72..e7868b1 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -6,7 +6,7 @@ - + From 72143380dc0f3858b089877eaec26a5bc568f02b Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Mon, 16 Oct 2017 01:17:15 +0200 Subject: [PATCH 70/94] Reorganized and fixed launch files, fixed missing references in subscription, homography scaling added --- CMakeLists.txt | 4 +- .../common_image_operations.h | 5 +- launch/road_detection.launch | 41 ++++++++-- launch/road_detection_debug.launch | 12 --- launch/road_detection_nodelet.launch | 31 ------- launch/street_crossing.launch | 24 +++++- launch/street_crossing_nodelet.launch | 22 ----- launch/warp_image.launch | 36 +++++++-- launch/warp_image_nodelet.launch | 29 ------- src/road_detection.cpp | 2 +- src/warp_image.cpp | 80 ++++++++++++++++--- 11 files changed, 161 insertions(+), 125 deletions(-) delete mode 100644 launch/road_detection_debug.launch delete mode 100644 launch/road_detection_nodelet.launch delete mode 100644 launch/street_crossing_nodelet.launch delete mode 100644 launch/warp_image_nodelet.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c10e1b..10d9dad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -207,11 +207,11 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch FILES_MATCHING PATTERN "*.launch" ) install(DIRECTORY config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config FILES_MATCHING PATTERN "*.yaml" ) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 9b460c0..06e2570 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -116,7 +116,7 @@ class TransformHelper { homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, - cam2world_, world2cam_, homography_received_)); + std::ref(cam2world_), std::ref(world2cam_), std::ref(homography_received_))); other.cam_info_sub_.shutdown(); return *this; } @@ -192,6 +192,9 @@ class TransformHelper { // todo: set correct camera height and use it to calculate x and y of the world point double cam_height = 10.0; cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); + cam_ray_point.x = cam_ray_point.x*cam_height; + cam_ray_point.y = cam_ray_point.y*cam_height; + cam_ray_point.z = cam_height; geometry_msgs::PointStamped camera_point; camera_point.header.frame_id = cam_model_.tfFrame(); diff --git a/launch/road_detection.launch b/launch/road_detection.launch index e7868b1..712fef6 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -2,13 +2,38 @@ + + + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/road_detection_debug.launch b/launch/road_detection_debug.launch deleted file mode 100644 index bc41ed7..0000000 --- a/launch/road_detection_debug.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - diff --git a/launch/road_detection_nodelet.launch b/launch/road_detection_nodelet.launch deleted file mode 100644 index 2b08469..0000000 --- a/launch/road_detection_nodelet.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch index 10a7269..58778b5 100644 --- a/launch/street_crossing.launch +++ b/launch/street_crossing.launch @@ -1,7 +1,25 @@ + + + - - - + + + + + + + + + + + + + + + + + + diff --git a/launch/street_crossing_nodelet.launch b/launch/street_crossing_nodelet.launch deleted file mode 100644 index 761270c..0000000 --- a/launch/street_crossing_nodelet.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/warp_image.launch b/launch/warp_image.launch index eea7d51..50d6d82 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -2,11 +2,35 @@ + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/warp_image_nodelet.launch b/launch/warp_image_nodelet.launch deleted file mode 100644 index 87b711b..0000000 --- a/launch/warp_image_nodelet.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/road_detection.cpp b/src/road_detection.cpp index 59102a4..0c4d2b1 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -20,7 +20,7 @@ RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh translateEnvironment_(false), useWeights_(false), line_output_pub_(), - it_(pnh), + it_(nh), #ifdef PUBLISH_DEBUG debug_img_pub_(), detected_points_pub_(), diff --git a/src/warp_image.cpp b/src/warp_image.cpp index caf923b..4b0972a 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -9,7 +9,7 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): current_image_(), cam_mat_(3,3,CV_64F,cv::Scalar(0.0)), dist_coeffs_(8,1,CV_64F,cv::Scalar(0.0)), - it_(pnh), + it_(nh), cam_sub_(), worldToImageServer_(), imageToWorldServer_(), @@ -27,13 +27,11 @@ WarpContent::~WarpContent() { } bool WarpContent::init() { + homography_params_sub_ = nh_.subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + std::ref(cam2world_), std::ref(world2cam_), std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); - -// homography_params_sub_ = pnh_.subscribe("homography_in", 1, &WarpContent::homography_callback, this); - homography_params_sub_ = pnh_.subscribe("homography_in", 1, - boost::bind(homography_callback, _1, - cam2world_, world2cam_, homo_received_)); return true; } @@ -58,9 +56,11 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, cam_model_.fromCameraInfo(info_msg); // optionally: scale homography, as image is too small -// cv::Rect roi(cv::Point(current_image_.cols/2-(410/2),0),cv::Point(current_image_.cols/2+(410/2),current_image_.rows)); +// cv::Rect roi(cv::Point(current_image_.cols/2-(512/2),0),cv::Point(current_image_.cols/2+(512/2),current_image_.rows)); // current_image_ = current_image_(roi); -// cv::Mat S = cv::Mat::eye(3,3,CV_64F); + cv::Mat S = cv::Mat::eye(3,3,CV_64F); + S.at(0,0) = 0.007; + S.at(1,1) = 0.007; // S.at(0,0) = 410/current_image_.rows; // S.at(1,1) = 752/current_image_.cols; @@ -72,8 +72,68 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); // flag ensures that we directly use the matrix, as it is done in LMS - cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); -// img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); +// cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); +// cv::Mat output_mat; +// // Choose top-view image size +// cv::Size topViewSize = cv::Size(512, 512); + +// // Choose corner points (in real-world coordinates) +// std::vector coordinates; +// coordinates.emplace_back(0, -1.500); +// coordinates.emplace_back(0, 1.500); +// coordinates.emplace_back(3.000, -1.500); +// coordinates.emplace_back(3.000, 1.500); + +// std::vector pixels; +// pixels.emplace_back(0, topViewSize.height); +// pixels.emplace_back(0, 0); +// pixels.emplace_back(topViewSize.width, topViewSize.height); +// pixels.emplace_back(topViewSize.width, 0); + +// cv::Mat H = cv::findHomography(pixels, coordinates); +// cv::Mat trafo_mat = world2cam_ * H; + +// cv::Size topViewSize = current_image_.size(); + +// std::vector coordinates; +// coordinates.emplace_back(0, -1.500); +// coordinates.emplace_back(0, 1.500); +// coordinates.emplace_back(3.000, -1.500); +// coordinates.emplace_back(3.000, 1.500); + +// std::vector pixels; +// pixels.emplace_back(0, topViewSize.height); +// pixels.emplace_back(0, 0); +// pixels.emplace_back(topViewSize.width, topViewSize.height); +// pixels.emplace_back(topViewSize.width, 0); + +// cv::Mat H = cv::findHomography(pixels, coordinates); + + +// cv::Mat S = cv::Mat::eye(3,3,CV_64F); +// S.at(0,0) = 410/current_image_.rows; +// S.at(1,1) = 752/current_image_.cols; + cv::Mat output_mat(current_image_.size(),CV_8UC1,cv::Scalar(0)); + +// for(int i = 0; i < current_image_.rows; i++) +// { +// const double* Mi = current_image_.ptr(i); +// for(int j = 0; j < current_image_.cols; j++) { +// float a = i * cam2world_.data[0] + j * cam2world_.data[1] + cam2world_.data[2]; +// float b = i * cam2world_.data[3] + j * cam2world_.data[4] + cam2world_.data[5]; +// float c = i * cam2world_.data[6] + j * cam2world_.data[7] + cam2world_.data[8]; +// int x = a / c; +// int y = b / c; +//// ROS_INFO_STREAM("x: "<(x,y) = Mi[j]; +// } +// } + + cv::warpPerspective(undistorted_mat, output_mat, world2cam_*S, current_image_.size(), cv::WARP_INVERSE_MAP); + cv::namedWindow("Homographied",CV_WINDOW_NORMAL); + cv::imshow("Homographied",output_mat); + cv::waitKey(1); + img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, output_mat).toImageMsg()); } void WarpImageNodelet::onInit() From e2f67906de06375e9d2e617a1a2bfa6c263115a5 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sun, 22 Oct 2017 23:46:47 +0200 Subject: [PATCH 71/94] Fixed homography scaling, removed unneeded dependencies --- CMakeLists.txt | 4 +- config/homography.yaml | 2 + config/homography_matrix.yaml | 3 - .../common_image_operations.h | 40 ++++++- .../drive_ros_image_recognition/warp_image.h | 2 + launch/road_detection.launch | 5 + launch/warp_image.launch | 2 + package.xml | 2 - scripts/publish_road.sh | 0 src/road_detection.cpp | 2 +- src/warp_image.cpp | 105 ++++++------------ 11 files changed, 81 insertions(+), 86 deletions(-) create mode 100644 config/homography.yaml delete mode 100644 config/homography_matrix.yaml mode change 100755 => 100644 scripts/publish_road.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 10d9dad..4d408a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(drive_ros_image_recognition) SET(CMAKE_BUILD_TYPE Debug) # enable to compile with OpenCV window displays -add_definitions(-DDRAW_DEBUG) +#add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images #add_definitions(-DPUBLISH_DEBUG) # enable to use World2Cam homography @@ -23,10 +23,8 @@ find_package(catkin REQUIRED COMPONENTS drive_ros_msgs dynamic_reconfigure geometry_msgs - message_generation nodelet tf - message_filters ) #include Boost for lane line processing diff --git a/config/homography.yaml b/config/homography.yaml new file mode 100644 index 0000000..66b2425 --- /dev/null +++ b/config/homography.yaml @@ -0,0 +1,2 @@ +world_size: [5.0, 5.0] +image_size: [512, 512] diff --git a/config/homography_matrix.yaml b/config/homography_matrix.yaml deleted file mode 100644 index 19963a5..0000000 --- a/config/homography_matrix.yaml +++ /dev/null @@ -1,3 +0,0 @@ -homography_matrix: - world2cam: [-1076.85,677.839,-97.8148,-543.74,26.5117,-240.371,-1.65943,0.0135395,-0.154656] - cam2world: [-4.01363e-06,0.00049125,-0.760979,0.00149399,2.0045e-05,-0.976056,0.000173859,-0.00526928,1.61375] diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 06e2570..682c7bd 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -39,7 +39,7 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) namespace drive_ros_image_recognition { inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in, cv::Mat& cam2world, cv::Mat& world2cam, - bool& homography_received) { + cv::Mat& scaling_mat, bool& homography_received) { if (!homography_received) homography_received = true; @@ -60,6 +60,8 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i world2cam.at(i,j) = homo_in->world2cam.data[k++]; } } + + scaling_mat = world2cam*scaling_mat; } inline void cam_info_sub(const sensor_msgs::CameraInfoConstPtr& incoming_cam_info, image_geometry::PinholeCameraModel& cam_model) { @@ -114,9 +116,10 @@ class TransformHelper { boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); - homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, - boost::bind(homography_callback, _1, - std::ref(cam2world_), std::ref(world2cam_), std::ref(homography_received_))); +// homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, +// boost::bind(homography_callback, _1, +// std::ref(cam2world_), std::ref(world2cam_), +// std::ref(scaling_mat_), std::ref(homography_received_))); other.cam_info_sub_.shutdown(); return *this; } @@ -133,12 +136,15 @@ class TransformHelper { getCameraModelReference()) ); homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, - cam2world_, world2cam_, homography_received_)); + std::ref(cam2world_), std::ref(world2cam_), + std::ref(scaling_mat_), std::ref(homography_received_))); } TransformHelper(const image_geometry::PinholeCameraModel& cam_model): cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), + scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), + transformed_size_(0,0), cam_model_(cam_model), tf_listener_(), cam_info_sub_() @@ -149,13 +155,33 @@ class TransformHelper { } bool init() { + ros::NodeHandle pnh = ros::NodeHandle("~"); + std::vector world_size; + if(!pnh.getParam("world_size", world_size)) { + ROS_ERROR("Unable to load parameter world_size!"); + return false; + } + ROS_ASSERT(world_size.size() == 2); + std::vector image_size; + if(!pnh.getParam("image_size", image_size)) { + ROS_ERROR("Unable to load parameter world_size!"); + return false; + } + ROS_ASSERT(image_size.size() == 2); + transformed_size_ = cv::Size(image_size[0],image_size[1]); + scaling_mat_.at(0,0) = world_size[0]/image_size[0]; + scaling_mat_.at(1,1) = -world_size[1]/image_size[1]; + scaling_mat_.at(1,2) = world_size[1]/2; + scaling_mat_.at(2,2) = 1.0; + // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, boost::bind(&cam_info_sub, _1, getCameraModelReference()) ); homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, - cam2world_, world2cam_, homography_received_)); + std::ref(cam2world_), std::ref(world2cam_), + std::ref(scaling_mat_), std::ref(homography_received_))); } void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { @@ -236,6 +262,8 @@ class TransformHelper { bool homography_received_; cv::Mat cam2world_; cv::Mat world2cam_; + cv::Mat scaling_mat_; + cv::Size transformed_size_; tf::TransformListener tf_listener_; ros::Subscriber cam_info_sub_; ros::Subscriber homography_sub_; diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 52d134f..6e702a8 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -41,6 +41,8 @@ class WarpContent { ros::Subscriber homography_params_sub_; cv::Mat world2cam_; cv::Mat cam2world_; + cv::Mat scaling_mat_; + cv::Size transformed_size_; bool homo_received_; }; diff --git a/launch/road_detection.launch b/launch/road_detection.launch index 712fef6..cb4d739 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -3,6 +3,7 @@ + @@ -16,9 +17,11 @@ + + @@ -31,9 +34,11 @@ + + diff --git a/launch/warp_image.launch b/launch/warp_image.launch index 50d6d82..b1a8e8a 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -17,6 +17,7 @@ + @@ -31,6 +32,7 @@ + diff --git a/package.xml b/package.xml index b10de34..1de3705 100644 --- a/package.xml +++ b/package.xml @@ -50,7 +50,6 @@ nodelet geometry_msgs tf - message_filters drive_ros_msgs cv_bridge @@ -63,7 +62,6 @@ nodelet geometry_msgs tf - message_filters drive_ros_msgs diff --git a/scripts/publish_road.sh b/scripts/publish_road.sh old mode 100755 new mode 100644 diff --git a/src/road_detection.cpp b/src/road_detection.cpp index 0c4d2b1..67d57e6 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -94,7 +94,7 @@ bool RoadDetection::init() { img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); - line_output_pub_ = pnh_.advertise("line_out",10); + line_output_pub_ = nh_.advertise("line_out",10); #ifdef PUBLISH_DEBUG debug_img_pub_ = it_.advertise("debug_image_out", 10); diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 4b0972a..2ec3d6d 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -17,6 +17,8 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): homography_params_sub_(), cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), + scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), + transformed_size_(0,0), homo_received_(false) { img_pub_ = it_.advertise("warped_out", 1); @@ -27,9 +29,33 @@ WarpContent::~WarpContent() { } bool WarpContent::init() { + std::vector world_size; + double test; + ROS_INFO_STREAM("PNH namespace: "< image_size; + if(!pnh_.getParam("image_size", image_size)) { + ROS_ERROR("Unable to load parameter image_size!"); + return false; + } + ROS_ASSERT(image_size.size() == 2); + transformed_size_ = cv::Size(image_size[0],image_size[1]); + scaling_mat_.at(0,0) = world_size[0]/image_size[0]; + scaling_mat_.at(1,1) = -world_size[1]/image_size[1]; + scaling_mat_.at(1,2) = world_size[1]/2; + scaling_mat_.at(2,2) = 1.0; + ROS_INFO_STREAM("Calculated world2mat scaling matrix: "<("homography_in", 1, boost::bind(homography_callback, _1, - std::ref(cam2world_), std::ref(world2cam_), std::ref(homo_received_))); + std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), + std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); return true; @@ -53,86 +79,23 @@ void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, return; } - cam_model_.fromCameraInfo(info_msg); - - // optionally: scale homography, as image is too small -// cv::Rect roi(cv::Point(current_image_.cols/2-(512/2),0),cv::Point(current_image_.cols/2+(512/2),current_image_.rows)); -// current_image_ = current_image_(roi); - cv::Mat S = cv::Mat::eye(3,3,CV_64F); - S.at(0,0) = 0.007; - S.at(1,1) = 0.007; -// S.at(0,0) = 410/current_image_.rows; -// S.at(1,1) = 752/current_image_.cols; + cam_model_.fromCameraInfo(info_msg); // undistort and apply homography transformation cv::Mat undistorted_mat; cv::undistort(current_image_, undistorted_mat, cam_model_.fullIntrinsicMatrix(), cam_model_.distortionCoeffs()); undistort_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); - // opionally: apply scaled homography - // cv::warpPerspective(current_image_, current_image_, S*world2cam_*S.inv(), current_image_.size(),cv::WARP_INVERSE_MAP); - - // flag ensures that we directly use the matrix, as it is done in LMS -// cv::warpPerspective(undistorted_mat, undistorted_mat, world2cam_, current_image_.size(),cv::WARP_INVERSE_MAP); -// cv::Mat output_mat; -// // Choose top-view image size -// cv::Size topViewSize = cv::Size(512, 512); - -// // Choose corner points (in real-world coordinates) -// std::vector coordinates; -// coordinates.emplace_back(0, -1.500); -// coordinates.emplace_back(0, 1.500); -// coordinates.emplace_back(3.000, -1.500); -// coordinates.emplace_back(3.000, 1.500); - -// std::vector pixels; -// pixels.emplace_back(0, topViewSize.height); -// pixels.emplace_back(0, 0); -// pixels.emplace_back(topViewSize.width, topViewSize.height); -// pixels.emplace_back(topViewSize.width, 0); -// cv::Mat H = cv::findHomography(pixels, coordinates); -// cv::Mat trafo_mat = world2cam_ * H; +// cv::Mat topView2cam = world2cam_ * pixels_to_meters; -// cv::Size topViewSize = current_image_.size(); - -// std::vector coordinates; -// coordinates.emplace_back(0, -1.500); -// coordinates.emplace_back(0, 1.500); -// coordinates.emplace_back(3.000, -1.500); -// coordinates.emplace_back(3.000, 1.500); - -// std::vector pixels; -// pixels.emplace_back(0, topViewSize.height); -// pixels.emplace_back(0, 0); -// pixels.emplace_back(topViewSize.width, topViewSize.height); -// pixels.emplace_back(topViewSize.width, 0); - -// cv::Mat H = cv::findHomography(pixels, coordinates); - - -// cv::Mat S = cv::Mat::eye(3,3,CV_64F); -// S.at(0,0) = 410/current_image_.rows; -// S.at(1,1) = 752/current_image_.cols; - cv::Mat output_mat(current_image_.size(),CV_8UC1,cv::Scalar(0)); - -// for(int i = 0; i < current_image_.rows; i++) -// { -// const double* Mi = current_image_.ptr(i); -// for(int j = 0; j < current_image_.cols; j++) { -// float a = i * cam2world_.data[0] + j * cam2world_.data[1] + cam2world_.data[2]; -// float b = i * cam2world_.data[3] + j * cam2world_.data[4] + cam2world_.data[5]; -// float c = i * cam2world_.data[6] + j * cam2world_.data[7] + cam2world_.data[8]; -// int x = a / c; -// int y = b / c; -//// ROS_INFO_STREAM("x: "<(x,y) = Mi[j]; -// } -// } - - cv::warpPerspective(undistorted_mat, output_mat, world2cam_*S, current_image_.size(), cv::WARP_INVERSE_MAP); + // flag ensures that we directly use the matrix, as it is done in LMS + cv::Mat output_mat; + cv::warpPerspective(undistorted_mat, output_mat, scaling_mat_, transformed_size_, cv::WARP_INVERSE_MAP); +#ifdef DRAW_DEBUG cv::namedWindow("Homographied",CV_WINDOW_NORMAL); cv::imshow("Homographied",output_mat); cv::waitKey(1); +#endif img_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, output_mat).toImageMsg()); } From f9e19f33262277ac0ef836085d5bf066bc3c6b4a Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Thu, 2 Nov 2017 00:55:26 +0100 Subject: [PATCH 72/94] Fixed transform callbacks, fixed duplicate callbacks due to helper setup, added test for transform callbacks, added some debug callbacks --- CMakeLists.txt | 4 +- .../common_image_operations.h | 203 +++++++++++++----- .../road_detection.h | 7 +- .../drive_ros_image_recognition/warp_image.h | 1 + launch/road_detection.launch | 10 +- src/road_detection.cpp | 73 ++++++- src/warp_image.cpp | 3 +- test/common_image_operations_test.cpp | 37 +++- 8 files changed, 267 insertions(+), 71 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d408a1..18d7f97 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,11 +5,13 @@ project(drive_ros_image_recognition) SET(CMAKE_BUILD_TYPE Debug) # enable to compile with OpenCV window displays -#add_definitions(-DDRAW_DEBUG) +add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images #add_definitions(-DPUBLISH_DEBUG) # enable to use World2Cam homography #add_definitions(-DUSE_WORLD2CAM_HOMOGRAPHY) +# enable publishing of detected points (for rviz display) +#add_definitions(-DPUBLISH_WORLD_POINTS) add_definitions(-std=c++11) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 682c7bd..09e93d7 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -39,7 +39,7 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) namespace drive_ros_image_recognition { inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in, cv::Mat& cam2world, cv::Mat& world2cam, - cv::Mat& scaling_mat, bool& homography_received) { + cv::Mat& scaling_mat, cv::Mat& scaling_mat_inv, bool& homography_received) { if (!homography_received) homography_received = true; @@ -61,11 +61,23 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i } } + ROS_INFO_STREAM("Scaling mat: "<("camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); + boost::bind(&camInfo_callback, _1, + std::ref(cam_model_), + std::ref(camera_model_received_) ) ); + + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, + boost::bind(homography_callback, _1, + std::ref(cam2world_), std::ref(world2cam_), + std::ref(scaling_mat_), std::ref(scaling_mat_inv_), + std::ref(homography_received_))); -// homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, -// boost::bind(homography_callback, _1, -// std::ref(cam2world_), std::ref(world2cam_), -// std::ref(scaling_mat_), std::ref(homography_received_))); other.cam_info_sub_.shutdown(); + other.homography_sub_.shutdown(); return *this; } @@ -131,23 +157,32 @@ class TransformHelper { cam2world_ = helper.cam2world_; world2cam_ = helper.world2cam_; cam_model_ = helper.cam_model_; + world_frame_ = helper.world_frame_; + camera_frame_ = helper.camera_frame_; cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); + boost::bind(&camInfo_callback, _1, + std::ref(cam_model_), + std::ref(camera_model_received_))); homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), - std::ref(scaling_mat_), std::ref(homography_received_))); + std::ref(scaling_mat_), std::ref(scaling_mat_inv_), + std::ref(homography_received_))); } TransformHelper(const image_geometry::PinholeCameraModel& cam_model): cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), + scaling_mat_inv_(3,3,CV_64FC1,cv::Scalar(0.0)), transformed_size_(0,0), cam_model_(cam_model), tf_listener_(), - cam_info_sub_() + cam_info_sub_(), + camera_model_received_(false), + // temporary solution until have embedded correct frame in camerainfo messages (including the bag files) + camera_frame_(""), + world_frame_("") { } @@ -176,28 +211,71 @@ class TransformHelper { // subscribe Camera model to TransformHelper-> this is kind of hacky, but should keep the camera model on the transform helper updated cam_info_sub_ = ros::NodeHandle().subscribe("camera_info", 1, - boost::bind(&cam_info_sub, _1, - getCameraModelReference()) ); + boost::bind(&camInfo_callback, _1, + std::ref(cam_model_), + std::ref(camera_model_received_) ) ); homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), - std::ref(scaling_mat_), std::ref(homography_received_))); + std::ref(scaling_mat_), std::ref(scaling_mat_inv_), + std::ref(homography_received_))); } void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { cam_model_ = cam_model; } + void setWorldFrame(const std::string& frame) { + if (camera_frame_ != std::string("")) { + try { + ros::Duration timeout(1.0); + tf_listener_.waitForTransform(camera_frame_, frame, + ros::Time::now(), timeout); + } + catch (tf::TransformException& ex) { + ROS_WARN("[TransformHelper] TF exception, unable to find transform to new camera frame %s, will not set it, reason:\n%s", frame.c_str(), ex.what()); + return; + } + } + world_frame_ = frame; + } + + // placeholder until we get updated bags with camera messages + // cannot access PinHoleCameraModel after it has been created, so just use a string instead + void setCameraFrame(const std::string& frame) { + if (world_frame_ != std::string("")) { + try { + ros::Duration timeout(1.0); + tf_listener_.waitForTransform(world_frame_, frame, + ros::Time::now(), timeout); + } + catch (tf::TransformException& ex) { + ROS_WARN("[TransformHelper] TF exception, unable to find transform to new world frame %s, will not set it, reason:\n%s", frame.c_str(), ex.what()); + return; + } + } + camera_frame_ = frame; + } + bool worldToImage(const cv::Point3f& world_point, cv::Point& image_point) { + + if(!homography_received_ || !camera_model_received_) { + if (!homography_received_) + ROS_WARN_STREAM("[WorldToImage] Homography not received yet, skipping world to image transformation requested for world point "<(0,0) = pixel_coords.x; mat_pixel_coords.at(0,1) = pixel_coords.y; cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); - cv::perspectiveTransform(mat_pixel_coords, mat_image_point, world2cam_); - // todo: check if we lose precision here + cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_inv_); image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); return true; } @@ -206,42 +284,61 @@ class TransformHelper { return worldToImage(cv::Point3f(world_point.x, world_point.y, 0.0f), image_point); } - bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point) { - cv::Mat mat_point_image(1,1,CV_64FC2,cv::Scalar(0.0)); - mat_point_image.at(0,0) = image_point.x; - mat_point_image.at(0,1) = image_point.y; - cv::Mat mat_point_world(1,1,CV_64FC2,cv::Scalar(0.0)); - cv::perspectiveTransform(mat_point_image, mat_point_world, cam2world_); - cv::Point2f cam_point(mat_point_world.at(0,0),mat_point_world.at(0,1)); + bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point, std::string target_frame = std::string("/rear_axis_middle_ground"), cv::Mat test_image = cv::Mat()) { + if(!homography_received_ || !camera_model_received_) { + if (!homography_received_) + ROS_WARN_STREAM("[ImageToWorld] Homography not received yet, skipping image to world transformation requested for image point "< obj_corners(1); + obj_corners[0] = cv::Point2f(image_point.x, image_point.y); + std::vector scene_corners(1); + cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_); // this returns a point with z=1, the pixel is on a ray from origin to this point - // todo: set correct camera height and use it to calculate x and y of the world point - double cam_height = 10.0; - cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(cam_point); - cam_ray_point.x = cam_ray_point.x*cam_height; - cam_ray_point.y = cam_ray_point.y*cam_height; - cam_ray_point.z = cam_height; + cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(scene_corners[0]); + // this gives us a ray defined by two points, now we transform to the fixed frame + // where we can easily define the road plane and intersect the ray with it to get the world point geometry_msgs::PointStamped camera_point; - camera_point.header.frame_id = cam_model_.tfFrame(); + camera_point.header.frame_id = camera_frame_; camera_point.point.x = cam_ray_point.x; camera_point.point.y = cam_ray_point.y; camera_point.point.z = cam_ray_point.z; - geometry_msgs::PointStamped geom_world_point; - + geometry_msgs::PointStamped camera_point_world; + tf::StampedTransform transform; try { - ros::Duration timeout(1.0 / 30); - tf_listener_.waitForTransform(cam_model_.tfFrame(), "tf_front_axis_middle", - ros::Time::now(), timeout); - tf_listener_.transformPoint("tf_front_axis_middle", camera_point, geom_world_point); + tf_listener_.transformPoint(target_frame, camera_point, camera_point_world); + tf_listener_.lookupTransform(target_frame, camera_frame_, + ros::Time::now(), transform); } catch (tf::TransformException& ex) { ROS_WARN("[ImageToWorld] TF exception, unable to transform:\n%s", ex.what()); return false; } - world_point.x = geom_world_point.point.x; - world_point.y = geom_world_point.point.y; - return true; + + // ray-plane intersection (plane is defined by P=[0 0 0] and n=[0 0 1] + tf::Point ray_point_tf; + tf::Vector3 plane_normal(0,0,1); + tf::Vector3 plane_point(0,0,0); + tf::pointMsgToTF(camera_point_world.point, ray_point_tf); + tf::Vector3 ray_dir = ray_point_tf - transform.getOrigin(); + double denom = plane_normal.dot(ray_dir); + + if (std::abs(denom) > 1e-9) { + double scale_axis = (plane_point-transform.getOrigin()).dot(plane_normal)/denom; + tf::Vector3 return_point = transform.getOrigin()+scale_axis*ray_dir; + world_point.x = return_point.x(); + world_point.y = return_point.y(); + return true; + } + else { + ROS_WARN_STREAM("[imageToWorld] Camera ray and ground plane are parallel, unable to transform image point "< pxlPeakWidth*config_.minLineWidthMul && lineWidths[i] < pxlPeakWidth*config_.maxLineWidthMul) { - helper_.imageToWorld(imagePoints[i], wMid); + imageToWorld(imagePoints[i], wMid); foundPoints.push_back(wMid); } } @@ -501,7 +597,6 @@ class ImageOperator { } private: - TransformHelper helper_; LineDetectionConfig config_; }; // class ImageOperator diff --git a/include/drive_ros_image_recognition/road_detection.h b/include/drive_ros_image_recognition/road_detection.h index 75fac14..f6124bc 100644 --- a/include/drive_ros_image_recognition/road_detection.h +++ b/include/drive_ros_image_recognition/road_detection.h @@ -77,11 +77,16 @@ class RoadDetection { dynamic_reconfigure::Server dsrv_server_; dynamic_reconfigure::Server::CallbackType dsrv_cb_; void debugImageCallback(const sensor_msgs::ImageConstPtr& img_in); + void debugDrawFrameCallback(const sensor_msgs::ImageConstPtr& img_in, + const std::string camera_frame, + const std::string draw_frame); void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in); void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); bool find(); void processSearchLine(const SearchLine &line); - TransformHelper transform_helper_; +#ifdef PUBLISH_WORLD_POINTS + ros::Publisher world_point_pub; +#endif ImageOperator image_operator_; public: diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 6e702a8..b15520f 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -42,6 +42,7 @@ class WarpContent { cv::Mat world2cam_; cv::Mat cam2world_; cv::Mat scaling_mat_; + cv::Mat scaling_mat_inv_; cv::Size transformed_size_; bool homo_received_; }; diff --git a/launch/road_detection.launch b/launch/road_detection.launch index cb4d739..839f54b 100644 --- a/launch/road_detection.launch +++ b/launch/road_detection.launch @@ -4,6 +4,8 @@ + + @@ -16,8 +18,10 @@ - + + + @@ -33,8 +37,10 @@ - + + + diff --git a/src/road_detection.cpp b/src/road_detection.cpp index 67d57e6..bae5921 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -32,9 +32,11 @@ RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh linesToProcess_(0), current_image_(), road_points_buffer_(), - transform_helper_(), image_operator_(), img_sub_debug_() +#ifdef PUBLISH_WORLD_POINTS + ,world_point_pub() +#endif { } @@ -80,11 +82,28 @@ bool RoadDetection::init() { ROS_WARN_STREAM("Unable to load 'useWeights' parameter, using default: "<("worldPoints", 1000); +#endif + // to synchronize incoming images and the road, we use message filters // img_sub_.reset(new image_transport::SubscriberFilter(it_,"img_in", 1000)); // road_sub_.reset(new message_filters::Subscriber(pnh_,"road_in", 1000)); @@ -93,6 +112,8 @@ bool RoadDetection::init() { // sync_->registerCallback(boost::bind(&RoadDetection::syncCallback, this, _1, _2)); img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); +// img_sub_debug_ = it_.subscribe("img_in", 1000, boost::bind(&RoadDetection::debugDrawFrameCallback, this, +// _1, std::string("/camera_optical"), std::string("/front_axis_middle"))); line_output_pub_ = nh_.advertise("line_out",10); @@ -102,7 +123,6 @@ bool RoadDetection::init() { filtered_points_pub_ = it_.advertise("filtered_points_out", 10); #endif - // todo: we have not decided on an interface for these debug channels yet // debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); // debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); @@ -124,6 +144,39 @@ void RoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth_brightness); } +// draws draw_frame in camera image (if visible) +void RoadDetection::debugDrawFrameCallback(const sensor_msgs::ImageConstPtr& img_in, + const std::string camera_frame, + const std::string draw_frame) { + current_image_ = convertImageMessage(img_in); + + tf::TransformListener tf_listener_; + tf::StampedTransform transform; + try { + ros::Duration timeout(1); + tf_listener_.waitForTransform(camera_frame, draw_frame, + ros::Time::now(), timeout); + tf_listener_.lookupTransform(camera_frame, draw_frame, + ros::Time::now(), transform); + } + catch (tf::TransformException& ex) { + ROS_WARN("[debugDrawFrameCallback] TF exception:\n%s", ex.what()); + return; + } + + tf::Point pt = transform.getOrigin(); + cv::Point3f pt_cv(pt.x(), pt.y(), pt.z()); + cv::Point uv; + image_operator_.worldToImage(pt_cv, uv); + + cv::Mat draw_image; + cv::cvtColor(*current_image_, draw_image, cv::COLOR_GRAY2BGR); + cv::circle(draw_image, uv, 3, CV_RGB(255,0,0), -1); + cv::namedWindow("frame in image", CV_WINDOW_NORMAL); + cv::imshow("frame in image", draw_image); + cv::waitKey(1); +} + void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in){ current_image_ = convertImageMessage(img_in); road_points_buffer_ = road_in->points; @@ -204,14 +257,14 @@ bool RoadDetection::find(){ //check if the part is valid (middle should be always visible) cv::Point l_w_start, l_w_end; - transform_helper_.worldToImage(l.wStart, l.iStart); - transform_helper_.worldToImage(l.wEnd, l.iEnd); + image_operator_.worldToImage(l.wStart, l.iStart); + image_operator_.worldToImage(l.wEnd, l.iEnd); cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); // if the left side is out of the current view, use middle instead if(!img_rect.contains(l_w_start)){ // try middle lane -> should always be in image l.wStart = cv::Point2f(it_mid->x(),it_mid->y()); - transform_helper_.worldToImage(l.wMid, l.iStart); + image_operator_.worldToImage(l.wMid, l.iStart); if(img_rect.contains(l_w_start)){ continue; } @@ -220,8 +273,8 @@ bool RoadDetection::find(){ l.wEnd = cv::Point2f(it_mid->x(),it_mid->y()); } - transform_helper_.worldToImage(l.wStart,l.iStart); - transform_helper_.worldToImage(l.wEnd,l.iEnd); + image_operator_.worldToImage(l.wStart,l.iStart); + image_operator_.worldToImage(l.wEnd,l.iEnd); lines_.push_back(l); for(SearchLine &l:lines_){ processSearchLine(l); @@ -323,7 +376,7 @@ void RoadDetection::processSearchLine(const SearchLine &l) { cv::Mat filtered_points_mat = current_image_->clone(); cv::Point img_point; for (auto point : validPoints) { - transform_helper_.worldToImage(point, img_point); + image_operator_.worldToImage(point, img_point); cv::circle(filtered_points_mat, img_point, 2, cv::Scalar(255)); } #ifdef DRAW_DEBUG diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 2ec3d6d..efeb04a 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -18,6 +18,7 @@ WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), + scaling_mat_inv_(3,3,CV_64FC1,cv::Scalar(0.0)), transformed_size_(0,0), homo_received_(false) { @@ -55,7 +56,7 @@ bool WarpContent::init() { homography_params_sub_ = nh_.subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), - std::ref(homo_received_))); + std::ref(scaling_mat_inv_), std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); return true; diff --git a/test/common_image_operations_test.cpp b/test/common_image_operations_test.cpp index 5924139..89cbdf1 100644 --- a/test/common_image_operations_test.cpp +++ b/test/common_image_operations_test.cpp @@ -1,9 +1,42 @@ #include #include "drive_ros_image_recognition/common_image_operations.h" - +#include // Declare test TEST(CommonImageOperations, testCase0) { - EXPECT_EQ(1,1); + ros::NodeHandle pnh("~"); + std::string world_frame("/rear_axis_middle_ground"); + if (!pnh.getParam("world_frame", world_frame)) + ROS_WARN_STREAM("Parameter 'world_frame' not found, using default: "< Date: Fri, 3 Nov 2017 15:34:52 +0100 Subject: [PATCH 73/94] start using HoughLines for lane detection --- CMakeLists.txt | 2 +- cfg/LineDetection.cfg | 6 + .../street_crossing.h | 75 +++++- launch/street_crossing.launch | 6 + src/street_crossing.cpp | 224 +++++++++++++++++- 5 files changed, 295 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 18d7f97..5785f2c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,7 @@ SET(CMAKE_BUILD_TYPE Debug) # enable to compile with OpenCV window displays add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images -#add_definitions(-DPUBLISH_DEBUG) +add_definitions(-DPUBLISH_DEBUG) # enable to use World2Cam homography #add_definitions(-DUSE_WORLD2CAM_HOMOGRAPHY) # enable publishing of detected points (for rviz display) diff --git a/cfg/LineDetection.cfg b/cfg/LineDetection.cfg index 9011b9c..f67b20b 100755 --- a/cfg/LineDetection.cfg +++ b/cfg/LineDetection.cfg @@ -19,5 +19,11 @@ gen.add("translateEnvironment", bool_t, 0, "Translate environment", False) gen.add("useWeights", bool_t, 0, "Use weights", False) gen.add("lineWidth", double_t, 0, "Line width (world coordinates)", 0.4, 0.0, 1.0) +gen.add("cannyThreshold", int_t, 0, "cannyThreshold", 30, 10, 500) +gen.add("houghThreshold", int_t, 0, "houghThreshold", 20, 5, 100) +gen.add("houghMinLineLen", int_t, 0, "houghMinLineLen", 20, 5, 100) +gen.add("houghMaxLineGap", int_t, 0, "houghMaxLineGap", 10, 0, 300) +gen.add("sameLineThreshold", int_t, 0, "sameLineThreshold", 15, 1, 50) + exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "LineDetection")) diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h index dd8a0e3..1f1bc30 100644 --- a/include/drive_ros_image_recognition/street_crossing.h +++ b/include/drive_ros_image_recognition/street_crossing.h @@ -15,6 +15,48 @@ namespace drive_ros_image_recognition { +// todo: move to own file + +class StreetLine { +public: + cv::Point p1_, p2_, mid_; + int vote; + + StreetLine(cv::Point p1, cv::Point p2) + : vote(0) + { + if(p1.y < p2.y) { + p1_ = p1; + p2_ = p2; + } else { + p1_ = p2; + p2_ = p1; + } + mid_ = (p1 + p2) * .5; + } + + StreetLine(cv::Vec4i c) + : vote(0) + { + cv::Point p1(c[0], c[1]); + cv::Point p2(c[2], c[3]); + if(p1.y < p2.y) { + p1_ = p1; + p2_ = p2; + } else { + p1_ = p2; + p2_ = p1; + } + mid_ = ((p1_ + p2_) * .5); + } + + // returns the angle of the lines between 0 and PI + inline double getAngle() { return abs(atan2(p1_.y - p2_.y, p1_.x - p2_.x)); } + + // used to sort lines ascending wrt y-coordinate + bool operator() (StreetLine sl1, StreetLine sl2) { return sl1.mid_.y < sl2.mid_.y; } +}; + class StreetCrossingDetection { private: @@ -24,31 +66,54 @@ class StreetCrossingDetection { float maxLineWidthMul_; float lineWidth_; + int cannyThreshold_; + int houghThresold_; + int houghMinLineLen_; + int houghMaxLineGap_; + int sameLineThreshold_; + // variables std::vector searchLines_; CvImagePtr currentImage_; +// cv::Mat warpedImg_; +// std::vector streetLines_; + int startLineCounter_; cv::Point lastStartLinePos_; // communication image_transport::ImageTransport imageTransport_; image_transport::Subscriber imageSubscriber_; + image_transport::Subscriber warpedImageSubscriber_; ros::Publisher line_output_pub_; +#ifdef PUBLISH_DEBUG + image_transport::Publisher debugImagePublisher_; +#endif + + // homography components + ros::Subscriber homography_params_sub_; + cv::Mat world2cam_; + cv::Mat cam2world_; + cv::Mat scaling_mat_; + cv::Mat scaling_mat_inv_; + cv::Size transformed_size_; + bool homo_received_; ros::NodeHandle nh_; ros::NodeHandle pnh_; void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); - - -#ifdef PUBLISH_DEBUG - image_transport::Publisher debugImagePublisher; -#endif + void warpedImageCallback(const sensor_msgs::ImageConstPtr& warpedImageIn); // methods bool findStartline(); + void findLane(); + + // this vector must always be orderer by HoughLine.mid_ (ascending) + std::vector currentLaneMid; + bool onRightLine; TransformHelper transform_helper_; ImageOperator image_operator_; diff --git a/launch/street_crossing.launch b/launch/street_crossing.launch index 58778b5..32e7d47 100644 --- a/launch/street_crossing.launch +++ b/launch/street_crossing.launch @@ -1,5 +1,7 @@ + + @@ -10,6 +12,10 @@ + + + + diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp index 45d6d77..20c22eb 100644 --- a/src/street_crossing.cpp +++ b/src/street_crossing.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include "drive_ros_image_recognition/street_crossing.h" #include @@ -19,7 +20,10 @@ StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh, const , image_operator_() , dsrv_server_() , dsrv_cb_(boost::bind(&StreetCrossingDetection::reconfigureCB, this, _1, _2)) - , startLineCounter_(0) + , onRightLine(true) +// #ifdef DRAW_DEBUG +// , debugImage(0, 0, CV_8UC1) +// #endif { } @@ -27,29 +31,219 @@ StreetCrossingDetection::~StreetCrossingDetection() { } bool StreetCrossingDetection::init() { + // dynamic reconfigure dsrv_server_.setCallback(dsrv_cb_); - transform_helper_.init(); - image_operator_ = ImageOperator(transform_helper_); + // image operations +// if(!transform_helper_.init()) +// ROS_INFO_STREAM("Init transform_helper failed"); +// image_operator_ = ImageOperator(transform_helper_); + //subscribe to camera image imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); - ROS_INFO_STREAM("Subscriber image transport with topic "<("line_out",10); + // subscribe to warped image +// warpedImageSubscriber_ = imageTransport_.subscribe("warped_in", 10, &StreetCrossingDetection::warpedImageCallback, this); +// ROS_INFO_STREAM("Subscribed image transport to topic " << warpedImageSubscriber_.getTopic()); + + // publish road lane +// line_output_pub_ = pnh_.advertise("line_out",10); #ifdef PUBLISH_DEBUG - debugImagePublisher = imageTransport_.advertise("/street_crossing/debug_image", 10); - ROS_INFO("Will publish debug image to /street_crossing/debug_image"); -#endif -#ifdef DRAW_DEBUG - ROS_INFO("Draw debug image"); + debugImagePublisher_ = imageTransport_.advertise("/street_crossing/debug_image", 10); + ROS_INFO_STREAM("Publishing debug image on " << debugImagePublisher_.getTopic()); #endif + + // common image operations + image_operator_.init(); + std::string world_frame("/rear_axis_middle_ground"); + if (!pnh_.getParam("world_frame", world_frame)) + ROS_WARN_STREAM("Parameter 'world_frame' not found, using default: "<(t_end-t_start).count()) << "ms"); +} + +void StreetCrossingDetection::findLane() { + cv::Mat debugImg, outputImg; + cv::cvtColor(*currentImage_, outputImg, CV_GRAY2RGB); + cv::Mat laneImg = cv::Mat::zeros(outputImg.rows, outputImg.cols, outputImg.type()); + + // Blur the debugImage and find edges with Canny + cv::GaussianBlur(*currentImage_, debugImg, cv::Size(15, 15), 0, 0); + cv::Canny(debugImg, debugImg, cannyThreshold_, cannyThreshold_ * 3, 3); + + // Zero out the vehicle + int w = debugImg.cols; + int h = debugImg.rows; + cv::Point vehiclePolygon[1][4]; + vehiclePolygon[0][0] = cv::Point(w * .35, h); + vehiclePolygon[0][1] = cv::Point(w * .65, h); + vehiclePolygon[0][2] = cv::Point(w * .65, h * .8); + vehiclePolygon[0][3] = cv::Point(w * .35, h * .8); + const cv::Point* ppt[1] = { vehiclePolygon[0] }; + int npt[] = { 4 }; + cv::fillPoly(debugImg, ppt, npt, 1, cv::Scalar(), cv::LINE_8); + + // If we dont have a lane from last step (or this is the first search), use straight line forward + if(currentLaneMid.empty()) { + currentLaneMid.push_back(StreetLine(cv::Point(w/2, h), cv::Point(w/2, 0))); + } + + // Display the polygon which will be zeroed out +// cv::line(outputImg, vehiclePolygon[0][0], vehiclePolygon[0][1], cv::Scalar(255), 2, cv::LINE_AA); +// cv::line(outputImg, vehiclePolygon[0][1], vehiclePolygon[0][2], cv::Scalar(255), 2, cv::LINE_AA); +// cv::line(outputImg, vehiclePolygon[0][2], vehiclePolygon[0][3], cv::Scalar(255), 2, cv::LINE_AA); +// cv::line(outputImg, vehiclePolygon[0][3], vehiclePolygon[0][0], cv::Scalar(255), 2, cv::LINE_AA); + + // Get houghlines and draw them on the output image + std::vector hLinePoints; + std::vector leftLines, midLines, rightLines; + cv::HoughLinesP(debugImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); + + // Assign the hough-lines returned by HoughLinesP to left, mid, or right line + for(size_t i = 0; i < hLinePoints.size(); i++) { + auto l = StreetLine(hLinePoints[i]); + + // todo: find current line segment + StreetLine currentSegment = currentLaneMid.at(0); + + if(currentSegment.mid_.x < l.mid_.x) { + rightLines.push_back(l); + } else if((currentSegment.mid_.x - (w * .15)) > l.mid_.x) { + leftLines.push_back(l); + } else { + midLines.push_back(l); + } + + + // (0,0) (1280,344) + // (-0.0506457,3.06453) (6.85171,-0.989774) + cv::Point2f wPt1, wPt2; +// image_operator_.imageToWorld(l.p1_, wPt1); +// image_operator_.imageToWorld(l.p2_, wPt2); + image_operator_.imageToWorld(cv::Point(0, 0), wPt1); + image_operator_.imageToWorld(cv::Point(w, h), wPt2); + cv::line(laneImg, wPt1, wPt2, cv::Scalar(255), 2, cv::LINE_AA); + ROS_INFO_STREAM("Transformed (0,0) to (" << wPt1.x << "," << wPt1.y <<")"); + ROS_INFO_STREAM("Transformed (" << w << "," << h << " to (" << wPt2.x << "," << wPt2.y << ")"); + } + + + cv::line(outputImg, cv::Point(w * .15, h), cv::Point(w * .45, 0), cv::Scalar(0, 255, 255), 2, cv::LINE_AA); + + for(auto l : leftLines) + cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(255), 2, cv::LINE_AA); + for(auto l : midLines) + cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 255), 2, cv::LINE_AA); + for(auto l : rightLines) + cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); + +// auto a = l.getAngle(); +// if((a > (CV_PI/6)) && (a < 2.6)) { +// // lane line +// for(size_t j = 0; j < laneLines.size(); j++) { +// auto d = abs(l.mid_.x - laneLines.at(j).mid_.x); +// if(d < sameLineThreshold_) { +// laneLines.at(j).vote++; +// l.vote++; +// } +// } +// laneLines.push_back(l); +// } else { +// // stop line +// for(size_t j = 0; j < stopLines.size(); j++) { +// auto d = abs(l.mid_.y - stopLines.at(j).mid_.y); +// if(d < sameLineThreshold_) { +// stopLines.at(j).vote++; +// l.vote++; +// } +// } +// stopLines.push_back(l); +// } + +// for(auto l : laneLines) { +// if(l.vote > 0) +// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0,255), 2, cv::LINE_AA); +// else +// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); +// } + +// for(auto l : stopLines) { +// if(l.vote > 0) +// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(255), 2, cv::LINE_AA); +// else +// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); +// } + +// bool added = false; +// for(size_t j = 0; j < myLines.size(); j++) { +// // todo: use world distance here with linewidth +// auto d = cv::norm(l.mid_ - myLines.at(j).mid_); +// if(d < sameLineThreshold_) { +// myLines.at(j).vote++; +// added = true; +// } +// } +// if(!added) { +// myLines.push_back(l); +// } +// } + +// int linesDrawn = 0; +// for(size_t i = 0; i < myLines.size(); i++) { +// auto l = myLines.at(i); +// auto a = l.getAngle(); +// if(l.vote > 0) { +// linesDrawn++; +// if((a > (CV_PI/6)) && (a < 2.6)) { +// // lane marking +// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0,255), 2, cv::LINE_AA); +// } else { +// // stop or start line +// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(255), 2, cv::LINE_AA); +// } +// } else { +// // discarded line due to votes +// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); +// } +// } + +// ROS_INFO_STREAM("out of " << hLinePoints.size() << " hough lines, " << linesDrawn << " survived"); + +#ifdef PUBLISH_DEBUG + debugImagePublisher_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, outputImg).toImageMsg()); +#endif + + cv::namedWindow("debug image", CV_WINDOW_NORMAL); + cv::imshow("debug image", outputImg); + cv::namedWindow("lane image", CV_WINDOW_NORMAL); + cv::imshow("lane image", laneImg); + cv::waitKey(1); } bool StreetCrossingDetection::findStartline() { @@ -100,7 +294,7 @@ bool StreetCrossingDetection::findStartline() { cv::waitKey(1); #endif #ifdef PUBLISH_DEBUG - debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debug_image).toImageMsg()); +// debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debugImage).toImageMsg()); #endif // test array of lines @@ -113,6 +307,12 @@ void StreetCrossingDetection::reconfigureCB(drive_ros_image_recognition::LineDet minLineWidthMul_ = config.minLineWidthMul; maxLineWidthMul_ = config.maxLineWidthMul; lineWidth_ = config.lineWidth; + + cannyThreshold_ = config.cannyThreshold; + houghThresold_ = config.houghThreshold; + houghMinLineLen_ = config.houghMinLineLen; + houghMaxLineGap_ = config.houghMaxLineGap; + sameLineThreshold_ = config.sameLineThreshold; } void StreetCrossingDetectionNodelet::onInit() { From ee118cdded53ea82cad820aab72ceb3f915db86d Mon Sep 17 00:00:00 2001 From: Mykyta Denysov Date: Fri, 3 Nov 2017 15:31:42 +0100 Subject: [PATCH 74/94] Added option to transform from unwarped image, move to PointStamped in RoadLane callback --- .../common_image_operations.h | 24 +++++++++---- .../road_detection.h | 14 ++++---- src/road_detection.cpp | 36 ++++++++++--------- 3 files changed, 45 insertions(+), 29 deletions(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 09e93d7..c281532 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -105,7 +105,7 @@ class TransformHelper { public: - TransformHelper(): + TransformHelper(bool use_homography = true): tf_listener_(), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), cam_model_(), @@ -119,7 +119,8 @@ class TransformHelper { camera_model_received_(false), // temporary solution until have embedded correct frame in camerainfo messages (including the bag files) camera_frame_(""), - world_frame_("") + world_frame_(""), + use_homography_(use_homography) { } @@ -182,7 +183,8 @@ class TransformHelper { camera_model_received_(false), // temporary solution until have embedded correct frame in camerainfo messages (including the bag files) camera_frame_(""), - world_frame_("") + world_frame_(""), + use_homography_(true) { } @@ -275,8 +277,14 @@ class TransformHelper { mat_pixel_coords.at(0,0) = pixel_coords.x; mat_pixel_coords.at(0,1) = pixel_coords.y; cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); - cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_inv_); - image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); + if (use_homography_) { + cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_inv_); + image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); + } + else { + // nothing to transform if homography has not been applied + image_point = cv::Point(pixel_coords.x, pixel_coords.y); + } return true; } @@ -297,7 +305,10 @@ class TransformHelper { std::vector obj_corners(1); obj_corners[0] = cv::Point2f(image_point.x, image_point.y); std::vector scene_corners(1); - cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_); + if (homography_received_) + cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_); + else + scene_corners[0] = obj_corners[0]; // no homography transforms if we use camera image directly // this returns a point with z=1, the pixel is on a ray from origin to this point cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(scene_corners[0]); @@ -351,6 +362,7 @@ class TransformHelper { private: image_geometry::PinholeCameraModel cam_model_; + bool use_homography_; bool homography_received_; bool camera_model_received_; std::string world_frame_; diff --git a/include/drive_ros_image_recognition/road_detection.h b/include/drive_ros_image_recognition/road_detection.h index f6124bc..f6f2a7d 100644 --- a/include/drive_ros_image_recognition/road_detection.h +++ b/include/drive_ros_image_recognition/road_detection.h @@ -16,6 +16,8 @@ #include #include #include +#include +#include // for multithreading #include @@ -60,10 +62,10 @@ class RoadDetection { image_transport::ImageTransport it_; // road inputs and outputs image_transport::Subscriber img_sub_debug_; -// std::unique_ptr img_sub_; -// std::unique_ptr > road_sub_; -// typedef message_filters::sync_policies::ApproximateTime SyncImageToRoad; -// std::unique_ptr > sync_; + std::unique_ptr img_sub_; + std::unique_ptr > road_sub_; + typedef message_filters::sync_policies::ApproximateTime SyncImageToRoad; + std::unique_ptr > sync_; ros::Publisher line_output_pub_; #ifdef PUBLISH_DEBUG image_transport::Publisher debug_img_pub_; @@ -72,7 +74,7 @@ class RoadDetection { #endif ros::NodeHandle nh_; ros::NodeHandle pnh_; - std::vector road_points_buffer_; + std::vector road_points_buffer_; dynamic_reconfigure::Server dsrv_server_; dynamic_reconfigure::Server::CallbackType dsrv_cb_; @@ -85,7 +87,7 @@ class RoadDetection { bool find(); void processSearchLine(const SearchLine &line); #ifdef PUBLISH_WORLD_POINTS - ros::Publisher world_point_pub; + ros::Publisher world_point_pub_; #endif ImageOperator image_operator_; diff --git a/src/road_detection.cpp b/src/road_detection.cpp index bae5921..848503e 100644 --- a/src/road_detection.cpp +++ b/src/road_detection.cpp @@ -32,10 +32,10 @@ RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh linesToProcess_(0), current_image_(), road_points_buffer_(), - image_operator_(), - img_sub_debug_() + image_operator_() +, img_sub_debug_() #ifdef PUBLISH_WORLD_POINTS - ,world_point_pub() + ,world_point_pub_() #endif { } @@ -101,19 +101,20 @@ bool RoadDetection::init() { dsrv_server_.setCallback(dsrv_cb_); #ifdef PUBLISH_WORLD_POINTS - world_point_pub = pnh_.advertise("worldPoints", 1000); + world_point_pub_ = pnh_.advertise("worldPoints", 1000); #endif // to synchronize incoming images and the road, we use message filters -// img_sub_.reset(new image_transport::SubscriberFilter(it_,"img_in", 1000)); -// road_sub_.reset(new message_filters::Subscriber(pnh_,"road_in", 1000)); -// sync_.reset(new message_filters::Synchronizer(SyncImageToRoad(100), *img_sub_, *road_sub_)); -// sync_->setAgePenalty(1.0); + img_sub_.reset(new image_transport::SubscriberFilter(it_,"img_in", 1000)); + road_sub_.reset(new message_filters::Subscriber(pnh_,"road_in", 1000)); + sync_.reset(new message_filters::Synchronizer(SyncImageToRoad(100), *img_sub_, *road_sub_)); + sync_->setAgePenalty(1.0); // sync_->registerCallback(boost::bind(&RoadDetection::syncCallback, this, _1, _2)); - img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); -// img_sub_debug_ = it_.subscribe("img_in", 1000, boost::bind(&RoadDetection::debugDrawFrameCallback, this, -// _1, std::string("/camera_optical"), std::string("/front_axis_middle"))); + // Debug callbacks for testing +// img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); + img_sub_debug_ = it_.subscribe("img_in", 1000, boost::bind(&RoadDetection::debugDrawFrameCallback, this, + _1, std::string("/camera_optical"), std::string("/front_axis_middle"))); line_output_pub_ = nh_.advertise("line_out",10); @@ -230,8 +231,8 @@ bool RoadDetection::find(){ //create left/mid/right lane linestring mid, left, right; - for (auto point : road_points_buffer_) { - boost::geometry::append(mid,point_xy(point.x, point.y)); + for (auto point_stamped : road_points_buffer_) { + boost::geometry::append(mid,point_xy(point_stamped.point.x, point_stamped.point.y)); } // simplify mid (previously done with distance-based algorithm // skip this for now, can completely clear the line @@ -420,11 +421,12 @@ void RoadDetection::processSearchLine(const SearchLine &l) { // todo: check how this gets handled and adjust the interface drive_ros_msgs::RoadLane lane_out; lane_out.header.stamp = ros::Time::now(); - geometry_msgs::Point32 point_temp; - point_temp.z = 0.f; + geometry_msgs::PointStamped point_temp; + point_temp.header.frame_id = std::string("/rear_axis_middle"); + point_temp.point.z = 0.f; for (auto point: validPoints) { - point_temp.x = point.x; - point_temp.y = point.y; + point_temp.point.x = point.x; + point_temp.point.y = point.y; lane_out.points.push_back(point_temp); } lane_out.roadStateType = drive_ros_msgs::RoadLane::UNKNOWN; From e038daa599f0665f5a674ef70a47115a43be36f4 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Wed, 8 Nov 2017 20:26:53 +0100 Subject: [PATCH 75/94] general line detection --- CMakeLists.txt | 93 +--- README.md | 14 + cfg/LineDetection.cfg | 2 +- .../common_image_operations.h | 155 ++++-- .../line_detection.h | 68 +++ .../road_detection.h | 108 ---- .../street_crossing.h | 141 ------ .../drive_ros_image_recognition/street_line.h | 30 ++ .../drive_ros_image_recognition/warp_image.h | 2 + ..._crossing.launch => line_detection.launch} | 4 +- src/line_detection.cpp | 253 ++++++++++ src/line_detection_node.cpp | 26 + src/road_detection.cpp | 465 ------------------ src/road_detection_node.cpp | 26 - src/street_crossing.cpp | 332 ------------- src/street_crossing_node.cpp | 26 - src/warp_image.cpp | 1 + 17 files changed, 529 insertions(+), 1217 deletions(-) create mode 100644 include/drive_ros_image_recognition/line_detection.h delete mode 100644 include/drive_ros_image_recognition/road_detection.h delete mode 100644 include/drive_ros_image_recognition/street_crossing.h create mode 100644 include/drive_ros_image_recognition/street_line.h rename launch/{street_crossing.launch => line_detection.launch} (92%) create mode 100644 src/line_detection.cpp create mode 100644 src/line_detection_node.cpp delete mode 100644 src/road_detection.cpp delete mode 100644 src/road_detection_node.cpp delete mode 100644 src/street_crossing.cpp delete mode 100644 src/street_crossing_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5785f2c..9d4806d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,63 +50,6 @@ include_directories( ${Boost_INCLUDE_DIRS} ) - -################################################################################ -# road detection node (LMS port) -################################################################################ - -add_executable(road_detection_node - src/road_detection.cpp - src/road_detection_node.cpp - ) - -add_dependencies(road_detection_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(road_detection_node - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ${Boost_LIBRARIES} - ) - -install(TARGETS road_detection_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(road_detection_node_test launch/road_detection.test - test/common_image_operations_test.cpp - test/road_detection_test.cpp - test/geometry_common_test.cpp - ) - target_link_libraries(road_detection_node_test ${catkin_LIBRARIES}) -endif() - - -################################################################################ -# road detection nodelet (LMS port) -################################################################################ - -add_library(road_detection_nodelet - src/road_detection.cpp - ) - -add_dependencies(road_detection_nodelet ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(road_detection_nodelet - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ${Boost_LIBRARIES} - ) - -install(TARGETS road_detection_nodelet - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - ################################################################################ # Warp image node ################################################################################ @@ -149,45 +92,23 @@ install(TARGETS warp_image_nodelet ) ################################################################################ -# Street crossing detection nodelet (LMS port) -################################################################################ - -add_library(street_crossing_nodelet - src/street_crossing.cpp - ) - -add_dependencies(street_crossing_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(street_crossing_nodelet - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ${Boost_LIBRARIES} - ) - -install(TARGETS street_crossing_nodelet - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - -################################################################################ -# Street crossing detection node (LMS port) +# Line detection node ################################################################################ -add_executable(street_crossing_node - src/street_crossing.cpp - src/street_crossing_node.cpp +add_executable(line_detection_node + src/line_detection.cpp + src/line_detection_node.cpp ) -add_dependencies(street_crossing_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(line_detection_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(street_crossing_node +target_link_libraries(line_detection_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} ) -install(TARGETS street_crossing_node +install(TARGETS line_detection_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/README.md b/README.md index c20d287..cdfdc7b 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,16 @@ # drive_ros_image_recognition image recognition (lanes, crossings, start lines, ....) + +For using the line detection, we need +* a camera image (from ximea camera or a rosbag) +* the homography. Therefore you can use `drive_ros_ximea_importer/launch/homography_publischer_only.launch`. + +Now start `drive_ros_image_recognition/launch/line_recognition.launch`. + +The currently used files are: +* /include/drive_ros_image_recognition/common_image_operations.h +* /include/drive_ros_image_recognition/line_detection.h +* /include/drive_ros_image_recognition/street_line.h +* /launch/line_detection.h +* /src/line_detection.cpp +* /src/line_detection_node.cpp diff --git a/cfg/LineDetection.cfg b/cfg/LineDetection.cfg index f67b20b..131f8e5 100755 --- a/cfg/LineDetection.cfg +++ b/cfg/LineDetection.cfg @@ -23,7 +23,7 @@ gen.add("cannyThreshold", int_t, 0, "cannyThreshold", 30, 10, 500) gen.add("houghThreshold", int_t, 0, "houghThreshold", 20, 5, 100) gen.add("houghMinLineLen", int_t, 0, "houghMinLineLen", 20, 5, 100) gen.add("houghMaxLineGap", int_t, 0, "houghMaxLineGap", 10, 0, 300) -gen.add("sameLineThreshold", int_t, 0, "sameLineThreshold", 15, 1, 50) +gen.add("sameLineThreshold", int_t, 0, "sameLineThreshold", 40, 1, 100) exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "LineDetection")) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index c281532..7966936 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -39,10 +39,16 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) namespace drive_ros_image_recognition { inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in, cv::Mat& cam2world, cv::Mat& world2cam, - cv::Mat& scaling_mat, cv::Mat& scaling_mat_inv, bool& homography_received) { - if (!homography_received) - homography_received = true; + cv::Mat& scaling_mat, cv::Mat& scaling_mat_inv, cv::Mat& scaledCam2World, cv::Mat& scaledWorld2Cam, + bool& homography_received) { + if (homography_received) { + // the homography will not change during runtime + return; + } + homography_received = true; + + // extract cam2world matrix from the homography message ROS_ASSERT(homo_in->cam2world.layout.dim[0].size == 3 && homo_in->cam2world.layout.dim[1].size == 3 ); cam2world = cv::Mat::zeros(3,3,CV_64FC1); int k=0; @@ -52,6 +58,7 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i } } + // extract world2cam matrix from the homography message ROS_ASSERT(homo_in->world2cam.layout.dim[0].size == 3 && homo_in->world2cam.layout.dim[1].size == 3 ); world2cam = cv::Mat::zeros(3,3,CV_64FC1); k=0; @@ -61,9 +68,14 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i } } - ROS_INFO_STREAM("Scaling mat: "< &imagePoints, std::vector &worldPoints) { + if(!homography_received_) { + ROS_WARN_STREAM("[imagetoWorld] Homography not received yet"); return false; } + cv::perspectiveTransform(imagePoints, worldPoints, cam2world_); + return true; + } - // transform to image point and then repeat homography transform - cv::Point2d pixel_coords = cam_model_.project3dToPixel(world_point); - cv::Mat mat_pixel_coords(1,1,CV_64FC2,cv::Scalar(0.0)); - mat_pixel_coords.at(0,0) = pixel_coords.x; - mat_pixel_coords.at(0,1) = pixel_coords.y; - cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); - if (use_homography_) { - cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_inv_); - image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); + /// + /// \brief worldToImage + /// Converts all worldPoints to the corresponding imagePoints using the world2cam matrix. + /// \param worldPoints + /// \param imagePoints + /// \return true on success, false on failure. + /// + bool worldToImage(std::vector &worldPoints, std::vector &imagePoints) { + if(!homography_received_) { + ROS_WARN_STREAM("[worldToImage] Homography not received yet"); + return false; } - else { - // nothing to transform if homography has not been applied - image_point = cv::Point(pixel_coords.x, pixel_coords.y); + cv::perspectiveTransform(worldPoints, imagePoints, world2cam_); + return true; + } + + /// + /// \brief homographImage + /// Warps the srcImg to get the "bird eye view" image. + /// \param srcImg + /// \param dstImg + /// \return true on success, false on failure. + /// + bool homographImage(cv::Mat &srcImg, cv::Mat &dstImg) { + if(!homography_received_) { + ROS_WARN_STREAM("[homographImage] Homography not received yet"); + return false; } + dstImg = cv::Mat::zeros( srcImg.rows, srcImg.cols, srcImg.type() ); + cv::warpPerspective(srcImg, dstImg, scaledCam2world_, transformed_size_); return true; } - bool worldToImage(const cv::Point2f &world_point, cv::Point &image_point) { - return worldToImage(cv::Point3f(world_point.x, world_point.y, 0.0f), image_point); + /// + /// \brief cameraToWarpedImg + /// Converts a camera points to the corresponding points in the warped image. + /// \param cameraPoints + /// \param worldPoint + /// \return true on success, false on failure. + /// + bool cameraToWarpedImg(std::vector &cameraPoints, std::vector &warpedImgPoints) { + if(!homography_received_) { + ROS_WARN_STREAM("[cameraToWarpedImg] Homography not received yet"); + return false; + } + cv::perspectiveTransform(cameraPoints, warpedImgPoints, scaledCam2world_); + return true; } + /* + * Simon says: these two methods are not working properly, since we are doing the homography twice (first use the homog matrix + * and then the TF conversion. I will leave this in, in case we need the tf version later. + */ bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point, std::string target_frame = std::string("/rear_axis_middle_ground"), cv::Mat test_image = cv::Mat()) { if(!homography_received_ || !camera_model_received_) { if (!homography_received_) @@ -306,9 +357,14 @@ class TransformHelper { obj_corners[0] = cv::Point2f(image_point.x, image_point.y); std::vector scene_corners(1); if (homography_received_) - cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_); +// cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_inv_); + cv::perspectiveTransform(obj_corners, scene_corners, cam2world_); else - scene_corners[0] = obj_corners[0]; // no homography transforms if we use camera image directly + scene_corners[0] = obj_corners[0]; // no homography transform if we use camera image directly + + world_point = scene_corners.at(0); + return true; + // this returns a point with z=1, the pixel is on a ray from origin to this point cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(scene_corners[0]); @@ -352,6 +408,43 @@ class TransformHelper { } } + bool worldToImage(const cv::Point2f& world_point, cv::Point2f& image_point) { + if (!homography_received_) { + ROS_WARN_STREAM("[WorldToImage] Homography not received yet, skipping world to image transformation requested for world point "< worldPoints(1); + std::vector imagePoints(1); + worldPoints[0] = cv::Point2f(image_point.x, image_point.y); + cv::perspectiveTransform(worldPoints, imagePoints, world2cam_); + image_point = imagePoints.at(0); + + // transform to image point and then repeat homography transform + cv::Point2d pixel_coords = cam_model_.project3dToPixel(cv::Point3f(world_point.x, world_point.y, 0.0)); + cv::Mat mat_pixel_coords(1,1,CV_64FC2,cv::Scalar(0.0)); + mat_pixel_coords.at(0,0) = pixel_coords.x; + mat_pixel_coords.at(0,1) = pixel_coords.y; + cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); + if (use_homography_) { + cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_); + image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); + } + else { + // nothing to transform if homography has not been applied + image_point = cv::Point(pixel_coords.x, pixel_coords.y); + } + + return true; + } + void setcam2worldMat(const cv::Mat& cam2world) { cam2world_ = cam2world; } @@ -372,6 +465,8 @@ class TransformHelper { cv::Mat world2cam_; cv::Mat scaling_mat_; cv::Mat scaling_mat_inv_; + cv::Mat scaledWorld2cam_; + cv::Mat scaledCam2world_; cv::Size transformed_size_; tf::TransformListener tf_listener_; ros::Subscriber cam_info_sub_; diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h new file mode 100644 index 0000000..60c5cbd --- /dev/null +++ b/include/drive_ros_image_recognition/line_detection.h @@ -0,0 +1,68 @@ +#ifndef LINE_DETECTION_H +#define LINE_DETECTION_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "drive_ros_msgs/RoadLane.h" +#include "street_line.h" + +namespace drive_ros_image_recognition { + +class LineDetection { +private: + // our node handles + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // configs + float lineWidth_; + int cannyThreshold_; + int houghThresold_; + int houghMinLineLen_; + int houghMaxLineGap_; + + // variables + CvImagePtr currentImage_; + std::vector currentLane_; + ImageOperator image_operator_; + + // communication + image_transport::ImageTransport imageTransport_; + image_transport::Subscriber imageSubscriber_; + ros::Publisher line_output_pub_; // note: not used yet + ros::Subscriber homography_params_sub_; + + // homography components + cv::Mat world2cam_; + cv::Mat cam2world_; + cv::Mat scaling_mat_; + cv::Mat scaling_mat_inv_; + cv::Size transformed_size_; + bool homog_received_; + + // callbacks + void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); + void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); + dynamic_reconfigure::Server dsrv_server_; + dynamic_reconfigure::Server::CallbackType dsrv_cb_; + + // methods + void findLane(); + +public: + LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); + ~LineDetection(); + bool init(); +}; + +} // namepsace drive_ros_image_recognition + +#endif // LINE_DETECTION_H diff --git a/include/drive_ros_image_recognition/road_detection.h b/include/drive_ros_image_recognition/road_detection.h deleted file mode 100644 index f6f2a7d..0000000 --- a/include/drive_ros_image_recognition/road_detection.h +++ /dev/null @@ -1,108 +0,0 @@ -#ifndef ROAD_DETECTION_H -#define ROAD_DETECTION_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// for multithreading -#include -#include -#include - -namespace drive_ros_image_recognition { - -/** - * @brief Port of LMS module road_detection to ROS - **/ -class RoadDetection { - // configs - float searchOffset_; - float distanceBetweenSearchlines_; - bool findPointsBySobel_; - float minLineWidthMul_; - float maxLineWidthMul_; - int brightness_threshold_; - float laneWidthOffsetInMeter_; - bool translateEnvironment_; - bool useWeights_; - int sobelThreshold_; - - // ported in the WarpImage class, ports the entire image already -// lms::imaging::Homography homo; - //Datachannels - // todo: we have not determined all interfaces yet, so will leave this in for now until we have figured it out -// lms::ReadDataChannel image; -// lms::WriteDataChannel road; -// lms::WriteDataChannel output; -// lms::WriteDataChannel debugImage; -// lms::WriteDataChannel debugAllPoints; -// lms::WriteDataChannel debugValidPoints; -// lms::WriteDataChannel debugTranslatedPoints; -// lms::ReadDataChannel car; - - std::list lines_; - - int linesToProcess_; - CvImagePtr current_image_; - image_transport::ImageTransport it_; - // road inputs and outputs - image_transport::Subscriber img_sub_debug_; - std::unique_ptr img_sub_; - std::unique_ptr > road_sub_; - typedef message_filters::sync_policies::ApproximateTime SyncImageToRoad; - std::unique_ptr > sync_; - ros::Publisher line_output_pub_; -#ifdef PUBLISH_DEBUG - image_transport::Publisher debug_img_pub_; - image_transport::Publisher detected_points_pub_; - image_transport::Publisher filtered_points_pub_; -#endif - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - std::vector road_points_buffer_; - - dynamic_reconfigure::Server dsrv_server_; - dynamic_reconfigure::Server::CallbackType dsrv_cb_; - void debugImageCallback(const sensor_msgs::ImageConstPtr& img_in); - void debugDrawFrameCallback(const sensor_msgs::ImageConstPtr& img_in, - const std::string camera_frame, - const std::string draw_frame); - void syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in); - void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); - bool find(); - void processSearchLine(const SearchLine &line); -#ifdef PUBLISH_WORLD_POINTS - ros::Publisher world_point_pub_; -#endif - ImageOperator image_operator_; - -public: - RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); - ~RoadDetection(); - bool init(); -}; - -class RoadDetectionNodelet: public nodelet::Nodelet { -public: - virtual void onInit(); -private: - std::unique_ptr road_detection_; -}; - -} -#endif // ROAD_DETECTION_H diff --git a/include/drive_ros_image_recognition/street_crossing.h b/include/drive_ros_image_recognition/street_crossing.h deleted file mode 100644 index 1f1bc30..0000000 --- a/include/drive_ros_image_recognition/street_crossing.h +++ /dev/null @@ -1,141 +0,0 @@ -#ifndef STREET_CROSSING_H -#define STREET_CROSSING_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "drive_ros_msgs/RoadLane.h" - -namespace drive_ros_image_recognition { - -// todo: move to own file - -class StreetLine { -public: - cv::Point p1_, p2_, mid_; - int vote; - - StreetLine(cv::Point p1, cv::Point p2) - : vote(0) - { - if(p1.y < p2.y) { - p1_ = p1; - p2_ = p2; - } else { - p1_ = p2; - p2_ = p1; - } - mid_ = (p1 + p2) * .5; - } - - StreetLine(cv::Vec4i c) - : vote(0) - { - cv::Point p1(c[0], c[1]); - cv::Point p2(c[2], c[3]); - if(p1.y < p2.y) { - p1_ = p1; - p2_ = p2; - } else { - p1_ = p2; - p2_ = p1; - } - mid_ = ((p1_ + p2_) * .5); - } - - // returns the angle of the lines between 0 and PI - inline double getAngle() { return abs(atan2(p1_.y - p2_.y, p1_.x - p2_.x)); } - - // used to sort lines ascending wrt y-coordinate - bool operator() (StreetLine sl1, StreetLine sl2) { return sl1.mid_.y < sl2.mid_.y; } -}; - -class StreetCrossingDetection { - -private: - // configs - int sobelThreshold_; - float minLineWidthMul_; - float maxLineWidthMul_; - float lineWidth_; - - int cannyThreshold_; - int houghThresold_; - int houghMinLineLen_; - int houghMaxLineGap_; - int sameLineThreshold_; - - // variables - std::vector searchLines_; - CvImagePtr currentImage_; - -// cv::Mat warpedImg_; -// std::vector streetLines_; - - int startLineCounter_; - cv::Point lastStartLinePos_; - - // communication - image_transport::ImageTransport imageTransport_; - image_transport::Subscriber imageSubscriber_; - image_transport::Subscriber warpedImageSubscriber_; - - ros::Publisher line_output_pub_; -#ifdef PUBLISH_DEBUG - image_transport::Publisher debugImagePublisher_; -#endif - - // homography components - ros::Subscriber homography_params_sub_; - cv::Mat world2cam_; - cv::Mat cam2world_; - cv::Mat scaling_mat_; - cv::Mat scaling_mat_inv_; - cv::Size transformed_size_; - bool homo_received_; - - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); - void warpedImageCallback(const sensor_msgs::ImageConstPtr& warpedImageIn); - - // methods - bool findStartline(); - void findLane(); - - // this vector must always be orderer by HoughLine.mid_ (ascending) - std::vector currentLaneMid; - bool onRightLine; - - TransformHelper transform_helper_; - ImageOperator image_operator_; - - void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); - dynamic_reconfigure::Server dsrv_server_; - dynamic_reconfigure::Server::CallbackType dsrv_cb_; - -public: - StreetCrossingDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); - ~StreetCrossingDetection(); - bool init(); -}; - -class StreetCrossingDetectionNodelet: public nodelet::Nodelet { -public: - virtual void onInit(); -private: - std::unique_ptr street_crossing_detection_; -}; - - -} // namepsace drive_ros_image_recognition - -#endif // STREET_CROSSING_H diff --git a/include/drive_ros_image_recognition/street_line.h b/include/drive_ros_image_recognition/street_line.h new file mode 100644 index 0000000..f131f4f --- /dev/null +++ b/include/drive_ros_image_recognition/street_line.h @@ -0,0 +1,30 @@ +#ifndef STREET_LINE_H +#define STREET_LINE_H + +/// +/// \brief The StreetLine class +/// Represents a line, defined by two points. +/// The class saves the two points in image coordinates and world coordinates. +/// The world coordinates are in meters, e.g. 0.5 is 50 cm. +/// You are responsible that wP1_ is the correct world points for iP1_, same for wP2_ and iP2_. +/// +class StreetLine { +public: + cv::Point iP1_, iP2_, iMid_; + cv::Point2f wP1_, wP2_, wMid_; + + StreetLine(cv::Point iP1, cv::Point iP2, cv::Point2f wP1, cv::Point2f wP2) + : iP1_(iP1) + , iP2_(iP2) + , wP1_(wP1) + , wP2_(wP2) + { + iMid_ = (iP1_ + iP2_) * .5; + wMid_ = (wP1_ + wP2_) * .5; + } + + // returns the angle of the lines between 0 and PI + inline double getAngle() { return abs(atan2(iP1_.y - iP2_.y, iP1_.x - iP2_.x)); } +}; + +#endif // STREET_LINE_H diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index b15520f..09375bf 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -43,6 +43,8 @@ class WarpContent { cv::Mat cam2world_; cv::Mat scaling_mat_; cv::Mat scaling_mat_inv_; + cv::Mat scaledCam2world_; + cv::Mat scaledWorld2cam_; cv::Size transformed_size_; bool homo_received_; }; diff --git a/launch/street_crossing.launch b/launch/line_detection.launch similarity index 92% rename from launch/street_crossing.launch rename to launch/line_detection.launch index 32e7d47..c6174f8 100644 --- a/launch/street_crossing.launch +++ b/launch/line_detection.launch @@ -1,7 +1,7 @@ - + @@ -13,7 +13,7 @@ - + diff --git a/src/line_detection.cpp b/src/line_detection.cpp new file mode 100644 index 0000000..e40681b --- /dev/null +++ b/src/line_detection.cpp @@ -0,0 +1,253 @@ +#include +#include +#include +#include +#include "drive_ros_image_recognition/line_detection.h" +#include + +namespace drive_ros_image_recognition { + +LineDetection::LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh) + : nh_(nh) + , pnh_(pnh) + , imageTransport_(pnh) + , lineWidth_(0.4) + , line_output_pub_() + , image_operator_() + , dsrv_server_() + , dsrv_cb_(boost::bind(&LineDetection::reconfigureCB, this, _1, _2)) +{ +} + +LineDetection::~LineDetection() { +} + +/// +/// \brief LineDetection::init +/// Sets dynamic reconfigure server, subscribes to camera image, inits image_operator. +/// \return true for success, false for failure. +/// +bool LineDetection::init() { + // dynamic reconfigure + dsrv_server_.setCallback(dsrv_cb_); + + //subscribe to camera image + imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &LineDetection::imageCallback, this); + ROS_INFO_STREAM("Subscribed image transport to topic " << imageSubscriber_.getTopic()); + + // common image operations + if(!image_operator_.init()) { + ROS_WARN_STREAM("Failed to init image_operator"); + return false; + } + + return true; +} + +/// +/// \brief LineDetection::imageCallback +/// Called for incoming camera image. Extracts the image from the incoming message. +/// \param imgIn the incoming camera image message. +/// +void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { + currentImage_ = convertImageMessage(imgIn); + +// auto t_start = std::chrono::high_resolution_clock::now(); + findLane(); +// auto t_end = std::chrono::high_resolution_clock::now(); +// ROS_INFO_STREAM("Cycle time: " << (std::chrono::duration(t_end-t_start).count()) << "ms"); +} + +/// +/// \brief LineDetection::findLane +/// Uses the current image to find the street marking lanes. +/// +void LineDetection::findLane() { + cv::Mat processingImg; +#ifdef DRAW_DEBUG + cv::Mat outputImg; + cv::cvtColor(*currentImage_, outputImg, CV_GRAY2RGB); + cv::Mat allLinesImg = outputImg.clone(); +#endif + + // Blur the image and find edges with Canny + cv::GaussianBlur(*currentImage_, processingImg, cv::Size(15, 15), 0, 0); + cv::Canny(processingImg, processingImg, cannyThreshold_, cannyThreshold_ * 3, 3); + + // Zero out the vehicle + int imgWidth = processingImg.cols; + int imgHeight = processingImg.rows; + cv::Point vehiclePolygon[1][4]; + vehiclePolygon[0][0] = cv::Point(imgWidth * .35, imgHeight); + vehiclePolygon[0][1] = cv::Point(imgWidth * .65, imgHeight); + vehiclePolygon[0][2] = cv::Point(imgWidth * .65, imgHeight * .8); + vehiclePolygon[0][3] = cv::Point(imgWidth * .35, imgHeight * .8); + const cv::Point* polygonStarts[1] = { vehiclePolygon[0] }; + int polygonLengths[] = { 4 }; + cv::fillPoly(processingImg, polygonStarts, polygonLengths, 1, cv::Scalar(), cv::LINE_8); + +#ifdef DRAW_DEBUG + // Display the polygon which will be zeroed out +// cv::line(outputImg, vehiclePolygon[0][0], vehiclePolygon[0][1], cv::Scalar(255), 2, cv::LINE_AA); +// cv::line(outputImg, vehiclePolygon[0][1], vehiclePolygon[0][2], cv::Scalar(255), 2, cv::LINE_AA); +// cv::line(outputImg, vehiclePolygon[0][2], vehiclePolygon[0][3], cv::Scalar(255), 2, cv::LINE_AA); +// cv::line(outputImg, vehiclePolygon[0][3], vehiclePolygon[0][0], cv::Scalar(255), 2, cv::LINE_AA); +#endif + + // Get houghlines + std::vector hLinePoints; + std::vector filteredLines; + + cv::HoughLinesP(processingImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); + + // Extract points for houghLines and convert to worldCoordinates + std::vector imagePoints, worldPoints; + for(size_t i = 0; i < hLinePoints.size(); i++) { + cv::Vec4i currentPoints = hLinePoints.at(i); + imagePoints.push_back(cv::Point(currentPoints[0], currentPoints[1])); + imagePoints.push_back(cv::Point(currentPoints[2], currentPoints[3])); + } + + if(!image_operator_.imageToWorld(imagePoints, worldPoints)) { + // failed converting points + return; + } + + // Filter the lines + for(size_t i = 0; i < worldPoints.size(); i += 2) { + StreetLine sl(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1)); + filteredLines.push_back(sl); +#ifdef DRAW_DEBUG + cv::line(allLinesImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); +#endif + } + + // Create a dummy middle line, in case we do not have on from the previous frame + worldPoints.clear(); + imagePoints.clear(); + if(currentLane_.empty()) { + worldPoints.push_back(cv::Point2f(0.24, 0.5)); + worldPoints.push_back(cv::Point2f(0.4, 0.5)); + worldPoints.push_back(cv::Point2f(0.55, 0.5)); + worldPoints.push_back(cv::Point2f(0.70, 0.5)); + if(image_operator_.worldToImage(worldPoints, imagePoints)) { + for(size_t i = 0; i < imagePoints.size() - 1; i++) + currentLane_.push_back(StreetLine(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); + } else { + // no guess, means no detection + return; + } + } + +#ifdef DRAW_DEBUG + // Draw our "guess" (previous middle line) + for(size_t i = 0; i < currentLane_.size(); i++) { + cv::line(outputImg, currentLane_.at(i).iP1_, currentLane_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); + } +#endif + + // Use the guess to classify the lines + std::vector leftLine, middleLine, rightLine; + for(StreetLine sl : filteredLines) { + StreetLine segment = currentLane_.at(0); + bool isInSegment = false; + // find the coresponding segment + // todo: this only uses the x-coordinate right now, improve this + for(size_t i = 0; (i < currentLane_.size()) && !isInSegment; i++) { + segment = currentLane_.at(i); + if(sl.wMid_.x > segment.wP1_.x) { + if(sl.wMid_.x < segment.wP2_.x) { + isInSegment = true; + } + } else if(sl.wMid_.x > segment.wP2_.x) { + isInSegment = true; + } + } + + if(isInSegment) { + // classify the line + // lane width is 0.35 to 0.45 [m] + auto distanceToMidLine = sl.wMid_.y - segment.wMid_.y; + if(std::abs(distanceToMidLine) < 0.175) { + middleLine.push_back(sl); +#ifdef DRAW_DEBUG + cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); +#endif + } else if((sl.wMid_.y < segment.wMid_.y) && (std::abs(distanceToMidLine) < .6)) { + rightLine.push_back(sl); +#ifdef DRAW_DEBUG + cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); +#endif + } else if(distanceToMidLine < .6) { + leftLine.push_back(sl); +#ifdef DRAW_DEBUG + cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); +#endif + } else { +#ifdef DRAW_DEBUG + cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255, 255), 2, cv::LINE_AA); +#endif + } + } + } + + // Use the new middle line as our new guess +// currentLane_.clear(); +//// for(auto l : middleLine) +//// currentLane_.push_back(l); + +// // sort the middle lines based on the front distance of the vehicle +//// std::sort(middleLine.begin(), middleLine.end(), [](const StreetLine& a, const StreetLine& b){ return a.wMid_.x < b.wMid_.x; }); +// std::vector guessImgPoints, guessWorldPoints; +// for(auto lineIt = middleLine.begin(); lineIt != middleLine.end(); ++lineIt) { +// guessWorldPoints.push_back(lineIt->wP1_); +// guessWorldPoints.push_back(lineIt->wP2_); +// } +// std::sort(guessWorldPoints.begin(), guessWorldPoints.end(), [](const cv::Point2f& a, const cv::Point2f& b){ return a.x < b.x; }); +// // now add another lane as the last one to extend our current lane +// cv::Point lastLineDirection = guessWorldPoints.at(guessWorldPoints.size() - 1) - guessWorldPoints.at(guessWorldPoints.size() - 2); +// auto normFactor = 1 / cv::norm(lastLineDirection); +// ROS_INFO_STREAM("normFactor=" << normFactor); + +// if((guessWorldPoints.size() > 0) && (image_operator_.worldToImage(guessWorldPoints, guessImgPoints))) { +// for(size_t i = 1; i < guessImgPoints.size(); i++) { +// currentLane_.push_back(StreetLine(guessImgPoints.at(i - 1), guessImgPoints.at(i), guessWorldPoints.at(i - 1), guessWorldPoints.at(i))); +// } + +// } + + + // angle classification +// if((a > (CV_PI/6)) && (a < 2.6)) { +// // lane marking +// cv::line(outputImg, l.iP1_, l.iP2_, cv::Scalar(0,255), 2, cv::LINE_AA); +// } else { +// // stop or start line +// cv::line(outputImg, l.iP1_, l.iP2_, cv::Scalar(255), 2, cv::LINE_AA); +// } + +#ifdef DRAW_DEBUG + cv::namedWindow("All Lines", CV_WINDOW_NORMAL); + cv::imshow("All Lines", allLinesImg); + cv::namedWindow("Debug Image", CV_WINDOW_NORMAL); + cv::imshow("Debug Image", outputImg); + cv::waitKey(1); +#endif +} + +/// +/// \brief LineDetection::reconfigureCB +/// Used by the dynamic reconfigure server +/// \param config +/// \param level +/// +void LineDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level){ + image_operator_.setConfig(config); + lineWidth_ = config.lineWidth; + cannyThreshold_ = config.cannyThreshold; + houghThresold_ = config.houghThreshold; + houghMinLineLen_ = config.houghMinLineLen; + houghMaxLineGap_ = config.houghMaxLineGap; +} + +} // namespace drive_ros_image_recognition diff --git a/src/line_detection_node.cpp b/src/line_detection_node.cpp new file mode 100644 index 0000000..2bf3853 --- /dev/null +++ b/src/line_detection_node.cpp @@ -0,0 +1,26 @@ +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "line_detection"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + drive_ros_image_recognition::LineDetection lineDetection(nh, pnh); + if (!lineDetection.init()) { + return 1; + } + else { + ROS_INFO("Line detection node succesfully initialized"); + } + +#ifndef NDEBUG + // give GDB time to attach + ros::Duration(1.0).sleep(); +#endif + + while (ros::ok()) { + ros::spin(); + } + return 0; +} diff --git a/src/road_detection.cpp b/src/road_detection.cpp deleted file mode 100644 index 848503e..0000000 --- a/src/road_detection.cpp +++ /dev/null @@ -1,465 +0,0 @@ -#include "drive_ros_image_recognition/road_detection.h" -#include - -namespace drive_ros_image_recognition { - -RoadDetection::RoadDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh): - nh_(nh), - pnh_(pnh), -// img_sub_(), -// road_sub_(), -// sync_(), - searchOffset_(0.15), - distanceBetweenSearchlines_(0.1), - minLineWidthMul_(0.5), - maxLineWidthMul_(2), - findPointsBySobel_(true), - brightness_threshold_(50), - sobelThreshold_(50), - laneWidthOffsetInMeter_(0.1), - translateEnvironment_(false), - useWeights_(false), - line_output_pub_(), - it_(nh), -#ifdef PUBLISH_DEBUG - debug_img_pub_(), - detected_points_pub_(), - filtered_points_pub_(), -#endif - dsrv_server_(), - dsrv_cb_(boost::bind(&RoadDetection::reconfigureCB, this, _1, _2)), - lines_(), - linesToProcess_(0), - current_image_(), - road_points_buffer_(), - image_operator_() -, img_sub_debug_() -#ifdef PUBLISH_WORLD_POINTS - ,world_point_pub_() -#endif -{ -} - -bool RoadDetection::init() { - // load parameters - if (!pnh_.getParam("road_detection/searchOffset", searchOffset_)) { - ROS_WARN_STREAM("Unable to load 'searchOffset' parameter, using default: "<("worldPoints", 1000); -#endif - - // to synchronize incoming images and the road, we use message filters - img_sub_.reset(new image_transport::SubscriberFilter(it_,"img_in", 1000)); - road_sub_.reset(new message_filters::Subscriber(pnh_,"road_in", 1000)); - sync_.reset(new message_filters::Synchronizer(SyncImageToRoad(100), *img_sub_, *road_sub_)); - sync_->setAgePenalty(1.0); -// sync_->registerCallback(boost::bind(&RoadDetection::syncCallback, this, _1, _2)); - - // Debug callbacks for testing -// img_sub_debug_ = it_.subscribe("img_in", 1000, &RoadDetection::debugImageCallback, this); - img_sub_debug_ = it_.subscribe("img_in", 1000, boost::bind(&RoadDetection::debugDrawFrameCallback, this, - _1, std::string("/camera_optical"), std::string("/front_axis_middle"))); - - line_output_pub_ = nh_.advertise("line_out",10); - -#ifdef PUBLISH_DEBUG - debug_img_pub_ = it_.advertise("debug_image_out", 10); - detected_points_pub_ = it_.advertise("detected_points_out", 10); - filtered_points_pub_ = it_.advertise("filtered_points_out", 10); -#endif - - // todo: we have not decided on an interface for these debug channels yet - // debugAllPoints = writeChannel("DEBUG_ALL_POINTS"); - // debugValidPoints = writeChannel("DEBUG_VALID_POINTS"); - // debugTranslatedPoints = writeChannel("DEBUG_VALID_TRANSLATED_POINTS"); - - // todo: we have not defined the interface for this yet - // car = readChannel("CAR"); //TODO create ego-estimation service - return true; -} - -void RoadDetection::debugImageCallback(const sensor_msgs::ImageConstPtr& img_in) { - current_image_ = convertImageMessage(img_in); - // crop image to 410 width - cv::Rect roi(cv::Point(current_image_->cols/2-(410/2),0),cv::Point(current_image_->cols/2+(410/2),current_image_->rows)); - search_direction search_dir = search_direction::x; - search_method search_meth = search_method::sobel; - search_method search_meth_brightness = search_method::brightness; - image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth); - image_operator_.debugPointsImage((*current_image_)(roi), search_dir, search_meth_brightness); -} - -// draws draw_frame in camera image (if visible) -void RoadDetection::debugDrawFrameCallback(const sensor_msgs::ImageConstPtr& img_in, - const std::string camera_frame, - const std::string draw_frame) { - current_image_ = convertImageMessage(img_in); - - tf::TransformListener tf_listener_; - tf::StampedTransform transform; - try { - ros::Duration timeout(1); - tf_listener_.waitForTransform(camera_frame, draw_frame, - ros::Time::now(), timeout); - tf_listener_.lookupTransform(camera_frame, draw_frame, - ros::Time::now(), transform); - } - catch (tf::TransformException& ex) { - ROS_WARN("[debugDrawFrameCallback] TF exception:\n%s", ex.what()); - return; - } - - tf::Point pt = transform.getOrigin(); - cv::Point3f pt_cv(pt.x(), pt.y(), pt.z()); - cv::Point uv; - image_operator_.worldToImage(pt_cv, uv); - - cv::Mat draw_image; - cv::cvtColor(*current_image_, draw_image, cv::COLOR_GRAY2BGR); - cv::circle(draw_image, uv, 3, CV_RGB(255,0,0), -1); - cv::namedWindow("frame in image", CV_WINDOW_NORMAL); - cv::imshow("frame in image", draw_image); - cv::waitKey(1); -} - -void RoadDetection::syncCallback(const sensor_msgs::ImageConstPtr& img_in, const drive_ros_msgs::RoadLaneConstPtr& road_in){ - current_image_ = convertImageMessage(img_in); - road_points_buffer_ = road_in->points; - - //if we have a road(?), try to find the line - if(road_in->points.size() > 1){ - find(); //TODO use bool from find - } else { - ROS_WARN("Road buffer has no points stored"); - } - - -// float dx = 0; -// float dy = 0; -// float dPhi = 0; - - //update the course - // todo: no access to car command interface yet - // if(!translate_environment_){ - // logger.info("translation")<deltaPhi(); - // dPhi = car->deltaPhi(); - // } - - // todo: no interface for this yet, create course verification service to verfiy and publish - // lms::ServiceHandle localCourse = getService("LOCAL_COURSE_SERVICE"); - // if(localCourse.isValid()){ - // logger.time("localCourse"); - // localCourse->update(dx,dy,dPhi); - // *road = localCourse->getCourse(); - // logger.timeEnd("localCourse"); -#ifdef DRAW_DEBUG - // todo: no interface for this yet - // if(renderDebugImage){ - // debugTranslatedPoints->points() = localCourse->getPointsAdded(); - // } -#endif - // should do something like this - // line_output_pub_.publish(road_translated); - - // }else{ - // ROS_ERROR("LocalCourse invalid, aborting the callback!"); - // return; - // } -} - -bool RoadDetection::find(){ - //clear old lines - ROS_INFO("Creating new lines"); - lines_.clear(); - linesToProcess_ = 0; - - //create left/mid/right lane - linestring mid, left, right; - for (auto point_stamped : road_points_buffer_) { - boost::geometry::append(mid,point_xy(point_stamped.point.x, point_stamped.point.y)); - } - // simplify mid (previously done with distance-based algorithm - // skip this for now, can completely clear the line -// drive_ros_geometry_common::simplify(mid, mid, distanceBetweenSearchlines_); - drive_ros_geometry_common::moveOrthogonal(mid, left, -0.4); - drive_ros_geometry_common::moveOrthogonal(mid, right, 0.4); - if(mid.size() != left.size() || mid.size() != right.size()){ - ROS_ERROR("Generated lane sizes do not match! Aborting search!"); - return false; - } - //get all lines - auto it_mid = mid.begin(); - auto it_left = left.begin(); - auto it_right = right.begin(); - for(int it = 0; itx(),it_left->y()); - l.wEnd = cv::Point2f(it_right->x(),it_right->y()); - l.wLeft = cv::Point2f(it_left->x(),it_left->y()); - l.wMid = cv::Point2f(it_mid->x(),it_mid->y()); - l.wRight = cv::Point2f(it_right->x(),it_right->y()); - //check if the part is valid (middle should be always visible) - - cv::Point l_w_start, l_w_end; - image_operator_.worldToImage(l.wStart, l.iStart); - image_operator_.worldToImage(l.wEnd, l.iEnd); - cv::Rect img_rect(cv::Point(),cv::Point(current_image_->cols,current_image_->rows)); - // if the left side is out of the current view, use middle instead - if(!img_rect.contains(l_w_start)){ - // try middle lane -> should always be in image - l.wStart = cv::Point2f(it_mid->x(),it_mid->y()); - image_operator_.worldToImage(l.wMid, l.iStart); - if(img_rect.contains(l_w_start)){ - continue; - } - // if the right side is out of the current view, use middle instead - }else if(!img_rect.contains(l_w_end)){ - l.wEnd = cv::Point2f(it_mid->x(),it_mid->y()); - } - - image_operator_.worldToImage(l.wStart,l.iStart); - image_operator_.worldToImage(l.wEnd,l.iEnd); - lines_.push_back(l); - for(SearchLine &l:lines_){ - processSearchLine(l); - } - } - return true; -} - -void RoadDetection::processSearchLine(const SearchLine &l) { -// std::vector xv; -// std::vector yv; - - //calculate the offset - // done in search function -// float iDist = cv::norm(l.iEnd - l.iStart); -// float wDist = cv::norm(l.wEnd - l.wStart); - // add search offset -> do not need this as we do it directly in the image -// float pxlPerDist = iDist/wDist*searchOffset_; -// cv::Point2f iDiff = (l.i_start-l.i_end)/norm(l.i_start-l.i_end); -// cv::Point2f startLine = cv::Point2f(l.i_start)+iDiff*pxlPerDist; -// cv::Point2f endLine = cv::Point2f(l.i_end)-iDiff*pxlPerDist; - // //get all points in between - // lms::math::bresenhamLine(startLine.x,startLine.y,endLine.x,endLine.y,xv,yv); //wir suchen von links nach rechts! - - std::vector foundPoints; - - //find points - if(findPointsBySobel_){ - // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter - foundPoints = image_operator_.returnValidPoints(l, *current_image_, 0.02, search_direction::x, search_method::sobel); - }else{ - // todo: investigate why line width was hard-coded at 0.02 -> maybe add parameter - foundPoints = image_operator_.returnValidPoints(l, *current_image_, 0.02, search_direction::x, search_method::brightness); - } - - // draw unfiltered image points -#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) - cv::namedWindow("Unfiltered points", CV_WINDOW_NORMAL); - cv::Mat found_points_mat = current_image_->clone(); - for (auto point : foundPoints) { - cv::circle(found_points_mat, point, 2, cv::Scalar(255)); - } - // draw search line - cv::line(found_points_mat, l.iStart, l.iEnd, cv::Scalar(255)); -#ifdef DRAW_DEBUG - cv::imshow("Unfiltered points", found_points_mat); - cv::waitKey(1); -#endif - -#ifdef PUBLISH_DEBUG - detected_points_pub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, found_points_mat).toImageMsg()); -#endif -#endif - - //filter - //TODO filter points that are in poluted regions (for example the car itself) - //remove points with bad distances - std::vector validPoints; - if(foundPoints.size() > 2){ - std::vector foundCounter; - foundCounter.resize(foundPoints.size(),0); - for(std::size_t fp = 0; fp < foundPoints.size(); fp++){ - // todo: insert service here - const cv::Point2f &s = foundPoints[fp]; - for(std::size_t test = fp+1; test 0.4-laneWidthOffsetInMeter_ && distance < 0.4 + laneWidthOffsetInMeter_)|| (distance > 0.8-laneWidthOffsetInMeter_ && distance < 0.8+laneWidthOffsetInMeter_)){ - foundCounter[test]++; - foundCounter[fp]++; - } - } - } - //filter, all valid points should have the same counter and have the highest number - int max = 0; - for(int c:foundCounter){ - if(c > max){ - max = c; - } - } - for(int i = 0; i < (int)foundPoints.size(); i++){ - if(foundCounter[i] >= max){ - validPoints.push_back(foundPoints[i]); - } - } - //TODO if we have more than 3 points we know that there is an error! - // foundPoints = validPoints; - } - else { - // just use the 2 points we found otherwise - validPoints = foundPoints; - } - - // draw filtered points -#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) - // todo: make some more efficient storage method here, will translate back and forth here for now - cv::namedWindow("Filtered Points", CV_WINDOW_NORMAL); - cv::Mat filtered_points_mat = current_image_->clone(); - cv::Point img_point; - for (auto point : validPoints) { - image_operator_.worldToImage(point, img_point); - cv::circle(filtered_points_mat, img_point, 2, cv::Scalar(255)); - } -#ifdef DRAW_DEBUG - cv::imshow("Filtered Points", filtered_points_mat); -#endif -#ifdef PUBLISH_DEBUG - filtered_points_pub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, filtered_points_mat).toImageMsg()); -#endif -#endif - - //Handle found points - cv::Point2f diff; - std::vector distances; - for(int i = 0; i < (int)validPoints.size(); i++){ - float distanceToLeft = cv::norm(validPoints[i]-l.wLeft); - float distanceToRight= cv::norm(validPoints[i]-l.wRight); - float distanceToMid= cv::norm(validPoints[i]-l.wMid); - // normalize distance - diff = (l.wLeft - l.wRight)/cv::norm(l.wLeft-l.wRight); - if(distanceToLeft < distanceToMid && distanceToLeft < distanceToRight){ - //left point - validPoints[i]-=diff*0.4; - distances.push_back(distanceToLeft); - }else if(distanceToRight < distanceToMid ){ - validPoints[i]+=diff*0.4; - distances.push_back(distanceToRight); - }else{ - distances.push_back(distanceToMid); - } - } - - std::vector weights; - for(const float &dist:distances){ - //as distance is in meter, we multiply it by 100 - if(useWeights_) - weights.push_back(1/(dist*100+0.001)); //TODO hier etwas sinnvolles überlegen - else - weights.push_back(1); - } - - // todo: check how this gets handled and adjust the interface - drive_ros_msgs::RoadLane lane_out; - lane_out.header.stamp = ros::Time::now(); - geometry_msgs::PointStamped point_temp; - point_temp.header.frame_id = std::string("/rear_axis_middle"); - point_temp.point.z = 0.f; - for (auto point: validPoints) { - point_temp.point.x = point.x; - point_temp.point.y = point.y; - lane_out.points.push_back(point_temp); - } - lane_out.roadStateType = drive_ros_msgs::RoadLane::UNKNOWN; - line_output_pub_.publish(lane_out); -} - -void RoadDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig &config, uint32_t level){ - image_operator_.setConfig(config); - searchOffset_ = config.searchOffset; - findPointsBySobel_ = config.findBySobel; - minLineWidthMul_ = config.minLineWidthMul; - maxLineWidthMul_ = config.maxLineWidthMul; - brightness_threshold_ = config.brightness_threshold; - laneWidthOffsetInMeter_ = config.laneWidthOffsetInMeter; - useWeights_ = config.useWeights; - sobelThreshold_ = config.sobelThreshold; -} - -RoadDetection::~RoadDetection() { -} - -void RoadDetectionNodelet::onInit() { - road_detection_.reset(new RoadDetection(getNodeHandle(),getPrivateNodeHandle())); - if (!road_detection_->init()) { - ROS_ERROR("road_detection nodelet failed to initialize"); - // nodelet failing will kill the entire loader anyway - ros::shutdown(); - } - else { - ROS_INFO("road detection nodelet succesfully initialized"); - } -} - -} // namespace drive_ros_image_recognition - -PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::RoadDetectionNodelet, nodelet::Nodelet) diff --git a/src/road_detection_node.cpp b/src/road_detection_node.cpp deleted file mode 100644 index 90ff1b3..0000000 --- a/src/road_detection_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "road_detection"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - -#ifndef NDEBUG - // give GDB time to attach - ros::Duration(2.0).sleep(); -#endif - - drive_ros_image_recognition::RoadDetection road_detection(nh,pnh); - if (!road_detection.init()) { - return 1; - } - else { - ROS_INFO("road detection node succesfully initialized"); - } - - while (ros::ok()) { - ros::spin(); - } - return 0; -} diff --git a/src/street_crossing.cpp b/src/street_crossing.cpp deleted file mode 100644 index 20c22eb..0000000 --- a/src/street_crossing.cpp +++ /dev/null @@ -1,332 +0,0 @@ -#include -#include -#include -#include -#include "drive_ros_image_recognition/street_crossing.h" -#include - -namespace drive_ros_image_recognition { - -StreetCrossingDetection::StreetCrossingDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh) - : nh_(nh) - , pnh_(pnh) - , imageTransport_(pnh) - , sobelThreshold_(50) - , minLineWidthMul_(0.5) - , maxLineWidthMul_(2.0) - , lineWidth_(0.4) - , line_output_pub_() - , transform_helper_() - , image_operator_() - , dsrv_server_() - , dsrv_cb_(boost::bind(&StreetCrossingDetection::reconfigureCB, this, _1, _2)) - , onRightLine(true) -// #ifdef DRAW_DEBUG -// , debugImage(0, 0, CV_8UC1) -// #endif -{ -} - -StreetCrossingDetection::~StreetCrossingDetection() { -} - -bool StreetCrossingDetection::init() { - // dynamic reconfigure - dsrv_server_.setCallback(dsrv_cb_); - - // image operations -// if(!transform_helper_.init()) -// ROS_INFO_STREAM("Init transform_helper failed"); -// image_operator_ = ImageOperator(transform_helper_); - - //subscribe to camera image - imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &StreetCrossingDetection::imageCallback, this); - ROS_INFO_STREAM("Subscribed image transport to topic " << imageSubscriber_.getTopic()); - - // subscribe to warped image -// warpedImageSubscriber_ = imageTransport_.subscribe("warped_in", 10, &StreetCrossingDetection::warpedImageCallback, this); -// ROS_INFO_STREAM("Subscribed image transport to topic " << warpedImageSubscriber_.getTopic()); - - // publish road lane -// line_output_pub_ = pnh_.advertise("line_out",10); - -#ifdef PUBLISH_DEBUG - debugImagePublisher_ = imageTransport_.advertise("/street_crossing/debug_image", 10); - ROS_INFO_STREAM("Publishing debug image on " << debugImagePublisher_.getTopic()); -#endif - - // common image operations - image_operator_.init(); - std::string world_frame("/rear_axis_middle_ground"); - if (!pnh_.getParam("world_frame", world_frame)) - ROS_WARN_STREAM("Parameter 'world_frame' not found, using default: "<(t_end-t_start).count()) << "ms"); -} - -void StreetCrossingDetection::findLane() { - cv::Mat debugImg, outputImg; - cv::cvtColor(*currentImage_, outputImg, CV_GRAY2RGB); - cv::Mat laneImg = cv::Mat::zeros(outputImg.rows, outputImg.cols, outputImg.type()); - - // Blur the debugImage and find edges with Canny - cv::GaussianBlur(*currentImage_, debugImg, cv::Size(15, 15), 0, 0); - cv::Canny(debugImg, debugImg, cannyThreshold_, cannyThreshold_ * 3, 3); - - // Zero out the vehicle - int w = debugImg.cols; - int h = debugImg.rows; - cv::Point vehiclePolygon[1][4]; - vehiclePolygon[0][0] = cv::Point(w * .35, h); - vehiclePolygon[0][1] = cv::Point(w * .65, h); - vehiclePolygon[0][2] = cv::Point(w * .65, h * .8); - vehiclePolygon[0][3] = cv::Point(w * .35, h * .8); - const cv::Point* ppt[1] = { vehiclePolygon[0] }; - int npt[] = { 4 }; - cv::fillPoly(debugImg, ppt, npt, 1, cv::Scalar(), cv::LINE_8); - - // If we dont have a lane from last step (or this is the first search), use straight line forward - if(currentLaneMid.empty()) { - currentLaneMid.push_back(StreetLine(cv::Point(w/2, h), cv::Point(w/2, 0))); - } - - // Display the polygon which will be zeroed out -// cv::line(outputImg, vehiclePolygon[0][0], vehiclePolygon[0][1], cv::Scalar(255), 2, cv::LINE_AA); -// cv::line(outputImg, vehiclePolygon[0][1], vehiclePolygon[0][2], cv::Scalar(255), 2, cv::LINE_AA); -// cv::line(outputImg, vehiclePolygon[0][2], vehiclePolygon[0][3], cv::Scalar(255), 2, cv::LINE_AA); -// cv::line(outputImg, vehiclePolygon[0][3], vehiclePolygon[0][0], cv::Scalar(255), 2, cv::LINE_AA); - - // Get houghlines and draw them on the output image - std::vector hLinePoints; - std::vector leftLines, midLines, rightLines; - cv::HoughLinesP(debugImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); - - // Assign the hough-lines returned by HoughLinesP to left, mid, or right line - for(size_t i = 0; i < hLinePoints.size(); i++) { - auto l = StreetLine(hLinePoints[i]); - - // todo: find current line segment - StreetLine currentSegment = currentLaneMid.at(0); - - if(currentSegment.mid_.x < l.mid_.x) { - rightLines.push_back(l); - } else if((currentSegment.mid_.x - (w * .15)) > l.mid_.x) { - leftLines.push_back(l); - } else { - midLines.push_back(l); - } - - - // (0,0) (1280,344) - // (-0.0506457,3.06453) (6.85171,-0.989774) - cv::Point2f wPt1, wPt2; -// image_operator_.imageToWorld(l.p1_, wPt1); -// image_operator_.imageToWorld(l.p2_, wPt2); - image_operator_.imageToWorld(cv::Point(0, 0), wPt1); - image_operator_.imageToWorld(cv::Point(w, h), wPt2); - cv::line(laneImg, wPt1, wPt2, cv::Scalar(255), 2, cv::LINE_AA); - ROS_INFO_STREAM("Transformed (0,0) to (" << wPt1.x << "," << wPt1.y <<")"); - ROS_INFO_STREAM("Transformed (" << w << "," << h << " to (" << wPt2.x << "," << wPt2.y << ")"); - } - - - cv::line(outputImg, cv::Point(w * .15, h), cv::Point(w * .45, 0), cv::Scalar(0, 255, 255), 2, cv::LINE_AA); - - for(auto l : leftLines) - cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(255), 2, cv::LINE_AA); - for(auto l : midLines) - cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 255), 2, cv::LINE_AA); - for(auto l : rightLines) - cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); - -// auto a = l.getAngle(); -// if((a > (CV_PI/6)) && (a < 2.6)) { -// // lane line -// for(size_t j = 0; j < laneLines.size(); j++) { -// auto d = abs(l.mid_.x - laneLines.at(j).mid_.x); -// if(d < sameLineThreshold_) { -// laneLines.at(j).vote++; -// l.vote++; -// } -// } -// laneLines.push_back(l); -// } else { -// // stop line -// for(size_t j = 0; j < stopLines.size(); j++) { -// auto d = abs(l.mid_.y - stopLines.at(j).mid_.y); -// if(d < sameLineThreshold_) { -// stopLines.at(j).vote++; -// l.vote++; -// } -// } -// stopLines.push_back(l); -// } - -// for(auto l : laneLines) { -// if(l.vote > 0) -// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0,255), 2, cv::LINE_AA); -// else -// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); -// } - -// for(auto l : stopLines) { -// if(l.vote > 0) -// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(255), 2, cv::LINE_AA); -// else -// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); -// } - -// bool added = false; -// for(size_t j = 0; j < myLines.size(); j++) { -// // todo: use world distance here with linewidth -// auto d = cv::norm(l.mid_ - myLines.at(j).mid_); -// if(d < sameLineThreshold_) { -// myLines.at(j).vote++; -// added = true; -// } -// } -// if(!added) { -// myLines.push_back(l); -// } -// } - -// int linesDrawn = 0; -// for(size_t i = 0; i < myLines.size(); i++) { -// auto l = myLines.at(i); -// auto a = l.getAngle(); -// if(l.vote > 0) { -// linesDrawn++; -// if((a > (CV_PI/6)) && (a < 2.6)) { -// // lane marking -// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0,255), 2, cv::LINE_AA); -// } else { -// // stop or start line -// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(255), 2, cv::LINE_AA); -// } -// } else { -// // discarded line due to votes -// cv::line(outputImg, l.p1_, l.p2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); -// } -// } - -// ROS_INFO_STREAM("out of " << hLinePoints.size() << " hough lines, " << linesDrawn << " survived"); - -#ifdef PUBLISH_DEBUG - debugImagePublisher_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, outputImg).toImageMsg()); -#endif - - cv::namedWindow("debug image", CV_WINDOW_NORMAL); - cv::imshow("debug image", outputImg); - cv::namedWindow("lane image", CV_WINDOW_NORMAL); - cv::imshow("lane image", laneImg); - cv::waitKey(1); -} - -bool StreetCrossingDetection::findStartline() { - SearchLine slRightLane; -// SearchLine slLeftLane; - - // use search-line to find stop-line - slRightLane.iStart = cv::Point2f(currentImage_->cols / 2, currentImage_->rows * .4); - slRightLane.iEnd = cv::Point2f(currentImage_->cols / 2, currentImage_->rows * .8); - -// slLeftLane.iStart = cv::Point2f(currentImage_->cols / 8 * 3, currentImage_->rows * .4); -// slLeftLane.iEnd = cv::Point2f(currentImage_->cols / 4, currentImage_->rows * .8); -// slRightLane.iStart = cv::Point2f(currentImage_->cols * .55, currentImage_->rows * .4); -// slRightLane.iEnd = cv::Point2f(currentImage_->cols * .55, currentImage_->rows * .8); - -// slLeftLane.iStart = cv::Point2f(currentImage_->cols * .45, currentImage_->rows * .4); -// slLeftLane.iEnd = cv::Point2f(currentImage_->cols * .45, currentImage_->rows * .8); - - // find line using unified header - search_direction search_dir = search_direction::y; - search_method search_meth = search_method::sobel; - std::vector image_points; - std::vector line_widths; - image_operator_.findByLineSearch(slRightLane,*currentImage_,search_dir,search_meth,image_points,line_widths); -// image_operator_.findByLineSearch(slLeftLane,*currentImage_,search_dir,search_meth,image_points,line_widths); - if(image_points.empty()) { - startLineCounter_ = 0; - } else { - startLineCounter_++; - if(image_points.size() > 1) { - ROS_INFO("Found more than one start/stop line"); - } - } -#if defined(DRAW_DEBUG) || defined(PUBLISH_DEBUG) - cv::Mat debug_image; - cv::cvtColor(*currentImage_, debug_image, CV_GRAY2RGB); - cv::line(debug_image, slRightLane.iStart, slRightLane.iEnd, cv::Scalar(255, 255, 255), 2); -// cv::line(debug_image, slLeftLane.iStart, slLeftLane.iEnd, cv::Scalar(255, 255, 255), 2); - if(startLineCounter_ > 0) { - for (auto point: image_points) { - cv::circle(debug_image,point,2,cv::Scalar(0,255,0),3); - } - } -#endif -#ifdef DRAW_DEBUG - cv::namedWindow("Crossing Search debug",CV_WINDOW_NORMAL); - cv::imshow("Crossing Search debug",debug_image); - cv::waitKey(1); -#endif -#ifdef PUBLISH_DEBUG -// debugImagePublisher.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_8UC1, debugImage).toImageMsg()); -#endif - - // test array of lines - // image_operator_.debugPointsImage(*currentImage, search_dir, search_meth); -} - -void StreetCrossingDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level){ - image_operator_.setConfig(config); - sobelThreshold_ = config.sobelThreshold; - minLineWidthMul_ = config.minLineWidthMul; - maxLineWidthMul_ = config.maxLineWidthMul; - lineWidth_ = config.lineWidth; - - cannyThreshold_ = config.cannyThreshold; - houghThresold_ = config.houghThreshold; - houghMinLineLen_ = config.houghMinLineLen; - houghMaxLineGap_ = config.houghMaxLineGap; - sameLineThreshold_ = config.sameLineThreshold; -} - -void StreetCrossingDetectionNodelet::onInit() { - street_crossing_detection_.reset(new StreetCrossingDetection(getNodeHandle(),getPrivateNodeHandle())); - if (!street_crossing_detection_->init()) { - ROS_ERROR("StreetCrossing nodelet failed to initialize"); - // nodelet failing will kill the entire loader anyway - ros::shutdown(); - } - else { - ROS_INFO("StreetCrossing detection nodelet succesfully initialized"); - } -} - -} // namespace drive_ros_image_recognition - -PLUGINLIB_EXPORT_CLASS(drive_ros_image_recognition::StreetCrossingDetectionNodelet, nodelet::Nodelet) diff --git a/src/street_crossing_node.cpp b/src/street_crossing_node.cpp deleted file mode 100644 index 46f85a2..0000000 --- a/src/street_crossing_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "street_crossing_detection"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - - drive_ros_image_recognition::StreetCrossingDetection streetCrossingDetection(nh, pnh); - if (!streetCrossingDetection.init()) { - return 1; - } - else { - ROS_INFO("Street crossing detection node succesfully initialized"); - } - -#ifndef NDEBUG - // give GDB time to attach - ros::Duration(1.0).sleep(); -#endif - - while (ros::ok()) { - ros::spin(); - } - return 0; -} diff --git a/src/warp_image.cpp b/src/warp_image.cpp index efeb04a..4251760 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -56,6 +56,7 @@ bool WarpContent::init() { homography_params_sub_ = nh_.subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), + std::ref(scaledCam2world_), std::ref(scaledWorld2cam_), std::ref(scaling_mat_inv_), std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); From 7ff2b6354c305faa6c7c6c9e3e78f9571967e824 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Wed, 8 Nov 2017 20:48:34 +0100 Subject: [PATCH 76/94] finish merge --- CMakeLists.txt | 64 --------------- .../common_image_operations.h | 79 ------------------- .../drive_ros_image_recognition/warp_image.h | 3 - src/road_detection_node.cpp | 26 ------ src/warp_image.cpp | 3 - 5 files changed, 175 deletions(-) delete mode 100644 src/road_detection_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 14807b7..9d4806d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,66 +50,6 @@ include_directories( ${Boost_INCLUDE_DIRS} ) -<<<<<<< HEAD -======= - -################################################################################ -# road detection node (LMS port) -################################################################################ - -add_executable(road_detection_node - src/road_detection.cpp - src/road_detection_node.cpp - ) - -add_dependencies(road_detection_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(road_detection_node - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ${Boost_LIBRARIES} - ) - -install(TARGETS road_detection_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(road_detection_node_test launch/road_detection.test - test/common_image_operations_test.cpp - test/road_detection_test.cpp - test/geometry_common_test.cpp - ) - target_link_libraries(road_detection_node_test ${catkin_LIBRARIES}) -endif() - - -################################################################################ -# road detection nodelet (LMS port) -################################################################################ - -add_library(road_detection_nodelet - src/road_detection.cpp - ) - -add_dependencies(road_detection_nodelet ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(road_detection_nodelet - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ${Boost_LIBRARIES} - ) - -install(TARGETS road_detection_nodelet - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da ################################################################################ # Warp image node ################################################################################ @@ -152,11 +92,7 @@ install(TARGETS warp_image_nodelet ) ################################################################################ -<<<<<<< HEAD # Line detection node -======= -# Street crossing detection nodelet (LMS port) ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da ################################################################################ add_executable(line_detection_node diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index c239f10..dc6f40c 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -39,7 +39,6 @@ inline CvImagePtr convertImageMessage(const sensor_msgs::ImageConstPtr& img_in) namespace drive_ros_image_recognition { inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_in, cv::Mat& cam2world, cv::Mat& world2cam, -<<<<<<< HEAD cv::Mat& scaling_mat, cv::Mat& scaling_mat_inv, cv::Mat& scaledCam2World, cv::Mat& scaledWorld2Cam, bool& homography_received) { if (homography_received) { @@ -50,12 +49,6 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i homography_received = true; // extract cam2world matrix from the homography message -======= - cv::Mat& scaling_mat, cv::Mat& scaling_mat_inv, bool& homography_received) { - if (!homography_received) - homography_received = true; - ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da ROS_ASSERT(homo_in->cam2world.layout.dim[0].size == 3 && homo_in->cam2world.layout.dim[1].size == 3 ); cam2world = cv::Mat::zeros(3,3,CV_64FC1); int k=0; @@ -65,10 +58,7 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i } } -<<<<<<< HEAD // extract world2cam matrix from the homography message -======= ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da ROS_ASSERT(homo_in->world2cam.layout.dim[0].size == 3 && homo_in->world2cam.layout.dim[1].size == 3 ); world2cam = cv::Mat::zeros(3,3,CV_64FC1); k=0; @@ -78,7 +68,6 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i } } -<<<<<<< HEAD // we need the scaled matrices for the warped image (bird eye view) scaling_mat_inv = scaling_mat.inv(); scaledCam2World = scaling_mat_inv * cam2world; @@ -87,11 +76,6 @@ inline void homography_callback(const drive_ros_msgs::HomographyConstPtr& homo_i ROS_INFO_STREAM("Scaling mat:\n" << scaling_mat); ROS_INFO_STREAM("received cam2world:\n" << cam2world); ROS_INFO_STREAM("received world2cam:\n" << world2cam); -======= - ROS_INFO_STREAM("Scaling mat: "<>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da } inline void camInfo_callback(const sensor_msgs::CameraInfoConstPtr& incoming_cam_info, image_geometry::PinholeCameraModel& cam_model, bool& cam_info_received) { @@ -172,10 +156,7 @@ class TransformHelper { boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), std::ref(scaling_mat_inv_), -<<<<<<< HEAD std::ref(scaledCam2world_), std::ref(scaledWorld2cam_), -======= ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da std::ref(homography_received_))); other.cam_info_sub_.shutdown(); @@ -200,10 +181,7 @@ class TransformHelper { boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), std::ref(scaling_mat_inv_), -<<<<<<< HEAD std::ref(scaledCam2world_), std::ref(scaledWorld2cam_), -======= ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da std::ref(homography_received_))); } @@ -256,10 +234,7 @@ class TransformHelper { boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), std::ref(scaling_mat_inv_), -<<<<<<< HEAD std::ref(scaledCam2world_), std::ref(scaledWorld2cam_), -======= ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da std::ref(homography_received_))); } @@ -299,7 +274,6 @@ class TransformHelper { camera_frame_ = frame; } -<<<<<<< HEAD /// /// \brief imageToWorld /// Converts all imagePoints to the corresponding worldPoints using the cam2world matrix. @@ -313,34 +287,6 @@ class TransformHelper { return false; } cv::perspectiveTransform(imagePoints, worldPoints, cam2world_); -======= - bool worldToImage(const cv::Point3f& world_point, - cv::Point& image_point) { - - if(!homography_received_ || !camera_model_received_) { - if (!homography_received_) - ROS_WARN_STREAM("[WorldToImage] Homography not received yet, skipping world to image transformation requested for world point "<(0,0) = pixel_coords.x; - mat_pixel_coords.at(0,1) = pixel_coords.y; - cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); - if (use_homography_) { - cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_inv_); - image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); - } - else { - // nothing to transform if homography has not been applied - image_point = cv::Point(pixel_coords.x, pixel_coords.y); - } ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da return true; } @@ -360,7 +306,6 @@ class TransformHelper { return true; } -<<<<<<< HEAD /// /// \brief homographImage /// Warps the srcImg to get the "bird eye view" image. @@ -398,8 +343,6 @@ class TransformHelper { * Simon says: these two methods are not working properly, since we are doing the homography twice (first use the homog matrix * and then the TF conversion. I will leave this in, in case we need the tf version later. */ -======= ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da bool imageToWorld(const cv::Point& image_point, cv::Point2f& world_point, std::string target_frame = std::string("/rear_axis_middle_ground"), cv::Mat test_image = cv::Mat()) { if(!homography_received_ || !camera_model_received_) { if (!homography_received_) @@ -409,21 +352,6 @@ class TransformHelper { world_point = cv::Point2f(0.0,0.0); return false; } -<<<<<<< HEAD - - std::vector obj_corners(1); - obj_corners[0] = cv::Point2f(image_point.x, image_point.y); - std::vector scene_corners(1); - if (homography_received_) -// cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_inv_); - cv::perspectiveTransform(obj_corners, scene_corners, cam2world_); - else - scene_corners[0] = obj_corners[0]; // no homography transform if we use camera image directly - - world_point = scene_corners.at(0); - return true; - -======= std::vector obj_corners(1); obj_corners[0] = cv::Point2f(image_point.x, image_point.y); @@ -432,7 +360,6 @@ class TransformHelper { cv::perspectiveTransform(obj_corners, scene_corners, scaling_mat_); else scene_corners[0] = obj_corners[0]; // no homography transforms if we use camera image directly ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da // this returns a point with z=1, the pixel is on a ray from origin to this point cv::Point3d cam_ray_point = cam_model_.projectPixelTo3dRay(scene_corners[0]); @@ -474,7 +401,6 @@ class TransformHelper { ROS_WARN_STREAM("[imageToWorld] Camera ray and ground plane are parallel, unable to transform image point "<>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da } void setcam2worldMat(const cv::Mat& cam2world) { @@ -536,11 +460,8 @@ class TransformHelper { cv::Mat world2cam_; cv::Mat scaling_mat_; cv::Mat scaling_mat_inv_; -<<<<<<< HEAD cv::Mat scaledWorld2cam_; cv::Mat scaledCam2world_; -======= ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da cv::Size transformed_size_; tf::TransformListener tf_listener_; ros::Subscriber cam_info_sub_; diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 8475442..09375bf 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -43,11 +43,8 @@ class WarpContent { cv::Mat cam2world_; cv::Mat scaling_mat_; cv::Mat scaling_mat_inv_; -<<<<<<< HEAD cv::Mat scaledCam2world_; cv::Mat scaledWorld2cam_; -======= ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da cv::Size transformed_size_; bool homo_received_; }; diff --git a/src/road_detection_node.cpp b/src/road_detection_node.cpp deleted file mode 100644 index 90ff1b3..0000000 --- a/src/road_detection_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "road_detection"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - -#ifndef NDEBUG - // give GDB time to attach - ros::Duration(2.0).sleep(); -#endif - - drive_ros_image_recognition::RoadDetection road_detection(nh,pnh); - if (!road_detection.init()) { - return 1; - } - else { - ROS_INFO("road detection node succesfully initialized"); - } - - while (ros::ok()) { - ros::spin(); - } - return 0; -} diff --git a/src/warp_image.cpp b/src/warp_image.cpp index cf3f313..4251760 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -56,10 +56,7 @@ bool WarpContent::init() { homography_params_sub_ = nh_.subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), -<<<<<<< HEAD std::ref(scaledCam2world_), std::ref(scaledWorld2cam_), -======= ->>>>>>> 28a7bf5183753e1b6f8cbad083fa477061a344da std::ref(scaling_mat_inv_), std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); From f294dbe3b72b84fa169702440ef176b03c08f2d0 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Thu, 9 Nov 2017 13:24:49 +0100 Subject: [PATCH 77/94] remove unused code. warp_image node uses image_operator now --- README.md | 9 +- cfg/LineDetection.cfg | 25 +- config/road_detection.yaml | 13 - .../common_image_operations.h | 257 +----------------- .../{street_line.h => line.h} | 11 +- .../line_detection.h | 4 +- .../drive_ros_image_recognition/warp_image.h | 22 +- launch/line_detection.launch | 1 - launch/road_detection.launch | 50 ---- launch/road_detection.test | 3 - launch/warp_image.launch | 4 +- src/line_detection.cpp | 16 +- src/warp_image.cpp | 77 ++---- 13 files changed, 57 insertions(+), 435 deletions(-) delete mode 100644 config/road_detection.yaml rename include/drive_ros_image_recognition/{street_line.h => line.h} (75%) delete mode 100644 launch/road_detection.launch delete mode 100644 launch/road_detection.test diff --git a/README.md b/README.md index cdfdc7b..6dbec09 100644 --- a/README.md +++ b/README.md @@ -6,11 +6,4 @@ For using the line detection, we need * the homography. Therefore you can use `drive_ros_ximea_importer/launch/homography_publischer_only.launch`. Now start `drive_ros_image_recognition/launch/line_recognition.launch`. - -The currently used files are: -* /include/drive_ros_image_recognition/common_image_operations.h -* /include/drive_ros_image_recognition/line_detection.h -* /include/drive_ros_image_recognition/street_line.h -* /launch/line_detection.h -* /src/line_detection.cpp -* /src/line_detection_node.cpp +For `drive_ros_image_recognition/launch/warp_image.launch` you have to do the same two steps from above. diff --git a/cfg/LineDetection.cfg b/cfg/LineDetection.cfg index 131f8e5..206bf8a 100755 --- a/cfg/LineDetection.cfg +++ b/cfg/LineDetection.cfg @@ -5,25 +5,12 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("searchOffset", double_t, 0, "Search offset", 0.15, 0.0, 2.0) -gen.add("distanceBetweenSearchlines", double_t, 0, "Distance between search lines", 0.1, 0.0, 1.0) -gen.add("minLineWidthMul", double_t, 0, "minLineWidthMul", 0.5, 0.0, 1.0) -gen.add("maxLineWidthMul", double_t, 0, "maxLineWidthMul", 2, 0.0, 5.0) -gen.add("findBySobel", bool_t, 0, "Use sobel", True) -gen.add("brightness_threshold", int_t, 0, "Brightness threshold", 150, 0, 255) -#gen.add("blob_area_threshold", int_t, 0, "Blob area threshold", 5000, 0, 10000) -#gen.add("blob_aspect_threshold", double_t, 0, "Blob aspect ratio threshold", 2.0, 0.0, 5.0) -gen.add("sobelThreshold", int_t, 0, "Sobel threshold", 50, 0, 200) -gen.add("laneWidthOffsetInMeter", double_t, 0, "Lane width offset (m)", 0.1, 0.0, 2.0) -gen.add("translateEnvironment", bool_t, 0, "Translate environment", False) -gen.add("useWeights", bool_t, 0, "Use weights", False) -gen.add("lineWidth", double_t, 0, "Line width (world coordinates)", 0.4, 0.0, 1.0) - -gen.add("cannyThreshold", int_t, 0, "cannyThreshold", 30, 10, 500) -gen.add("houghThreshold", int_t, 0, "houghThreshold", 20, 5, 100) -gen.add("houghMinLineLen", int_t, 0, "houghMinLineLen", 20, 5, 100) -gen.add("houghMaxLineGap", int_t, 0, "houghMaxLineGap", 10, 0, 300) -gen.add("sameLineThreshold", int_t, 0, "sameLineThreshold", 40, 1, 100) +gen.add("lineWidth", double_t, 0, "Line width (world coordinates)", 0.4, 0.0, 1.0) +gen.add("cannyThreshold", int_t, 0, "cannyThreshold", 30, 10, 500) +gen.add("houghThreshold", int_t, 0, "houghThreshold", 20, 5, 100) +gen.add("houghMinLineLen", int_t, 0, "houghMinLineLen", 20, 5, 100) +gen.add("houghMaxLineGap", int_t, 0, "houghMaxLineGap", 10, 0, 300) +gen.add("sameLineThreshold", int_t, 0, "sameLineThreshold", 40, 1, 100) exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "LineDetection")) diff --git a/config/road_detection.yaml b/config/road_detection.yaml deleted file mode 100644 index 9009a35..0000000 --- a/config/road_detection.yaml +++ /dev/null @@ -1,13 +0,0 @@ -road_detection: - searchOffset: 0.15 - distanceBetweenSearchlines: 0.1 - minLineWidthMul: 0.5 - maxLineWidthMul: 2 - findBySobel: true - brightness_threshold: 50 - sobelThreshold: 50 - laneWidthOffsetInMeter: 0.1 - translateEnvironment: false - useWeights: false - renderDebugImage: true - threads: 0 diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index dc6f40c..01e715e 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -92,32 +92,10 @@ inline void camInfo_callback(const sensor_msgs::CameraInfoConstPtr& incoming_cam cam_info_received = true; } -enum search_direction { - x = false, - y = true -}; - -enum search_method { - sobel = false, - brightness = true -}; - -struct SearchLine{ - cv::Point2f wStart; - cv::Point2f wEnd; - cv::Point2i iStart; - cv::Point2i iEnd; - - cv::Point2f wLeft; - cv::Point2f wMid; - cv::Point2f wRight; -}; - class TransformHelper { public: - - TransformHelper(bool use_homography = true): + TransformHelper(): tf_listener_(), world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), cam_model_(), @@ -131,8 +109,7 @@ class TransformHelper { camera_model_received_(false), // temporary solution until have embedded correct frame in camerainfo messages (including the bag files) camera_frame_(""), - world_frame_(""), - use_homography_(use_homography) + world_frame_("") { } @@ -197,8 +174,7 @@ class TransformHelper { camera_model_received_(false), // temporary solution until have embedded correct frame in camerainfo messages (including the bag files) camera_frame_(""), - world_frame_(""), - use_homography_(true) + world_frame_("") { } @@ -236,10 +212,7 @@ class TransformHelper { std::ref(scaling_mat_), std::ref(scaling_mat_inv_), std::ref(scaledCam2world_), std::ref(scaledWorld2cam_), std::ref(homography_received_))); - } - - void setCamModel(const image_geometry::PinholeCameraModel& cam_model) { - cam_model_ = cam_model; + return true; } void setWorldFrame(const std::string& frame) { @@ -277,6 +250,8 @@ class TransformHelper { /// /// \brief imageToWorld /// Converts all imagePoints to the corresponding worldPoints using the cam2world matrix. + /// Important: x and y switches when converted. The origin is on the camera mount point on the ground. + /// The y-cooridnate goes to the front of the car, the x-coordinate to the left. /// \param imagePoints /// \param worldPoints /// \return true on success, false on failure. @@ -293,6 +268,8 @@ class TransformHelper { /// /// \brief worldToImage /// Converts all worldPoints to the corresponding imagePoints using the world2cam matrix. + /// Important: x and y switches when converted. The origin is on the camera mount point on the ground. + /// The y-cooridnate goes to the front of the car, the x-coordinate to the left. /// \param worldPoints /// \param imagePoints /// \return true on success, false on failure. @@ -428,29 +405,15 @@ class TransformHelper { mat_pixel_coords.at(0,0) = pixel_coords.x; mat_pixel_coords.at(0,1) = pixel_coords.y; cv::Mat mat_image_point(1,1,CV_64FC2,cv::Scalar(0.0)); - if (use_homography_) { - cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_); - image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); - } - else { - // nothing to transform if homography has not been applied - image_point = cv::Point(pixel_coords.x, pixel_coords.y); - } + cv::perspectiveTransform(mat_pixel_coords, mat_image_point, scaling_mat_); + image_point = cv::Point(mat_image_point.at(0,0), mat_image_point.at(0,1)); - return true; - } - void setcam2worldMat(const cv::Mat& cam2world) { - cam2world_ = cam2world; - } - - void setworld2camMat(const cv::Mat& world2cam) { - world2cam_ = world2cam; + return true; } private: image_geometry::PinholeCameraModel cam_model_; - bool use_homography_; bool homography_received_; bool camera_model_received_; std::string world_frame_; @@ -476,8 +439,6 @@ class ImageOperator : public TransformHelper { TransformHelper(), config_() { - config_.sobelThreshold = 50; - config_.brightness_threshold = 150; } ImageOperator(ImageOperator& image_operator): @@ -496,202 +457,6 @@ class ImageOperator : public TransformHelper { TransformHelper(helper), config_() { - config_.sobelThreshold = 50; - config_.brightness_threshold = 150; - } - - std::vector returnValidPoints(const SearchLine &sl, - const cv::Mat& current_image, - const float lineWidth, - const search_direction search_dir, - const search_method method) - { - std::vector imagePoints; - std::vector lineWidths; - std::vector foundPoints; - - float iDist = cv::norm(sl.iEnd - sl.iStart); - float wDist = cv::norm(sl.wEnd - sl.wStart); - float pxlPeakWidth = iDist/wDist*lineWidth; - findByLineSearch(sl, current_image, search_dir, method, imagePoints, lineWidths); - - cv::Point2f wMid; - - for (int i=0; i pxlPeakWidth*config_.minLineWidthMul && lineWidths[i] < pxlPeakWidth*config_.maxLineWidthMul) { - imageToWorld(imagePoints[i], wMid); - foundPoints.push_back(wMid); - } - } - return foundPoints; - } - - void findByLineSearch(const SearchLine& sl, - const cv::Mat& current_image, - const search_direction search_dir, - const search_method search_meth, - std::vector& image_points, - std::vector& line_widths - ) - { - cv::Mat processed_image; - // step 0: cut image to line - processed_image = current_image(cv::Rect(sl.iStart, - cv::Point(sl.iEnd.x+(search_dir==search_direction::y), - sl.iEnd.y+(search_dir==search_direction::x)) - )); - - // step 1: threshold image with mat: - if (search_meth == search_method::brightness) { - cv::threshold(processed_image, processed_image, config_.brightness_threshold, 255, CV_THRESH_BINARY); - // testing removal of bright white blobs -// std::vector > image_contours; -// cv::findContours(processed_image, image_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); -// processed_image = cv::Mat::zeros(current_image.rows, current_image.cols, CV_8UC1); -// cv::Mat debug_exclude_image = cv::Mat::zeros(current_image.rows, current_image.cols, CV_8UC1); -// for (int cnt = 0; cnt < image_contours.size(); ++cnt) { -// if (cv::contourArea(image_contours[cnt]) < config_.blob_area_threshold) -// cv::drawContours(processed_image, image_contours, cnt, cv::Scalar(255), CV_FILLED); -// else if (cv::contourArea(image_contours[cnt])/cv::boundingRect(image_contours[cnt]).area() > line_contours; - cv::findContours(processed_image, line_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); - -#ifdef DRAW_DEBUG - // line contours - cv::namedWindow("Line contours", CV_WINDOW_NORMAL); - cv::Mat line_contour_image = processed_image.clone(); - for (int i=0; i 6 && bounds.area() < 20) { - line_widths.push_back(bounds.br().x-bounds.tl().x); - temp_point = (bounds.tl()+bounds.br())/2; - if (search_dir == search_direction::x) { - temp_point.y = sl.iEnd.y; - } else { - temp_point.x = sl.iEnd.x; - } - image_points.push_back(temp_point); - } - } - } else if (search_meth == search_method::sobel) { - bool lowhigh_found = false; - int first = 0; - for(int i=0; i(i) > config_.sobelThreshold) { - lowhigh_found = true; - first = i; - } else if (lowhigh_found && processed_image.at(i) < -config_.sobelThreshold) { - line_widths.push_back(i-first); - if (search_dir == search_direction::x) { - image_points.push_back(cv::Point(sl.iStart.x+first+(i-first)/2,sl.iStart.y)); - } - else if (search_dir == search_direction::y) { - image_points.push_back(cv::Point(sl.iStart.x,sl.iStart.y+first+(i-first)/2)); - } - lowhigh_found = false; - } - } - } - - return; - } - - // debug to perform line check - void debugPointsImage (const cv::Mat& current_image, - const search_direction search_dir, - const search_method search_meth - ) { - std::vector image_points; - std::vector image_widths; -#ifdef DRAW_DEBUG - cv::Mat debug_image; - cv::cvtColor(current_image, debug_image, CV_GRAY2RGB); -#endif - - int search_steps = 6; - int pixel_step = 50; - SearchLine lines[search_steps]; - for (int it = 3; it < search_steps; ++it) { - if (search_dir == search_direction::x) { - lines[it].iStart = cv::Point(0,it*pixel_step); - lines[it].iEnd = cv::Point(current_image.cols, it*pixel_step); - } else if (search_dir == search_direction::y) { - lines[it].iStart = cv::Point(it*pixel_step, 0); - lines[it].iEnd = cv::Point(it*pixel_step, current_image.rows); - } -#ifdef DRAW_DEBUG - cv::line(debug_image, lines[it].iStart, lines[it].iEnd, cv::Scalar(255,255,255)); -#endif - findByLineSearch(lines[it], - current_image, - search_dir, - search_meth, - image_points, - image_widths - ); -#ifdef DRAW_DEBUG - for (auto point: image_points) { - cv::circle(debug_image,point,2,cv::Scalar(0,255,0),2); - } -#endif - } -#ifdef DRAW_DEBUG - if (search_meth == search_method::sobel) { - cv::namedWindow("Debug lineCheck sobel", CV_WINDOW_NORMAL); - cv::imshow("Debug lineCheck sobel", debug_image); - cv::waitKey(1); - } - if (search_meth == search_method::brightness) { - cv::namedWindow("Debug lineCheck brightness", CV_WINDOW_NORMAL); - cv::imshow("Debug lineCheck brightness", debug_image); - cv::waitKey(1); - } -#endif - return; } void setConfig(const LineDetectionConfig& config) { diff --git a/include/drive_ros_image_recognition/street_line.h b/include/drive_ros_image_recognition/line.h similarity index 75% rename from include/drive_ros_image_recognition/street_line.h rename to include/drive_ros_image_recognition/line.h index f131f4f..93066cd 100644 --- a/include/drive_ros_image_recognition/street_line.h +++ b/include/drive_ros_image_recognition/line.h @@ -1,5 +1,5 @@ -#ifndef STREET_LINE_H -#define STREET_LINE_H +#ifndef LINE_H +#define LINE_H /// /// \brief The StreetLine class @@ -7,13 +7,14 @@ /// The class saves the two points in image coordinates and world coordinates. /// The world coordinates are in meters, e.g. 0.5 is 50 cm. /// You are responsible that wP1_ is the correct world points for iP1_, same for wP2_ and iP2_. +/// Important: The x and y coordinates switch when converted from image to world or back. /// -class StreetLine { +class Line { public: cv::Point iP1_, iP2_, iMid_; cv::Point2f wP1_, wP2_, wMid_; - StreetLine(cv::Point iP1, cv::Point iP2, cv::Point2f wP1, cv::Point2f wP2) + Line(cv::Point iP1, cv::Point iP2, cv::Point2f wP1, cv::Point2f wP2) : iP1_(iP1) , iP2_(iP2) , wP1_(wP1) @@ -27,4 +28,4 @@ class StreetLine { inline double getAngle() { return abs(atan2(iP1_.y - iP2_.y, iP1_.x - iP2_.x)); } }; -#endif // STREET_LINE_H +#endif // LINE_H diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index 60c5cbd..f7b8053 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -12,7 +12,7 @@ #include #include #include "drive_ros_msgs/RoadLane.h" -#include "street_line.h" +#include "line.h" namespace drive_ros_image_recognition { @@ -31,7 +31,7 @@ class LineDetection { // variables CvImagePtr currentImage_; - std::vector currentLane_; + std::vector currentLane_; ImageOperator image_operator_; // communication diff --git a/include/drive_ros_image_recognition/warp_image.h b/include/drive_ros_image_recognition/warp_image.h index 09375bf..c752564 100644 --- a/include/drive_ros_image_recognition/warp_image.h +++ b/include/drive_ros_image_recognition/warp_image.h @@ -3,14 +3,13 @@ #include #include +#include #include #include #include #include #include -#include - namespace drive_ros_image_recognition { class WarpContent { @@ -23,30 +22,13 @@ class WarpContent { ros::NodeHandle pnh_; ros::NodeHandle nh_; cv::Mat current_image_; - // needs two components: camera model and homography for transformation - // camera model matrix and distortion coefficients - cv::Mat cam_mat_; - cv::Mat dist_coeffs_; + ImageOperator image_operator_; image_transport::ImageTransport it_; image_transport::CameraSubscriber cam_sub_; image_transport::Publisher img_pub_; - // debug undistort publisher image_transport::Publisher undistort_pub_; - ros::ServiceServer worldToImageServer_; - ros::ServiceServer imageToWorldServer_; // todo: moving to camera model subscription image_geometry::PinholeCameraModel cam_model_; - - // homography components - ros::Subscriber homography_params_sub_; - cv::Mat world2cam_; - cv::Mat cam2world_; - cv::Mat scaling_mat_; - cv::Mat scaling_mat_inv_; - cv::Mat scaledCam2world_; - cv::Mat scaledWorld2cam_; - cv::Size transformed_size_; - bool homo_received_; }; class WarpImageNodelet : public nodelet::Nodelet { diff --git a/launch/line_detection.launch b/launch/line_detection.launch index c6174f8..dfed4d8 100644 --- a/launch/line_detection.launch +++ b/launch/line_detection.launch @@ -14,7 +14,6 @@ - diff --git a/launch/road_detection.launch b/launch/road_detection.launch deleted file mode 100644 index 839f54b..0000000 --- a/launch/road_detection.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/road_detection.test b/launch/road_detection.test deleted file mode 100644 index c4b1250..0000000 --- a/launch/road_detection.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/launch/warp_image.launch b/launch/warp_image.launch index b1a8e8a..f8bacdf 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -1,8 +1,9 @@ + - + @@ -17,6 +18,7 @@ + diff --git a/src/line_detection.cpp b/src/line_detection.cpp index e40681b..ca11bf0 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -96,7 +96,7 @@ void LineDetection::findLane() { // Get houghlines std::vector hLinePoints; - std::vector filteredLines; + std::vector filteredLines; cv::HoughLinesP(processingImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); @@ -115,7 +115,7 @@ void LineDetection::findLane() { // Filter the lines for(size_t i = 0; i < worldPoints.size(); i += 2) { - StreetLine sl(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1)); + Line sl(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1)); filteredLines.push_back(sl); #ifdef DRAW_DEBUG cv::line(allLinesImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); @@ -132,7 +132,7 @@ void LineDetection::findLane() { worldPoints.push_back(cv::Point2f(0.70, 0.5)); if(image_operator_.worldToImage(worldPoints, imagePoints)) { for(size_t i = 0; i < imagePoints.size() - 1; i++) - currentLane_.push_back(StreetLine(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); + currentLane_.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); } else { // no guess, means no detection return; @@ -147,9 +147,9 @@ void LineDetection::findLane() { #endif // Use the guess to classify the lines - std::vector leftLine, middleLine, rightLine; - for(StreetLine sl : filteredLines) { - StreetLine segment = currentLane_.at(0); + std::vector leftLine, middleLine, rightLine; + for(Line sl : filteredLines) { + Line segment = currentLane_.at(0); bool isInSegment = false; // find the coresponding segment // todo: this only uses the x-coordinate right now, improve this @@ -197,7 +197,7 @@ void LineDetection::findLane() { //// currentLane_.push_back(l); // // sort the middle lines based on the front distance of the vehicle -//// std::sort(middleLine.begin(), middleLine.end(), [](const StreetLine& a, const StreetLine& b){ return a.wMid_.x < b.wMid_.x; }); +//// std::sort(middleLine.begin(), middleLine.end(), [](const Line& a, const Line& b){ return a.wMid_.x < b.wMid_.x; }); // std::vector guessImgPoints, guessWorldPoints; // for(auto lineIt = middleLine.begin(); lineIt != middleLine.end(); ++lineIt) { // guessWorldPoints.push_back(lineIt->wP1_); @@ -211,7 +211,7 @@ void LineDetection::findLane() { // if((guessWorldPoints.size() > 0) && (image_operator_.worldToImage(guessWorldPoints, guessImgPoints))) { // for(size_t i = 1; i < guessImgPoints.size(); i++) { -// currentLane_.push_back(StreetLine(guessImgPoints.at(i - 1), guessImgPoints.at(i), guessWorldPoints.at(i - 1), guessWorldPoints.at(i))); +// currentLane_.push_back(Line(guessImgPoints.at(i - 1), guessImgPoints.at(i), guessWorldPoints.at(i - 1), guessWorldPoints.at(i))); // } // } diff --git a/src/warp_image.cpp b/src/warp_image.cpp index 4251760..e9606c6 100644 --- a/src/warp_image.cpp +++ b/src/warp_image.cpp @@ -3,24 +3,14 @@ namespace drive_ros_image_recognition { -WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh): - nh_(nh), - pnh_(pnh), - current_image_(), - cam_mat_(3,3,CV_64F,cv::Scalar(0.0)), - dist_coeffs_(8,1,CV_64F,cv::Scalar(0.0)), - it_(nh), - cam_sub_(), - worldToImageServer_(), - imageToWorldServer_(), - cam_model_(), - homography_params_sub_(), - cam2world_(3,3,CV_64FC1,cv::Scalar(0.0)), - world2cam_(3,3,CV_64FC1,cv::Scalar(0.0)), - scaling_mat_(3,3,CV_64FC1,cv::Scalar(0.0)), - scaling_mat_inv_(3,3,CV_64FC1,cv::Scalar(0.0)), - transformed_size_(0,0), - homo_received_(false) +WarpContent::WarpContent(const ros::NodeHandle& nh, const ros::NodeHandle& pnh) + : nh_(nh) + , pnh_(pnh) + , current_image_() + , it_(nh) + , cam_sub_() + , cam_model_() + , image_operator_() { img_pub_ = it_.advertise("warped_out", 1); undistort_pub_ = it_.advertise("undistort_out", 1); @@ -30,69 +20,38 @@ WarpContent::~WarpContent() { } bool WarpContent::init() { - std::vector world_size; - double test; - ROS_INFO_STREAM("PNH namespace: "< image_size; - if(!pnh_.getParam("image_size", image_size)) { - ROS_ERROR("Unable to load parameter image_size!"); + // common image operations + if(!image_operator_.init()) { + ROS_WARN_STREAM("Failed to init image_operator"); return false; } - ROS_ASSERT(image_size.size() == 2); - transformed_size_ = cv::Size(image_size[0],image_size[1]); - scaling_mat_.at(0,0) = world_size[0]/image_size[0]; - scaling_mat_.at(1,1) = -world_size[1]/image_size[1]; - scaling_mat_.at(1,2) = world_size[1]/2; - scaling_mat_.at(2,2) = 1.0; - ROS_INFO_STREAM("Calculated world2mat scaling matrix: "<("homography_in", 1, - boost::bind(homography_callback, _1, - std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), - std::ref(scaledCam2world_), std::ref(scaledWorld2cam_), - std::ref(scaling_mat_inv_), std::ref(homo_received_))); // initialize combined subscriber for camera image and model cam_sub_ = it_.subscribeCamera("img_in", 10, &WarpContent::world_image_callback, this); return true; } -void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& info_msg) { - if (!homo_received_) { - ROS_WARN_THROTTLE(10, "Homography callback not yet received, waiting"); - return; - } +void WarpContent::world_image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) { - try - { + try { // copy current_image_ = cv_bridge::toCvCopy(msg, "")->image; - } - catch (cv_bridge::Exception& e) - { + } catch (cv_bridge::Exception& e) { ROS_WARN("Could not convert incoming image from '%s' to 'CV_16U', skipping.", msg->encoding.c_str()); return; } cam_model_.fromCameraInfo(info_msg); - // undistort and apply homography transformation + // undistort image and publish cv::Mat undistorted_mat; cv::undistort(current_image_, undistorted_mat, cam_model_.fullIntrinsicMatrix(), cam_model_.distortionCoeffs()); undistort_pub_.publish(cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_8UC1, undistorted_mat).toImageMsg()); -// cv::Mat topView2cam = world2cam_ * pixels_to_meters; - - // flag ensures that we directly use the matrix, as it is done in LMS + // apply homography cv::Mat output_mat; - cv::warpPerspective(undistorted_mat, output_mat, scaling_mat_, transformed_size_, cv::WARP_INVERSE_MAP); + image_operator_.homographImage(undistorted_mat, output_mat); + #ifdef DRAW_DEBUG cv::namedWindow("Homographied",CV_WINDOW_NORMAL); cv::imshow("Homographied",output_mat); From 5af78c8637fcd3fefd7a118d7e0943e84c7cdad0 Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 18 Nov 2017 11:38:24 +0100 Subject: [PATCH 78/94] fix launch file --- launch/line_detection.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/line_detection.launch b/launch/line_detection.launch index dfed4d8..f4a9190 100644 --- a/launch/line_detection.launch +++ b/launch/line_detection.launch @@ -2,7 +2,7 @@ - + @@ -10,7 +10,7 @@ - + From 67a40e0545125f377fa6690267a4ce3b93240271 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Tue, 5 Dec 2017 20:23:28 +0100 Subject: [PATCH 79/94] use knowledge about the environment to classify lines --- cfg/LineDetection.cfg | 6 +- .../common_image_operations.h | 6 + include/drive_ros_image_recognition/line.h | 35 ++- .../line_detection.h | 4 + launch/line_detection.launch | 2 +- src/line_detection.cpp | 241 ++++++++++++++---- 6 files changed, 231 insertions(+), 63 deletions(-) diff --git a/cfg/LineDetection.cfg b/cfg/LineDetection.cfg index 206bf8a..8e7b5a5 100755 --- a/cfg/LineDetection.cfg +++ b/cfg/LineDetection.cfg @@ -5,12 +5,14 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("lineWidth", double_t, 0, "Line width (world coordinates)", 0.4, 0.0, 1.0) +gen.add("lineWidth", double_t, 0, "Line width [m]", 0.45, 0.1, 1.0) +gen.add("lineVar", double_t, 0, "Line search corridor [m]", 0.1, 0.01, 0.20) +gen.add("lineAngle", double_t, 0, "Max line angle (rad)", 0.7, 0.1, 3.1) +gen.add("maxSenseRange", double_t, 0, "Max view range for lines", 1.6, 0.5, 5.0) gen.add("cannyThreshold", int_t, 0, "cannyThreshold", 30, 10, 500) gen.add("houghThreshold", int_t, 0, "houghThreshold", 20, 5, 100) gen.add("houghMinLineLen", int_t, 0, "houghMinLineLen", 20, 5, 100) gen.add("houghMaxLineGap", int_t, 0, "houghMaxLineGap", 10, 0, 300) -gen.add("sameLineThreshold", int_t, 0, "sameLineThreshold", 40, 1, 100) exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "LineDetection")) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 01e715e..2f32501 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -257,6 +257,9 @@ class TransformHelper { /// \return true on success, false on failure. /// bool imageToWorld(std::vector &imagePoints, std::vector &worldPoints) { + if(imagePoints.empty()) { + return false; + } if(!homography_received_) { ROS_WARN_STREAM("[imagetoWorld] Homography not received yet"); return false; @@ -275,6 +278,9 @@ class TransformHelper { /// \return true on success, false on failure. /// bool worldToImage(std::vector &worldPoints, std::vector &imagePoints) { + if(worldPoints.empty()) { + return false; + } if(!homography_received_) { ROS_WARN_STREAM("[worldToImage] Homography not received yet"); return false; diff --git a/include/drive_ros_image_recognition/line.h b/include/drive_ros_image_recognition/line.h index 93066cd..8cc6a20 100644 --- a/include/drive_ros_image_recognition/line.h +++ b/include/drive_ros_image_recognition/line.h @@ -6,6 +6,7 @@ /// Represents a line, defined by two points. /// The class saves the two points in image coordinates and world coordinates. /// The world coordinates are in meters, e.g. 0.5 is 50 cm. +/// wP1 is always closer to the vehicle than wP2. /// You are responsible that wP1_ is the correct world points for iP1_, same for wP2_ and iP2_. /// Important: The x and y coordinates switch when converted from image to world or back. /// @@ -15,17 +16,41 @@ class Line { cv::Point2f wP1_, wP2_, wMid_; Line(cv::Point iP1, cv::Point iP2, cv::Point2f wP1, cv::Point2f wP2) - : iP1_(iP1) - , iP2_(iP2) - , wP1_(wP1) - , wP2_(wP2) +// : iP1_(iP1) +// , iP2_(iP2) +// , wP1_(wP1) +// , wP2_(wP2) { + if(wP1.x < wP2.x) { + iP1_ = iP1; + iP2_ = iP2; + wP1_ = wP1; + wP2_ = wP2; + } else { + iP1_ = iP2; + iP2_ = iP1; + wP1_ = wP2; + wP2_ = wP1; + } + iMid_ = (iP1_ + iP2_) * .5; wMid_ = (wP1_ + wP2_) * .5; } // returns the angle of the lines between 0 and PI - inline double getAngle() { return abs(atan2(iP1_.y - iP2_.y, iP1_.x - iP2_.x)); } + inline double getAngle() { return std::abs(atan2(wP1_.x - wP2_.x, wP1_.y - wP2_.y)); } + + float getLength() { + auto xDir = wP2_.x - wP1_.x; + auto yDir = wP2_.y - wP1_.y; + return sqrt(xDir * xDir + yDir * yDir); + } }; +inline float getDistance(const cv::Point2f a, const cv::Point2f b) { + auto dX = a.x - b.x; + auto dY = a.y - b.y; + return sqrt(dX * dX + dY * dY); +} + #endif // LINE_H diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index f7b8053..d57b2d9 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -24,6 +24,9 @@ class LineDetection { // configs float lineWidth_; + float lineAngle_; + float lineVar_; + float maxViewRange_; int cannyThreshold_; int houghThresold_; int houghMinLineLen_; @@ -39,6 +42,7 @@ class LineDetection { image_transport::Subscriber imageSubscriber_; ros::Publisher line_output_pub_; // note: not used yet ros::Subscriber homography_params_sub_; + image_transport::Publisher debugImgPub_; // homography components cv::Mat world2cam_; diff --git a/launch/line_detection.launch b/launch/line_detection.launch index dfed4d8..88ddbaa 100644 --- a/launch/line_detection.launch +++ b/launch/line_detection.launch @@ -10,7 +10,7 @@ - + diff --git a/src/line_detection.cpp b/src/line_detection.cpp index ca11bf0..33f287b 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -2,8 +2,9 @@ #include #include #include -#include "drive_ros_image_recognition/line_detection.h" #include +#include "drive_ros_image_recognition/line_detection.h" +#include "drive_ros_image_recognition/geometry_common.h" namespace drive_ros_image_recognition { @@ -35,6 +36,8 @@ bool LineDetection::init() { imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &LineDetection::imageCallback, this); ROS_INFO_STREAM("Subscribed image transport to topic " << imageSubscriber_.getTopic()); + debugImgPub_ = imageTransport_.advertise("line_detection/debug_image", 10); + // common image operations if(!image_operator_.init()) { ROS_WARN_STREAM("Failed to init image_operator"); @@ -118,7 +121,7 @@ void LineDetection::findLane() { Line sl(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1)); filteredLines.push_back(sl); #ifdef DRAW_DEBUG - cv::line(allLinesImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); +// cv::line(allLinesImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); #endif } @@ -126,6 +129,7 @@ void LineDetection::findLane() { worldPoints.clear(); imagePoints.clear(); if(currentLane_.empty()) { + ROS_INFO_STREAM("dummy guess"); worldPoints.push_back(cv::Point2f(0.24, 0.5)); worldPoints.push_back(cv::Point2f(0.4, 0.5)); worldPoints.push_back(cv::Point2f(0.55, 0.5)); @@ -141,9 +145,10 @@ void LineDetection::findLane() { #ifdef DRAW_DEBUG // Draw our "guess" (previous middle line) - for(size_t i = 0; i < currentLane_.size(); i++) { - cv::line(outputImg, currentLane_.at(i).iP1_, currentLane_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); - } +// for(size_t i = 0; i < currentLane_.size(); i++) { +// cv::line(outputImg, currentLane_.at(i).iP1_, currentLane_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); +// } +// todo: we could draw the search corridor here #endif // Use the guess to classify the lines @@ -151,7 +156,7 @@ void LineDetection::findLane() { for(Line sl : filteredLines) { Line segment = currentLane_.at(0); bool isInSegment = false; - // find the coresponding segment + // find the corresponding segment // todo: this only uses the x-coordinate right now, improve this for(size_t i = 0; (i < currentLane_.size()) && !isInSegment; i++) { segment = currentLane_.at(i); @@ -166,24 +171,28 @@ void LineDetection::findLane() { if(isInSegment) { // classify the line - // lane width is 0.35 to 0.45 [m] - auto distanceToMidLine = sl.wMid_.y - segment.wMid_.y; - if(std::abs(distanceToMidLine) < 0.175) { - middleLine.push_back(sl); + if(std::abs(sl.getAngle() - segment.getAngle()) < lineAngle_) { + // lane width is 0.35 to 0.45 [m] + // todo: distance should be calculated based on orthogonal distance. currently this leads to problems in curves + auto absDistanceToMidLine = std::abs(sl.wMid_.y - segment.wMid_.y); + if(absDistanceToMidLine < (lineWidth_ / 2)) { + middleLine.push_back(sl); #ifdef DRAW_DEBUG - cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); + cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); #endif - } else if((sl.wMid_.y < segment.wMid_.y) && (std::abs(distanceToMidLine) < .6)) { - rightLine.push_back(sl); + } else if((sl.wMid_.y < segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { + rightLine.push_back(sl); #ifdef DRAW_DEBUG - cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); + cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); #endif - } else if(distanceToMidLine < .6) { - leftLine.push_back(sl); + } else if((sl.wMid_.y > segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { + leftLine.push_back(sl); #ifdef DRAW_DEBUG - cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); + cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); #endif + } } else { + // todo: here we still have to check the distance from our lane #ifdef DRAW_DEBUG cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255, 255), 2, cv::LINE_AA); #endif @@ -191,48 +200,167 @@ void LineDetection::findLane() { } } - // Use the new middle line as our new guess -// currentLane_.clear(); -//// for(auto l : middleLine) -//// currentLane_.push_back(l); - -// // sort the middle lines based on the front distance of the vehicle -//// std::sort(middleLine.begin(), middleLine.end(), [](const Line& a, const Line& b){ return a.wMid_.x < b.wMid_.x; }); -// std::vector guessImgPoints, guessWorldPoints; -// for(auto lineIt = middleLine.begin(); lineIt != middleLine.end(); ++lineIt) { -// guessWorldPoints.push_back(lineIt->wP1_); -// guessWorldPoints.push_back(lineIt->wP2_); -// } -// std::sort(guessWorldPoints.begin(), guessWorldPoints.end(), [](const cv::Point2f& a, const cv::Point2f& b){ return a.x < b.x; }); -// // now add another lane as the last one to extend our current lane -// cv::Point lastLineDirection = guessWorldPoints.at(guessWorldPoints.size() - 1) - guessWorldPoints.at(guessWorldPoints.size() - 2); -// auto normFactor = 1 / cv::norm(lastLineDirection); -// ROS_INFO_STREAM("normFactor=" << normFactor); + if(middleLine.empty()) { + ROS_INFO_STREAM("no middle line found"); + currentLane_.clear(); + debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, outputImg).toImageMsg()); + return; + } -// if((guessWorldPoints.size() > 0) && (image_operator_.worldToImage(guessWorldPoints, guessImgPoints))) { -// for(size_t i = 1; i < guessImgPoints.size(); i++) { -// currentLane_.push_back(Line(guessImgPoints.at(i - 1), guessImgPoints.at(i), guessWorldPoints.at(i - 1), guessWorldPoints.at(i))); -// } + // middle line is 0.2m long and then interrupted for 0.2m + // sort our middle lines based on wP1_.x of line + std::sort(middleLine.begin(), middleLine.end(), [](Line &a, Line &b){ return a.wP1_.x < b.wP1_.x; }); + // build bounding boxes around group of lines, where lines in group are closer than 0.3m to each other + float currentMinX = middleLine.at(0).wP1_.x; + std::vector cPts, newMidLinePts; + for(auto l : middleLine) { + if(l.wP1_.x > (currentMinX + 0.3)) { + auto rect = cv::minAreaRect(cPts); + cv::Point2f a, b; + cv::Point2f vertices2f[4]; + rect.points(vertices2f); -// } - + currentMinX = l.wP1_.x; + + // todo: this should be in a function, since we are using it again after the for loop + // use the long side as line + if(std::abs(vertices2f[0].x - vertices2f[1].x) > std::abs(vertices2f[1].x - vertices2f[2].x)) { + a = vertices2f[0]; + b = vertices2f[1]; + } else { + a = vertices2f[1]; + b = vertices2f[2]; + } + // ensures the right order of the points in newMidLinePts vector + if(a.x > b.x) { + newMidLinePts.push_back(b); + newMidLinePts.push_back(a); + } else { + newMidLinePts.push_back(a); + newMidLinePts.push_back(b); + } - // angle classification -// if((a > (CV_PI/6)) && (a < 2.6)) { -// // lane marking -// cv::line(outputImg, l.iP1_, l.iP2_, cv::Scalar(0,255), 2, cv::LINE_AA); -// } else { -// // stop or start line -// cv::line(outputImg, l.iP1_, l.iP2_, cv::Scalar(255), 2, cv::LINE_AA); -// } + cPts.clear(); + } + cPts.push_back(l.wP1_); + cPts.push_back(l.wP2_); + } -#ifdef DRAW_DEBUG - cv::namedWindow("All Lines", CV_WINDOW_NORMAL); - cv::imshow("All Lines", allLinesImg); - cv::namedWindow("Debug Image", CV_WINDOW_NORMAL); - cv::imshow("Debug Image", outputImg); - cv::waitKey(1); -#endif + // finish the bounding box building step + if(!cPts.empty()) { + auto rect = cv::minAreaRect(cPts); + cv::Point2f vertices2f[4]; + rect.points(vertices2f); + cv::Point2f a, b; + + // use the long side as line + if(std::abs(vertices2f[0].x - vertices2f[1].x) > std::abs(vertices2f[1].x - vertices2f[2].x)) { + a = vertices2f[0]; + b = vertices2f[1]; + } else { + a = vertices2f[1]; + b = vertices2f[2]; + } + // ensures the right order of the points in newMidLinePts vector + if(a.x > b.x) { + newMidLinePts.push_back(b); + newMidLinePts.push_back(a); + } else { + newMidLinePts.push_back(a); + newMidLinePts.push_back(b); + } + } + + // build the middle line from out points + // extend the last line to the front + auto wPt1 = newMidLinePts.at(newMidLinePts.size() - 2); + auto wPt2 = newMidLinePts.at(newMidLinePts.size() - 1); + auto dir = wPt2 - wPt1; + auto dirLen = sqrt(dir.x * dir.x + dir.y * dir.y); + + auto newPt = wPt2 + (dir * (0.6 / dirLen)); + if(newPt.x < maxViewRange_) + newMidLinePts.push_back(newPt); + + // extend first line to the back + wPt1 = newMidLinePts.at(0); + wPt2 = newMidLinePts.at(1); + dir = wPt2 - wPt1; + dirLen = sqrt(dir.x * dir.x + dir.y * dir.y); + auto distToImageBoundary = wPt1.x - 0.24; // 0.24 is known. todo: magic numbers are not nice, put it in config + if(distToImageBoundary > 0.1) { // distance is positive + // dir, distToImageBoundary, dirLen are all positive + newPt = wPt1 - (dir * distToImageBoundary / dirLen); + newMidLinePts.push_back(newPt); + } + + // sort the world points based on the distance to the car + std::sort(newMidLinePts.begin(), newMidLinePts.end(), [](cv::Point2f &a, cv::Point2f &b) { return a.x < b.x; }); + + // convert points + imagePoints.clear(); + image_operator_.worldToImage(newMidLinePts, imagePoints); + // create new guess + currentLane_.clear(); + for(size_t i = 1; i < imagePoints.size(); i++) { + auto a = imagePoints.at(i - 1); + auto b = imagePoints.at(i); + // this makes the line coordinates inconsistant, but this should not be a serious problem + if(a.y > imgHeight) + a.y = imgHeight - 1; + if(b.y > imgHeight) + b.y = imgHeight - 1; + + currentLane_.push_back(Line(a, b, newMidLinePts.at(i - 1), newMidLinePts.at(i))); + } + + // validate our new guess + if(!currentLane_.empty()) { + if(currentLane_.at(currentLane_.size() - 1).wP2_.x < 0.5) { + // our guess is too short, it is better to use the default one + currentLane_.clear(); + } else if((currentLane_.at(0).getAngle() < 1.0) || (currentLane_.at(0).getAngle() > 2.6)) { + // the angle of the first segment is weird (probably a workaround for now, why can this happen?) + // if we have more than one segment, then we just delete the first + if(currentLane_.size() > 1) { + currentLane_.erase(currentLane_.begin()); + } else { + currentLane_.clear(); + } + } + + // angles between two connected segments should be plausible + bool done = false; + for(size_t i = 1; i < currentLane_.size() && !done; i++) { + if(std::abs(currentLane_.at(i - 1).getAngle() - currentLane_.at(i).getAngle()) > 0.8) { + // 0.8 = 45 degree + // todo: clear whole line or just to this segment? + currentLane_.erase(currentLane_.begin() + i, currentLane_.end()); + ROS_INFO_STREAM("clipped line because of angle difference"); + done = true; + } else if(currentLane_.at(i).wP2_.x > maxViewRange_) { + ROS_INFO_STREAM("clipped line because of maxViewRange_"); + currentLane_.erase(currentLane_.begin() + i, currentLane_.end()); + done = true; + } else if(currentLane_.at(i).getLength() > 0.7) { + ROS_INFO_STREAM("clipped line because of length"); + currentLane_.erase(currentLane_.begin() + i, currentLane_.end()); + done = true; + } + } + // draw the new (validated) guess + for(auto l : currentLane_) { + cv::line(outputImg, l.iP1_, l.iP2_, cv::Scalar(255,0,255), 2, cv::LINE_AA); + cv::circle(outputImg, l.iP1_, 5, cv::Scalar(255,0,255), 2); + cv::circle(outputImg, l.iP2_, 5, cv::Scalar(255,0,255), 2); + } + } + + // todo: move old guess with odometry and compare old and new guess + + debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, outputImg).toImageMsg()); + + // todo: publish points to road topic } /// @@ -244,6 +372,9 @@ void LineDetection::findLane() { void LineDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level){ image_operator_.setConfig(config); lineWidth_ = config.lineWidth; + lineAngle_ = config.lineAngle; + lineVar_ = config.lineVar; + maxViewRange_ = config.maxSenseRange; cannyThreshold_ = config.cannyThreshold; houghThresold_ = config.houghThreshold; houghMinLineLen_ = config.houghMinLineLen; From fe6d5b10a7904a372a900c1527e18e21f4b5217f Mon Sep 17 00:00:00 2001 From: FaHa Date: Sat, 18 Nov 2017 11:38:24 +0100 Subject: [PATCH 80/94] fix launch file --- launch/line_detection.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/line_detection.launch b/launch/line_detection.launch index 88ddbaa..f4a9190 100644 --- a/launch/line_detection.launch +++ b/launch/line_detection.launch @@ -2,7 +2,7 @@ - + From 40fcb27b308a7d609828c8698bc75aa6abeb9b9a Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Sat, 16 Dec 2017 16:28:26 +0100 Subject: [PATCH 81/94] publish middle line points to topic. improve line detection --- CMakeLists.txt | 2 +- include/drive_ros_image_recognition/line.h | 14 +- .../line_detection.h | 16 +- launch/line_detection.launch | 2 +- src/line_detection.cpp | 207 ++++++++++++------ 5 files changed, 156 insertions(+), 85 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d4806d..04b3cdf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(drive_ros_image_recognition) SET(CMAKE_BUILD_TYPE Debug) # enable to compile with OpenCV window displays -add_definitions(-DDRAW_DEBUG) +#add_definitions(-DDRAW_DEBUG) # enable to compile with publishers of internally processed images add_definitions(-DPUBLISH_DEBUG) # enable to use World2Cam homography diff --git a/include/drive_ros_image_recognition/line.h b/include/drive_ros_image_recognition/line.h index 8cc6a20..42a885a 100644 --- a/include/drive_ros_image_recognition/line.h +++ b/include/drive_ros_image_recognition/line.h @@ -1,6 +1,8 @@ #ifndef LINE_H #define LINE_H +namespace drive_ros_image_recognition { + /// /// \brief The StreetLine class /// Represents a line, defined by two points. @@ -16,10 +18,6 @@ class Line { cv::Point2f wP1_, wP2_, wMid_; Line(cv::Point iP1, cv::Point iP2, cv::Point2f wP1, cv::Point2f wP2) -// : iP1_(iP1) -// , iP2_(iP2) -// , wP1_(wP1) -// , wP2_(wP2) { if(wP1.x < wP2.x) { iP1_ = iP1; @@ -41,16 +39,12 @@ class Line { inline double getAngle() { return std::abs(atan2(wP1_.x - wP2_.x, wP1_.y - wP2_.y)); } float getLength() { +// return getDistanceBetweenPoints(wP2_, wP1_); auto xDir = wP2_.x - wP1_.x; auto yDir = wP2_.y - wP1_.y; return sqrt(xDir * xDir + yDir * yDir); } }; - -inline float getDistance(const cv::Point2f a, const cv::Point2f b) { - auto dX = a.x - b.x; - auto dY = a.y - b.y; - return sqrt(dX * dX + dY * dY); -} +} // namespace drive_ros_image_recognition #endif // LINE_H diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index d57b2d9..2d35a83 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -11,6 +11,7 @@ #include #include #include +#include #include "drive_ros_msgs/RoadLane.h" #include "line.h" @@ -34,7 +35,8 @@ class LineDetection { // variables CvImagePtr currentImage_; - std::vector currentLane_; + std::vector currentGuess_; + std::vector currentMiddleLine_; ImageOperator image_operator_; // communication @@ -42,7 +44,9 @@ class LineDetection { image_transport::Subscriber imageSubscriber_; ros::Publisher line_output_pub_; // note: not used yet ros::Subscriber homography_params_sub_; +#ifdef PUBLISH_DEBUG image_transport::Publisher debugImgPub_; +#endif // homography components cv::Mat world2cam_; @@ -52,6 +56,10 @@ class LineDetection { cv::Size transformed_size_; bool homog_received_; + // odometry components + tf::TransformListener tfListener_; + cv::Point2f oldPoint_; + // callbacks void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); @@ -61,6 +69,12 @@ class LineDetection { // methods void findLane(); + float getDistanceBetweenPoints(const cv::Point2f a, const cv::Point2f b) { + auto dX = a.x - b.x; + auto dY = a.y - b.y; + return sqrt(dX * dX + dY * dY); + } + public: LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); ~LineDetection(); diff --git a/launch/line_detection.launch b/launch/line_detection.launch index f4a9190..88ddbaa 100644 --- a/launch/line_detection.launch +++ b/launch/line_detection.launch @@ -2,7 +2,7 @@ - + diff --git a/src/line_detection.cpp b/src/line_detection.cpp index 33f287b..9196fa6 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -5,6 +5,7 @@ #include #include "drive_ros_image_recognition/line_detection.h" #include "drive_ros_image_recognition/geometry_common.h" +#include "drive_ros_msgs/RoadLine.h" namespace drive_ros_image_recognition { @@ -13,7 +14,6 @@ LineDetection::LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh , pnh_(pnh) , imageTransport_(pnh) , lineWidth_(0.4) - , line_output_pub_() , image_operator_() , dsrv_server_() , dsrv_cb_(boost::bind(&LineDetection::reconfigureCB, this, _1, _2)) @@ -29,6 +29,7 @@ LineDetection::~LineDetection() { /// \return true for success, false for failure. /// bool LineDetection::init() { + // todo: topics as params // dynamic reconfigure dsrv_server_.setCallback(dsrv_cb_); @@ -36,7 +37,13 @@ bool LineDetection::init() { imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &LineDetection::imageCallback, this); ROS_INFO_STREAM("Subscribed image transport to topic " << imageSubscriber_.getTopic()); - debugImgPub_ = imageTransport_.advertise("line_detection/debug_image", 10); + line_output_pub_ = nh_.advertise("roadLine", 10); + ROS_INFO_STREAM("Advertising road line on " << line_output_pub_.getTopic()); + +#ifdef PUBLISH_DEBUG + debugImgPub_ = imageTransport_.advertise("debug_image", 10); + ROS_INFO_STREAM("publishing debug image on topic " << debugImgPub_.getTopic()); +#endif // common image operations if(!image_operator_.init()) { @@ -67,10 +74,9 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { /// void LineDetection::findLane() { cv::Mat processingImg; -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG cv::Mat outputImg; cv::cvtColor(*currentImage_, outputImg, CV_GRAY2RGB); - cv::Mat allLinesImg = outputImg.clone(); #endif // Blur the image and find edges with Canny @@ -89,7 +95,7 @@ void LineDetection::findLane() { int polygonLengths[] = { 4 }; cv::fillPoly(processingImg, polygonStarts, polygonLengths, 1, cv::Scalar(), cv::LINE_8); -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG // Display the polygon which will be zeroed out // cv::line(outputImg, vehiclePolygon[0][0], vehiclePolygon[0][1], cv::Scalar(255), 2, cv::LINE_AA); // cv::line(outputImg, vehiclePolygon[0][1], vehiclePolygon[0][2], cv::Scalar(255), 2, cv::LINE_AA); @@ -99,67 +105,55 @@ void LineDetection::findLane() { // Get houghlines std::vector hLinePoints; - std::vector filteredLines; + std::vector houghLines; cv::HoughLinesP(processingImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); - // Extract points for houghLines and convert to worldCoordinates + // Extract points for houghLines and convert to world-coordinates std::vector imagePoints, worldPoints; for(size_t i = 0; i < hLinePoints.size(); i++) { cv::Vec4i currentPoints = hLinePoints.at(i); imagePoints.push_back(cv::Point(currentPoints[0], currentPoints[1])); imagePoints.push_back(cv::Point(currentPoints[2], currentPoints[3])); } + image_operator_.imageToWorld(imagePoints, worldPoints); - if(!image_operator_.imageToWorld(imagePoints, worldPoints)) { - // failed converting points - return; - } - - // Filter the lines + // Build lines from points for(size_t i = 0; i < worldPoints.size(); i += 2) { - Line sl(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1)); - filteredLines.push_back(sl); -#ifdef DRAW_DEBUG -// cv::line(allLinesImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); -#endif + houghLines.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); } - // Create a dummy middle line, in case we do not have on from the previous frame + // Create a static guess, in case we do not have on from the previous frame + // todo: we should only do, when the car starts in the start box. Otherwise we can get in trouble worldPoints.clear(); imagePoints.clear(); - if(currentLane_.empty()) { + if(currentGuess_.empty()) { ROS_INFO_STREAM("dummy guess"); worldPoints.push_back(cv::Point2f(0.24, 0.5)); worldPoints.push_back(cv::Point2f(0.4, 0.5)); worldPoints.push_back(cv::Point2f(0.55, 0.5)); worldPoints.push_back(cv::Point2f(0.70, 0.5)); - if(image_operator_.worldToImage(worldPoints, imagePoints)) { - for(size_t i = 0; i < imagePoints.size() - 1; i++) - currentLane_.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); - } else { - // no guess, means no detection - return; - } + image_operator_.worldToImage(worldPoints, imagePoints); + for(size_t i = 1; i < imagePoints.size(); i++) + currentGuess_.push_back(Line(imagePoints.at(i - 1), imagePoints.at(i), worldPoints.at(i - 1), worldPoints.at(i))); } -#ifdef DRAW_DEBUG - // Draw our "guess" (previous middle line) -// for(size_t i = 0; i < currentLane_.size(); i++) { -// cv::line(outputImg, currentLane_.at(i).iP1_, currentLane_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); -// } -// todo: we could draw the search corridor here +#ifdef PUBLISH_DEBUG + // Draw our guess + for(size_t i = 0; i < currentGuess_.size(); i++) { + cv::line(outputImg, currentGuess_.at(i).iP1_, currentGuess_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); + } #endif // Use the guess to classify the lines std::vector leftLine, middleLine, rightLine; - for(Line sl : filteredLines) { - Line segment = currentLane_.at(0); + for(Line sl : houghLines) { + Line segment = currentGuess_.at(0); bool isInSegment = false; // find the corresponding segment // todo: this only uses the x-coordinate right now, improve this - for(size_t i = 0; (i < currentLane_.size()) && !isInSegment; i++) { - segment = currentLane_.at(i); + for(size_t i = 0; (i < currentGuess_.size()) && !isInSegment; i++) { + segment = currentGuess_.at(i); if(sl.wMid_.x > segment.wP1_.x) { if(sl.wMid_.x < segment.wP2_.x) { isInSegment = true; @@ -173,27 +167,28 @@ void LineDetection::findLane() { // classify the line if(std::abs(sl.getAngle() - segment.getAngle()) < lineAngle_) { // lane width is 0.35 to 0.45 [m] - // todo: distance should be calculated based on orthogonal distance. currently this leads to problems in curves + // todo: distance should be calculated based on orthogonal distance. + // the current approach leads to problems in curves auto absDistanceToMidLine = std::abs(sl.wMid_.y - segment.wMid_.y); if(absDistanceToMidLine < (lineWidth_ / 2)) { middleLine.push_back(sl); -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); #endif } else if((sl.wMid_.y < segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { rightLine.push_back(sl); -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); #endif } else if((sl.wMid_.y > segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { leftLine.push_back(sl); -#ifdef DRAW_DEBUG +#ifdef PUBLISH_DEBUG cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); #endif } } else { - // todo: here we still have to check the distance from our lane -#ifdef DRAW_DEBUG + // todo: classify the line (stop, start, ...) +#ifdef PUBLISH_DEBUG cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255, 255), 2, cv::LINE_AA); #endif } @@ -201,20 +196,63 @@ void LineDetection::findLane() { } if(middleLine.empty()) { + // todo: handle this better ROS_INFO_STREAM("no middle line found"); - currentLane_.clear(); + currentGuess_.clear(); +#ifdef PUBLISH_DEBUG debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, outputImg).toImageMsg()); +#endif return; } + // transform old line based on odometry +// tf::StampedTransform transform; +// try{ +// tfListener_.lookupTransform("/odometry", "/rear_axis_middle_ground", ros::Time(0), transform); +// } +// catch (tf::TransformException ex){ +// ROS_ERROR("%s",ex.what()); +// ros::Duration(1.0).sleep(); +// } +// cv::Point2f newPoint(transform.getOrigin().x(), transform.getOrigin().y()); +// // auto xTransform = newPoint.x - oldPoint_.x; +// // auto yTransform = newPoint.y - oldPoint_y; +// auto ptsDifference = newPoint - oldPoint_; +// ROS_INFO_STREAM(ptsDifference.x << "," << ptsDifference.y); +// if(ptsDifference.x != 0.0){ +// oldPoint_ = newPoint; +// std::vector oldTransformedLine; +// for(auto seg : currentLane_) { +// oldTransformedLine.push_back(cv::Point2f(seg.wP1_ + ptsDifference)); +// oldTransformedLine.push_back(cv::Point2f(seg.wP2_ + ptsDifference)); +// } + + +// imagePoints.clear(); +// image_operator_.worldToImage(oldTransformedLine, imagePoints); +// for(auto i = 1; i < (imagePoints.size() - 1); i++) +// cv::line(outputImg, imagePoints.at(i - 1), imagePoints.at(i), cv::Scalar(255,255), 2); +// for(auto i = 0; i < currentLane_.size(); i++) { +// auto p1 = imagePoints.at(i * 2); +// auto p2 = imagePoints.at((i * 2) + 1); +// cv::circle(outputImg, currentLane_.at(i).iP1_, 5, cv::Scalar(124,124), 3); +// cv::circle(outputImg, p1, 5, cv::Scalar(255,255), 3); +// cv::line(outputImg, currentLane_.at(i).iP1_, p1, cv::Scalar(124,124), 2); +// cv::circle(outputImg, currentLane_.at(i).iP2_, 5, cv::Scalar(124,124), 3); +// cv::circle(outputImg, p2, 5, cv::Scalar(255,255), 3); +// cv::line(outputImg, currentLane_.at(i).iP2_, p2, cv::Scalar(124,124), 2); +// } +// } + // middle line is 0.2m long and then interrupted for 0.2m + // todo: this should be checked // sort our middle lines based on wP1_.x of line std::sort(middleLine.begin(), middleLine.end(), [](Line &a, Line &b){ return a.wP1_.x < b.wP1_.x; }); - // build bounding boxes around group of lines, where lines in group are closer than 0.3m to each other + // build bounding boxes around group of lines, where lines in group are closer than 0.25m to each other float currentMinX = middleLine.at(0).wP1_.x; std::vector cPts, newMidLinePts; for(auto l : middleLine) { - if(l.wP1_.x > (currentMinX + 0.3)) { + if(l.wP1_.x > (currentMinX + 0.25)) { auto rect = cv::minAreaRect(cPts); cv::Point2f a, b; cv::Point2f vertices2f[4]; @@ -287,7 +325,8 @@ void LineDetection::findLane() { wPt2 = newMidLinePts.at(1); dir = wPt2 - wPt1; dirLen = sqrt(dir.x * dir.x + dir.y * dir.y); - auto distToImageBoundary = wPt1.x - 0.24; // 0.24 is known. todo: magic numbers are not nice, put it in config + // the image starts 0.24m from the rear_axis_middle_ground. todo: sould be in config + auto distToImageBoundary = wPt1.x - 0.24; if(distToImageBoundary > 0.1) { // distance is positive // dir, distToImageBoundary, dirLen are all positive newPt = wPt1 - (dir * distToImageBoundary / dirLen); @@ -300,8 +339,8 @@ void LineDetection::findLane() { // convert points imagePoints.clear(); image_operator_.worldToImage(newMidLinePts, imagePoints); - // create new guess - currentLane_.clear(); + // create new guess and middle line + currentGuess_.clear(); for(size_t i = 1; i < imagePoints.size(); i++) { auto a = imagePoints.at(i - 1); auto b = imagePoints.at(i); @@ -311,56 +350,80 @@ void LineDetection::findLane() { if(b.y > imgHeight) b.y = imgHeight - 1; - currentLane_.push_back(Line(a, b, newMidLinePts.at(i - 1), newMidLinePts.at(i))); + currentGuess_.push_back(Line(a, b, newMidLinePts.at(i - 1), newMidLinePts.at(i))); } // validate our new guess - if(!currentLane_.empty()) { - if(currentLane_.at(currentLane_.size() - 1).wP2_.x < 0.5) { + if(!currentGuess_.empty()) { + if(currentGuess_.at(currentGuess_.size() - 1).wP2_.x < 0.5) { // our guess is too short, it is better to use the default one - currentLane_.clear(); - } else if((currentLane_.at(0).getAngle() < 1.0) || (currentLane_.at(0).getAngle() > 2.6)) { + currentGuess_.clear(); + } else if((currentGuess_.at(0).getAngle() < 1.0) || (currentGuess_.at(0).getAngle() > 2.6)) { // the angle of the first segment is weird (probably a workaround for now, why can this happen?) // if we have more than one segment, then we just delete the first - if(currentLane_.size() > 1) { - currentLane_.erase(currentLane_.begin()); + if(currentGuess_.size() > 1) { + currentGuess_.erase(currentGuess_.begin()); } else { - currentLane_.clear(); + // otherwise throw the guess away + currentGuess_.clear(); } } - // angles between two connected segments should be plausible + bool done = false; - for(size_t i = 1; i < currentLane_.size() && !done; i++) { - if(std::abs(currentLane_.at(i - 1).getAngle() - currentLane_.at(i).getAngle()) > 0.8) { + for(size_t i = 1; i < currentGuess_.size() && !done; i++) { + // angles between two connected segments should be plausible + if(std::abs(currentGuess_.at(i - 1).getAngle() - currentGuess_.at(i).getAngle()) > 0.8) { // 0.8 = 45 degree // todo: clear whole line or just to this segment? - currentLane_.erase(currentLane_.begin() + i, currentLane_.end()); - ROS_INFO_STREAM("clipped line because of angle difference"); + currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); +// ROS_INFO_STREAM("clipped line because of angle difference"); done = true; - } else if(currentLane_.at(i).wP2_.x > maxViewRange_) { - ROS_INFO_STREAM("clipped line because of maxViewRange_"); - currentLane_.erase(currentLane_.begin() + i, currentLane_.end()); + } else if(currentGuess_.at(i).wP2_.x > maxViewRange_) { +// ROS_INFO_STREAM("clipped line because of maxViewRange_"); + currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); done = true; - } else if(currentLane_.at(i).getLength() > 0.7) { + } else if((i < (currentGuess_.size() - 1)) && currentGuess_.at(i).getLength() > 0.3) { + // if a segment is longer than 0.3m, the line is probably not dashed (so no middle line) + // excluding the last segment, since we created this for searching forward ROS_INFO_STREAM("clipped line because of length"); - currentLane_.erase(currentLane_.begin() + i, currentLane_.end()); + currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); done = true; } } - // draw the new (validated) guess - for(auto l : currentLane_) { + + currentMiddleLine_.clear(); + currentMiddleLine_.insert(currentMiddleLine_.begin(), currentGuess_.begin(), currentGuess_.end() - (done ? 0 : 1)); + +#ifdef PUBLISH_DEBUG + // draw our middle line + for(auto l : currentMiddleLine_) { cv::line(outputImg, l.iP1_, l.iP2_, cv::Scalar(255,0,255), 2, cv::LINE_AA); - cv::circle(outputImg, l.iP1_, 5, cv::Scalar(255,0,255), 2); - cv::circle(outputImg, l.iP2_, 5, cv::Scalar(255,0,255), 2); } +#endif } // todo: move old guess with odometry and compare old and new guess +#ifdef PUBLISH_DEBUG debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, outputImg).toImageMsg()); +#endif + + // publish points to road topic + drive_ros_msgs::RoadLine msgMidLine; + msgMidLine.lineType = drive_ros_msgs::RoadLine::MIDDLE; + for(auto l : currentMiddleLine_) { + geometry_msgs::PointStamped pt1, pt2; + pt1.point.x = l.wP1_.x; + pt1.point.y = l.wP1_.y; + msgMidLine.points.push_back(pt1); + pt2.point.x = l.wP2_.x; + pt2.point.y = l.wP2_.y; + msgMidLine.points.push_back(pt2); + } + + line_output_pub_.publish(msgMidLine); - // todo: publish points to road topic } /// From 250b4cffdb3a5aaa4c152b2590ad8c9a4f160ec6 Mon Sep 17 00:00:00 2001 From: Fabian Hanke Date: Sun, 25 Feb 2018 16:05:49 +0100 Subject: [PATCH 82/94] added output argument in launch files --- launch/line_detection.launch | 9 ++++++--- launch/warp_image.launch | 9 ++++++--- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/launch/line_detection.launch b/launch/line_detection.launch index 88ddbaa..67513a1 100644 --- a/launch/line_detection.launch +++ b/launch/line_detection.launch @@ -6,11 +6,14 @@ + + + - + @@ -22,8 +25,8 @@ - - + + diff --git a/launch/warp_image.launch b/launch/warp_image.launch index f8bacdf..b99dff0 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -8,11 +8,14 @@ + + + - + @@ -27,8 +30,8 @@ - - + + From df2050dd407266295f00b8cd6aa5814f7d5f34c6 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Mon, 28 May 2018 17:47:42 +0200 Subject: [PATCH 83/94] Start homography publisher in launch file. Small changes * Add todos in code * Add output for some topics --- .../common_image_operations.h | 3 ++ .../line_detection.h | 2 +- launch/line_detection.launch | 9 ++++-- src/line_detection.cpp | 32 +++++++------------ 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 2f32501..5ec11d9 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -206,12 +206,15 @@ class TransformHelper { boost::bind(&camInfo_callback, _1, std::ref(cam_model_), std::ref(camera_model_received_) ) ); + ROS_INFO("TransformHelper subscribes to '%s' for camera_info", cam_info_sub_.getTopic().c_str()); + homography_sub_ = ros::NodeHandle().subscribe("homography_in", 1, boost::bind(homography_callback, _1, std::ref(cam2world_), std::ref(world2cam_), std::ref(scaling_mat_), std::ref(scaling_mat_inv_), std::ref(scaledCam2world_), std::ref(scaledWorld2cam_), std::ref(homography_received_))); + ROS_INFO("TransformHelper subscribes to '%s' for homography", homography_sub_.getTopic().c_str()); return true; } diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index 2d35a83..b92240a 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -42,7 +42,7 @@ class LineDetection { // communication image_transport::ImageTransport imageTransport_; image_transport::Subscriber imageSubscriber_; - ros::Publisher line_output_pub_; // note: not used yet + ros::Publisher line_output_pub_; ros::Subscriber homography_params_sub_; #ifdef PUBLISH_DEBUG image_transport::Publisher debugImgPub_; diff --git a/launch/line_detection.launch b/launch/line_detection.launch index 67513a1..da27cb7 100644 --- a/launch/line_detection.launch +++ b/launch/line_detection.launch @@ -1,7 +1,7 @@ - + @@ -14,7 +14,7 @@ - + @@ -30,4 +30,9 @@ + + + + + diff --git a/src/line_detection.cpp b/src/line_detection.cpp index 9196fa6..5a61c58 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -29,12 +30,11 @@ LineDetection::~LineDetection() { /// \return true for success, false for failure. /// bool LineDetection::init() { - // todo: topics as params // dynamic reconfigure dsrv_server_.setCallback(dsrv_cb_); //subscribe to camera image - imageSubscriber_ = imageTransport_.subscribe("img_in", 10, &LineDetection::imageCallback, this); + imageSubscriber_ = imageTransport_.subscribe("/img_in", 10, &LineDetection::imageCallback, this); ROS_INFO_STREAM("Subscribed image transport to topic " << imageSubscriber_.getTopic()); line_output_pub_ = nh_.advertise("roadLine", 10); @@ -42,7 +42,7 @@ bool LineDetection::init() { #ifdef PUBLISH_DEBUG debugImgPub_ = imageTransport_.advertise("debug_image", 10); - ROS_INFO_STREAM("publishing debug image on topic " << debugImgPub_.getTopic()); + ROS_INFO_STREAM("Publishing debug image on topic " << debugImgPub_.getTopic()); #endif // common image operations @@ -61,7 +61,6 @@ bool LineDetection::init() { /// void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { currentImage_ = convertImageMessage(imgIn); - // auto t_start = std::chrono::high_resolution_clock::now(); findLane(); // auto t_end = std::chrono::high_resolution_clock::now(); @@ -95,14 +94,6 @@ void LineDetection::findLane() { int polygonLengths[] = { 4 }; cv::fillPoly(processingImg, polygonStarts, polygonLengths, 1, cv::Scalar(), cv::LINE_8); -#ifdef PUBLISH_DEBUG - // Display the polygon which will be zeroed out -// cv::line(outputImg, vehiclePolygon[0][0], vehiclePolygon[0][1], cv::Scalar(255), 2, cv::LINE_AA); -// cv::line(outputImg, vehiclePolygon[0][1], vehiclePolygon[0][2], cv::Scalar(255), 2, cv::LINE_AA); -// cv::line(outputImg, vehiclePolygon[0][2], vehiclePolygon[0][3], cv::Scalar(255), 2, cv::LINE_AA); -// cv::line(outputImg, vehiclePolygon[0][3], vehiclePolygon[0][0], cv::Scalar(255), 2, cv::LINE_AA); -#endif - // Get houghlines std::vector hLinePoints; std::vector houghLines; @@ -124,11 +115,11 @@ void LineDetection::findLane() { } // Create a static guess, in case we do not have on from the previous frame - // todo: we should only do, when the car starts in the start box. Otherwise we can get in trouble + // todo: we should only do this, when the car starts in the start box. Otherwise we can get in trouble worldPoints.clear(); imagePoints.clear(); if(currentGuess_.empty()) { - ROS_INFO_STREAM("dummy guess"); + // todo: we should have a state like "LOST_LINE", where we try to find the middle line again (more checks, if it is really the middle one) worldPoints.push_back(cv::Point2f(0.24, 0.5)); worldPoints.push_back(cv::Point2f(0.4, 0.5)); worldPoints.push_back(cv::Point2f(0.55, 0.5)); @@ -139,7 +130,7 @@ void LineDetection::findLane() { } #ifdef PUBLISH_DEBUG - // Draw our guess + // Draw our guess (in blue) for(size_t i = 0; i < currentGuess_.size(); i++) { cv::line(outputImg, currentGuess_.at(i).iP1_, currentGuess_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); } @@ -196,7 +187,7 @@ void LineDetection::findLane() { } if(middleLine.empty()) { - // todo: handle this better + // todo: handle this better, e.g. use left and right lane and compute the middle lane if the distances are plausible ROS_INFO_STREAM("no middle line found"); currentGuess_.clear(); #ifdef PUBLISH_DEBUG @@ -206,6 +197,7 @@ void LineDetection::findLane() { } // transform old line based on odometry + // todo: maybe we do not need this for now, since we have high frame rate and therefore the offset between frames is very small // tf::StampedTransform transform; // try{ // tfListener_.lookupTransform("/odometry", "/rear_axis_middle_ground", ros::Time(0), transform); @@ -245,6 +237,7 @@ void LineDetection::findLane() { // } // middle line is 0.2m long and then interrupted for 0.2m + // todo: in the extendes cup we can have double middle lines -> find a solution to this // todo: this should be checked // sort our middle lines based on wP1_.x of line std::sort(middleLine.begin(), middleLine.end(), [](Line &a, Line &b){ return a.wP1_.x < b.wP1_.x; }); @@ -269,7 +262,7 @@ void LineDetection::findLane() { a = vertices2f[1]; b = vertices2f[2]; } - // ensures the right order of the points in newMidLinePts vector + // ensures the correct order of the points in newMidLinePts vector if(a.x > b.x) { newMidLinePts.push_back(b); newMidLinePts.push_back(a); @@ -369,7 +362,6 @@ void LineDetection::findLane() { } } - bool done = false; for(size_t i = 1; i < currentGuess_.size() && !done; i++) { // angles between two connected segments should be plausible @@ -384,7 +376,7 @@ void LineDetection::findLane() { currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); done = true; } else if((i < (currentGuess_.size() - 1)) && currentGuess_.at(i).getLength() > 0.3) { - // if a segment is longer than 0.3m, the line is probably not dashed (so no middle line) + // if a segment is longer than 0.3m, the line is probably not dashed (so no middle line) todo: line does not have to be dashed // excluding the last segment, since we created this for searching forward ROS_INFO_STREAM("clipped line because of length"); currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); @@ -396,7 +388,7 @@ void LineDetection::findLane() { currentMiddleLine_.insert(currentMiddleLine_.begin(), currentGuess_.begin(), currentGuess_.end() - (done ? 0 : 1)); #ifdef PUBLISH_DEBUG - // draw our middle line + // draw our middle line (yellow) for(auto l : currentMiddleLine_) { cv::line(outputImg, l.iP1_, l.iP2_, cv::Scalar(255,0,255), 2, cv::LINE_AA); } From 82e49a3416918e24337d1e3f5aff1f0048b341ec Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Mon, 11 Jun 2018 21:15:30 +0200 Subject: [PATCH 84/94] Refactor code for future use * use lines to find road markings based on current road state --- .../line_detection.h | 26 +- src/line_detection.cpp | 238 ++++++++---------- 2 files changed, 126 insertions(+), 138 deletions(-) diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index b92240a..41a456d 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -23,6 +23,18 @@ class LineDetection { ros::NodeHandle nh_; ros::NodeHandle pnh_; + // drive state enum + enum DriveState { + StartBox = 0, + Parking = 1, + Intersection = 2, + Street = 3 + }; + + // drive mode config + bool isObstaceCourse; // true for Obstacle Evasion Course; false for Free Drive (w/o Obstacles) & Parking + DriveState driveState; + // configs float lineWidth_; float lineAngle_; @@ -34,10 +46,14 @@ class LineDetection { int houghMaxLineGap_; // variables - CvImagePtr currentImage_; +// CvImagePtr currentImage_; std::vector currentGuess_; std::vector currentMiddleLine_; ImageOperator image_operator_; + int imgHeight_; +#ifdef PUBLISH_DEBUG + cv::Mat debugImg_; +#endif // communication image_transport::ImageTransport imageTransport_; @@ -68,13 +84,21 @@ class LineDetection { // methods void findLane(); + void findLinesWithHough(CvImagePtr img, std::vector &houghLines); + bool findLanesFromStartbox(std::vector &lines); // called at the start to initialy find the lanes + bool findLaneSimple(std::vector &lines); // finds the lane assuming there are no markings missing (e.g. for Parking area) + bool findLaneAdvanced(std::vector &lines); // finds the lane assuming some markings could be missing + bool findIntersectionExit(std::vector &lines); // find the end of an intersection (used while in intersection) + // helper functions float getDistanceBetweenPoints(const cv::Point2f a, const cv::Point2f b) { auto dX = a.x - b.x; auto dY = a.y - b.y; return sqrt(dX * dX + dY * dY); } + void buildBbAroundLines(std::vector ¢erPoints, std::vector &midLinePoints); + public: LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); ~LineDetection(); diff --git a/src/line_detection.cpp b/src/line_detection.cpp index 5a61c58..e3cf74f 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -13,6 +13,8 @@ namespace drive_ros_image_recognition { LineDetection::LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh) : nh_(nh) , pnh_(pnh) + , isObstaceCourse(false) + , driveState(DriveState::Street) // TODO: this should be StartBox in real conditions , imageTransport_(pnh) , lineWidth_(0.4) , image_operator_() @@ -60,32 +62,54 @@ bool LineDetection::init() { /// \param imgIn the incoming camera image message. /// void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { - currentImage_ = convertImageMessage(imgIn); + // TODO: so far these states are only for the free drive (w/o obstacles) and probably not even complete, yet + auto currentImage = convertImageMessage(imgIn); + imgHeight_ = currentImage->rows; + std::vector linesInImage; + findLinesWithHough(currentImage, linesInImage); + if(driveState == DriveState::StartBox) { +// findLanesFromStartbox(linesInImage); + } else if(driveState == DriveState::Street) { + findLaneAdvanced(linesInImage); + } else if(driveState == DriveState::Parking) { +// findLaneSimple(linesInImage); + } else if(driveState == DriveState::Intersection) { +// findIntersectionExit(linesInImage); + } else { + + } + +#ifdef PUBLISH_DEBUG + debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, debugImg_).toImageMsg()); +#endif + // auto t_start = std::chrono::high_resolution_clock::now(); - findLane(); +// findLane(); // auto t_end = std::chrono::high_resolution_clock::now(); // ROS_INFO_STREAM("Cycle time: " << (std::chrono::duration(t_end-t_start).count()) << "ms"); } /// -/// \brief LineDetection::findLane -/// Uses the current image to find the street marking lanes. +/// \brief LineDetection::findLinesWithHough +/// Extract Lines from the image using Hough Lines +/// \param img A CvImagePtr to the current image where we want to search for lines +/// \param houghLines A std::vector where the Lines will be returned. Has to be empty. /// -void LineDetection::findLane() { +void LineDetection::findLinesWithHough(CvImagePtr img, std::vector &houghLines) { cv::Mat processingImg; #ifdef PUBLISH_DEBUG - cv::Mat outputImg; - cv::cvtColor(*currentImage_, outputImg, CV_GRAY2RGB); + cv::cvtColor(*img, debugImg_, CV_GRAY2RGB); #endif // Blur the image and find edges with Canny - cv::GaussianBlur(*currentImage_, processingImg, cv::Size(15, 15), 0, 0); + cv::GaussianBlur(*img, processingImg, cv::Size(15, 15), 0, 0); cv::Canny(processingImg, processingImg, cannyThreshold_, cannyThreshold_ * 3, 3); // Zero out the vehicle int imgWidth = processingImg.cols; int imgHeight = processingImg.rows; cv::Point vehiclePolygon[1][4]; + // TODO: these factor should be in a config vehiclePolygon[0][0] = cv::Point(imgWidth * .35, imgHeight); vehiclePolygon[0][1] = cv::Point(imgWidth * .65, imgHeight); vehiclePolygon[0][2] = cv::Point(imgWidth * .65, imgHeight * .8); @@ -96,7 +120,6 @@ void LineDetection::findLane() { // Get houghlines std::vector hLinePoints; - std::vector houghLines; cv::HoughLinesP(processingImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); @@ -109,15 +132,25 @@ void LineDetection::findLane() { } image_operator_.imageToWorld(imagePoints, worldPoints); - // Build lines from points + // Build lines from points to return them for(size_t i = 0; i < worldPoints.size(); i += 2) { houghLines.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); } + return; +} + +/// +/// \brief LineDetection::findLaneAdvanced +/// In this scenario lines can be missing, we could have a double middle line, intersection lines, +/// and the start line could occur. +/// \param lines +/// \return +/// +bool LineDetection::findLaneAdvanced(std::vector &lines) { // Create a static guess, in case we do not have on from the previous frame // todo: we should only do this, when the car starts in the start box. Otherwise we can get in trouble - worldPoints.clear(); - imagePoints.clear(); + std::vector imagePoints, worldPoints; if(currentGuess_.empty()) { // todo: we should have a state like "LOST_LINE", where we try to find the middle line again (more checks, if it is really the middle one) worldPoints.push_back(cv::Point2f(0.24, 0.5)); @@ -132,13 +165,13 @@ void LineDetection::findLane() { #ifdef PUBLISH_DEBUG // Draw our guess (in blue) for(size_t i = 0; i < currentGuess_.size(); i++) { - cv::line(outputImg, currentGuess_.at(i).iP1_, currentGuess_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); + cv::line(debugImg_, currentGuess_.at(i).iP1_, currentGuess_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); } #endif // Use the guess to classify the lines - std::vector leftLine, middleLine, rightLine; - for(Line sl : houghLines) { + std::vector leftLines, middleLines, rightLines, horizontalLines; + for(Line sl : lines) { Line segment = currentGuess_.at(0); bool isInSegment = false; // find the corresponding segment @@ -162,116 +195,49 @@ void LineDetection::findLane() { // the current approach leads to problems in curves auto absDistanceToMidLine = std::abs(sl.wMid_.y - segment.wMid_.y); if(absDistanceToMidLine < (lineWidth_ / 2)) { - middleLine.push_back(sl); + middleLines.push_back(sl); #ifdef PUBLISH_DEBUG - cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); + cv::line(debugImg_, sl.iP1_, sl.iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); #endif } else if((sl.wMid_.y < segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { - rightLine.push_back(sl); + rightLines.push_back(sl); #ifdef PUBLISH_DEBUG - cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); + cv::line(debugImg_, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); #endif } else if((sl.wMid_.y > segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { - leftLine.push_back(sl); + leftLines.push_back(sl); #ifdef PUBLISH_DEBUG - cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); + cv::line(debugImg_, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); #endif } } else { - // todo: classify the line (stop, start, ...) + horizontalLines.push_back(sl); #ifdef PUBLISH_DEBUG - cv::line(outputImg, sl.iP1_, sl.iP2_, cv::Scalar(255, 255), 2, cv::LINE_AA); + cv::line(debugImg_, sl.iP1_, sl.iP2_, cv::Scalar(255, 255), 2, cv::LINE_AA); #endif } } } - if(middleLine.empty()) { - // todo: handle this better, e.g. use left and right lane and compute the middle lane if the distances are plausible - ROS_INFO_STREAM("no middle line found"); + if(middleLines.empty()/* && leftLines.empty() && rightLines.empty()*/) { + // TODO: this should not happen. maybe set state to sth. like RECOVER + ROS_WARN_STREAM("No lane markings found"); currentGuess_.clear(); -#ifdef PUBLISH_DEBUG - debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, outputImg).toImageMsg()); -#endif - return; + return false; } - // transform old line based on odometry - // todo: maybe we do not need this for now, since we have high frame rate and therefore the offset between frames is very small -// tf::StampedTransform transform; -// try{ -// tfListener_.lookupTransform("/odometry", "/rear_axis_middle_ground", ros::Time(0), transform); -// } -// catch (tf::TransformException ex){ -// ROS_ERROR("%s",ex.what()); -// ros::Duration(1.0).sleep(); -// } -// cv::Point2f newPoint(transform.getOrigin().x(), transform.getOrigin().y()); -// // auto xTransform = newPoint.x - oldPoint_.x; -// // auto yTransform = newPoint.y - oldPoint_y; -// auto ptsDifference = newPoint - oldPoint_; -// ROS_INFO_STREAM(ptsDifference.x << "," << ptsDifference.y); -// if(ptsDifference.x != 0.0){ -// oldPoint_ = newPoint; -// std::vector oldTransformedLine; -// for(auto seg : currentLane_) { -// oldTransformedLine.push_back(cv::Point2f(seg.wP1_ + ptsDifference)); -// oldTransformedLine.push_back(cv::Point2f(seg.wP2_ + ptsDifference)); -// } - - -// imagePoints.clear(); -// image_operator_.worldToImage(oldTransformedLine, imagePoints); -// for(auto i = 1; i < (imagePoints.size() - 1); i++) -// cv::line(outputImg, imagePoints.at(i - 1), imagePoints.at(i), cv::Scalar(255,255), 2); -// for(auto i = 0; i < currentLane_.size(); i++) { -// auto p1 = imagePoints.at(i * 2); -// auto p2 = imagePoints.at((i * 2) + 1); -// cv::circle(outputImg, currentLane_.at(i).iP1_, 5, cv::Scalar(124,124), 3); -// cv::circle(outputImg, p1, 5, cv::Scalar(255,255), 3); -// cv::line(outputImg, currentLane_.at(i).iP1_, p1, cv::Scalar(124,124), 2); -// cv::circle(outputImg, currentLane_.at(i).iP2_, 5, cv::Scalar(124,124), 3); -// cv::circle(outputImg, p2, 5, cv::Scalar(255,255), 3); -// cv::line(outputImg, currentLane_.at(i).iP2_, p2, cv::Scalar(124,124), 2); -// } -// } - - // middle line is 0.2m long and then interrupted for 0.2m - // todo: in the extendes cup we can have double middle lines -> find a solution to this - // todo: this should be checked + // TODO: find a better and more efficient way to do this. maybe also project left and right line into middle to use all points. + // in the free drive event, double lines are treated as normal dashed lines // sort our middle lines based on wP1_.x of line - std::sort(middleLine.begin(), middleLine.end(), [](Line &a, Line &b){ return a.wP1_.x < b.wP1_.x; }); + std::sort(middleLines.begin(), middleLines.end(), [](Line &a, Line &b){ return a.wP1_.x < b.wP1_.x; }); // build bounding boxes around group of lines, where lines in group are closer than 0.25m to each other - float currentMinX = middleLine.at(0).wP1_.x; + float currentMinX = middleLines.at(0).wP1_.x; std::vector cPts, newMidLinePts; - for(auto l : middleLine) { + for(auto l : middleLines) { if(l.wP1_.x > (currentMinX + 0.25)) { - auto rect = cv::minAreaRect(cPts); - cv::Point2f a, b; - cv::Point2f vertices2f[4]; - rect.points(vertices2f); - - currentMinX = l.wP1_.x; - - // todo: this should be in a function, since we are using it again after the for loop - // use the long side as line - if(std::abs(vertices2f[0].x - vertices2f[1].x) > std::abs(vertices2f[1].x - vertices2f[2].x)) { - a = vertices2f[0]; - b = vertices2f[1]; - } else { - a = vertices2f[1]; - b = vertices2f[2]; - } - // ensures the correct order of the points in newMidLinePts vector - if(a.x > b.x) { - newMidLinePts.push_back(b); - newMidLinePts.push_back(a); - } else { - newMidLinePts.push_back(a); - newMidLinePts.push_back(b); - } - + buildBbAroundLines(cPts, newMidLinePts); cPts.clear(); + currentMinX = l.wP1_.x; } cPts.push_back(l.wP1_); cPts.push_back(l.wP2_); @@ -279,27 +245,7 @@ void LineDetection::findLane() { // finish the bounding box building step if(!cPts.empty()) { - auto rect = cv::minAreaRect(cPts); - cv::Point2f vertices2f[4]; - rect.points(vertices2f); - cv::Point2f a, b; - - // use the long side as line - if(std::abs(vertices2f[0].x - vertices2f[1].x) > std::abs(vertices2f[1].x - vertices2f[2].x)) { - a = vertices2f[0]; - b = vertices2f[1]; - } else { - a = vertices2f[1]; - b = vertices2f[2]; - } - // ensures the right order of the points in newMidLinePts vector - if(a.x > b.x) { - newMidLinePts.push_back(b); - newMidLinePts.push_back(a); - } else { - newMidLinePts.push_back(a); - newMidLinePts.push_back(b); - } + buildBbAroundLines(cPts, newMidLinePts); } // build the middle line from out points @@ -318,7 +264,7 @@ void LineDetection::findLane() { wPt2 = newMidLinePts.at(1); dir = wPt2 - wPt1; dirLen = sqrt(dir.x * dir.x + dir.y * dir.y); - // the image starts 0.24m from the rear_axis_middle_ground. todo: sould be in config + // the image starts 0.24m from the rear_axis_middle_ground. TODO: sould be in config auto distToImageBoundary = wPt1.x - 0.24; if(distToImageBoundary > 0.1) { // distance is positive // dir, distToImageBoundary, dirLen are all positive @@ -338,10 +284,10 @@ void LineDetection::findLane() { auto a = imagePoints.at(i - 1); auto b = imagePoints.at(i); // this makes the line coordinates inconsistant, but this should not be a serious problem - if(a.y > imgHeight) - a.y = imgHeight - 1; - if(b.y > imgHeight) - b.y = imgHeight - 1; + if(a.y > imgHeight_) + a.y = imgHeight_ - 1; + if(b.y > imgHeight_) + b.y = imgHeight_ - 1; currentGuess_.push_back(Line(a, b, newMidLinePts.at(i - 1), newMidLinePts.at(i))); } @@ -349,10 +295,10 @@ void LineDetection::findLane() { // validate our new guess if(!currentGuess_.empty()) { if(currentGuess_.at(currentGuess_.size() - 1).wP2_.x < 0.5) { - // our guess is too short, it is better to use the default one + // our guess is too short, it is better to use the default one; TODO: bad idea -> RECOVER state currentGuess_.clear(); } else if((currentGuess_.at(0).getAngle() < 1.0) || (currentGuess_.at(0).getAngle() > 2.6)) { - // the angle of the first segment is weird (probably a workaround for now, why can this happen?) + // the angle of the first segment is weird (TODO: workaround for now, why can this happen?) // if we have more than one segment, then we just delete the first if(currentGuess_.size() > 1) { currentGuess_.erase(currentGuess_.begin()); @@ -378,7 +324,7 @@ void LineDetection::findLane() { } else if((i < (currentGuess_.size() - 1)) && currentGuess_.at(i).getLength() > 0.3) { // if a segment is longer than 0.3m, the line is probably not dashed (so no middle line) todo: line does not have to be dashed // excluding the last segment, since we created this for searching forward - ROS_INFO_STREAM("clipped line because of length"); +// ROS_INFO_STREAM("clipped line because of length"); currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); done = true; } @@ -390,18 +336,13 @@ void LineDetection::findLane() { #ifdef PUBLISH_DEBUG // draw our middle line (yellow) for(auto l : currentMiddleLine_) { - cv::line(outputImg, l.iP1_, l.iP2_, cv::Scalar(255,0,255), 2, cv::LINE_AA); + cv::line(debugImg_, l.iP1_, l.iP2_, cv::Scalar(255,0,255), 2, cv::LINE_AA); } #endif } - // todo: move old guess with odometry and compare old and new guess - -#ifdef PUBLISH_DEBUG - debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, outputImg).toImageMsg()); -#endif - // publish points to road topic + // TODO: we want to publish not only the middle line, but also start line, stop lines, intersections, barred areas, ... drive_ros_msgs::RoadLine msgMidLine; msgMidLine.lineType = drive_ros_msgs::RoadLine::MIDDLE; for(auto l : currentMiddleLine_) { @@ -415,7 +356,30 @@ void LineDetection::findLane() { } line_output_pub_.publish(msgMidLine); +} +void LineDetection::buildBbAroundLines(std::vector ¢erPoints, std::vector &midLinePoints) { + auto rect = cv::minAreaRect(centerPoints); + cv::Point2f a, b; + cv::Point2f vertices2f[4]; + rect.points(vertices2f); + + // use the long side as line + if(std::abs(vertices2f[0].x - vertices2f[1].x) > std::abs(vertices2f[1].x - vertices2f[2].x)) { + a = vertices2f[0]; + b = vertices2f[1]; + } else { + a = vertices2f[1]; + b = vertices2f[2]; + } + // ensures the correct order of the points in newMidLinePts vector + if(a.x > b.x) { + midLinePoints.push_back(b); + midLinePoints.push_back(a); + } else { + midLinePoints.push_back(a); + midLinePoints.push_back(b); + } } /// From c505dd168a72d54055f438d2b08cb54c8b7154dd Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Wed, 13 Jun 2018 14:02:12 +0200 Subject: [PATCH 85/94] Bundle debug image output in function --- include/drive_ros_image_recognition/line.h | 18 +- .../line_detection.h | 10 +- src/line_detection.cpp | 164 ++++++++++++------ 3 files changed, 138 insertions(+), 54 deletions(-) diff --git a/include/drive_ros_image_recognition/line.h b/include/drive_ros_image_recognition/line.h index 42a885a..731a568 100644 --- a/include/drive_ros_image_recognition/line.h +++ b/include/drive_ros_image_recognition/line.h @@ -14,10 +14,26 @@ namespace drive_ros_image_recognition { /// class Line { public: + enum LineType { + UNKNOWN = 0, + LEFT_LINE = 1, + MIDDLE_LINE = 2, + RIGHT_LINE = 3, + HORIZONTAL_LEFT_LANE = 4, + HORIZONTAL_RIGHT_LANE = 5, + HORIZONTAL_OUTER_LEFT = 6, + HORIZONTAL_OUTER_RIGHT = 7, + STOP_LINE = 8, + START_LINE = 9, + GUESS = 10 + }; + cv::Point iP1_, iP2_, iMid_; cv::Point2f wP1_, wP2_, wMid_; + LineType lineType_; - Line(cv::Point iP1, cv::Point iP2, cv::Point2f wP1, cv::Point2f wP2) + Line(cv::Point iP1, cv::Point iP2, cv::Point2f wP1, cv::Point2f wP2, LineType lineType) + : lineType_(lineType) { if(wP1.x < wP2.x) { iP1_ = iP1; diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index 41a456d..cf956a4 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -15,6 +15,8 @@ #include "drive_ros_msgs/RoadLane.h" #include "line.h" +#define GUESS_RESOLUTION = 0.2 + namespace drive_ros_image_recognition { class LineDetection { @@ -47,10 +49,11 @@ class LineDetection { // variables // CvImagePtr currentImage_; - std::vector currentGuess_; +// std::vector currentGuess_; std::vector currentMiddleLine_; ImageOperator image_operator_; int imgHeight_; + int imgWidth_; #ifdef PUBLISH_DEBUG cv::Mat debugImg_; #endif @@ -97,8 +100,13 @@ class LineDetection { return sqrt(dX * dX + dY * dY); } + void classifyHorizontalLine(Line &line, float worldDistToMiddleLine); void buildBbAroundLines(std::vector ¢erPoints, std::vector &midLinePoints); +#ifdef PUBLISH_DEBUG + void drawAndPublishDebugLines(std::vector &lines); +#endif + public: LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh); ~LineDetection(); diff --git a/src/line_detection.cpp b/src/line_detection.cpp index e3cf74f..314d5c2 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -65,6 +65,7 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { // TODO: so far these states are only for the free drive (w/o obstacles) and probably not even complete, yet auto currentImage = convertImageMessage(imgIn); imgHeight_ = currentImage->rows; + imgWidth_ = currentImage->cols; std::vector linesInImage; findLinesWithHough(currentImage, linesInImage); if(driveState == DriveState::StartBox) { @@ -134,7 +135,7 @@ void LineDetection::findLinesWithHough(CvImagePtr img, std::vector &houghL // Build lines from points to return them for(size_t i = 0; i < worldPoints.size(); i += 2) { - houghLines.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); + houghLines.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1), Line::LineType::UNKNOWN)); } return; @@ -148,26 +149,30 @@ void LineDetection::findLinesWithHough(CvImagePtr img, std::vector &houghL /// \return /// bool LineDetection::findLaneAdvanced(std::vector &lines) { - // Create a static guess, in case we do not have on from the previous frame + // Last frames middle line is our new guess. Clear old middle line + std::vector currentGuess_; + if(!currentMiddleLine_.empty()) { + for(auto l : currentMiddleLine_) { + l.lineType_ = Line::LineType::GUESS; + currentGuess_.push_back(l); + } + currentMiddleLine_.clear(); + } + // todo: we should only do this, when the car starts in the start box. Otherwise we can get in trouble std::vector imagePoints, worldPoints; if(currentGuess_.empty()) { - // todo: we should have a state like "LOST_LINE", where we try to find the middle line again (more checks, if it is really the middle one) + // todo: we should have a state like "RECOVER", where we try to find the middle line again (more checks, if it is really the middle one) worldPoints.push_back(cv::Point2f(0.24, 0.5)); worldPoints.push_back(cv::Point2f(0.4, 0.5)); worldPoints.push_back(cv::Point2f(0.55, 0.5)); worldPoints.push_back(cv::Point2f(0.70, 0.5)); image_operator_.worldToImage(worldPoints, imagePoints); for(size_t i = 1; i < imagePoints.size(); i++) - currentGuess_.push_back(Line(imagePoints.at(i - 1), imagePoints.at(i), worldPoints.at(i - 1), worldPoints.at(i))); + currentGuess_.push_back(Line(imagePoints.at(i - 1), imagePoints.at(i), worldPoints.at(i - 1), worldPoints.at(i), Line::LineType::GUESS)); } -#ifdef PUBLISH_DEBUG - // Draw our guess (in blue) - for(size_t i = 0; i < currentGuess_.size(); i++) { - cv::line(debugImg_, currentGuess_.at(i).iP1_, currentGuess_.at(i).iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); - } -#endif + drawAndPublishDebugLines(currentGuess_); // Use the guess to classify the lines std::vector leftLines, middleLines, rightLines, horizontalLines; @@ -195,34 +200,63 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { // the current approach leads to problems in curves auto absDistanceToMidLine = std::abs(sl.wMid_.y - segment.wMid_.y); if(absDistanceToMidLine < (lineWidth_ / 2)) { + sl.lineType_ = Line::LineType::MIDDLE_LINE; middleLines.push_back(sl); -#ifdef PUBLISH_DEBUG - cv::line(debugImg_, sl.iP1_, sl.iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); -#endif } else if((sl.wMid_.y < segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { + sl.lineType_ = Line::LineType::RIGHT_LINE; rightLines.push_back(sl); -#ifdef PUBLISH_DEBUG - cv::line(debugImg_, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); -#endif } else if((sl.wMid_.y > segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { + sl.lineType_ = Line::LineType::LEFT_LINE; leftLines.push_back(sl); -#ifdef PUBLISH_DEBUG - cv::line(debugImg_, sl.iP1_, sl.iP2_, cv::Scalar(255), 2, cv::LINE_AA); -#endif } } else { + classifyHorizontalLine(sl, segment.wMid_.y - sl.wMid_.y); horizontalLines.push_back(sl); -#ifdef PUBLISH_DEBUG - cv::line(debugImg_, sl.iP1_, sl.iP2_, cv::Scalar(255, 255), 2, cv::LINE_AA); -#endif } } } + // Classify the horizontal lines + // Find start line + // TODO: check if we have lines on left and right lane. in this case, set all left and right ones (in a certain x-interval to start line) + for(auto firstIt = horizontalLines.begin(); firstIt != horizontalLines.end(); ++firstIt) { + for(auto secondIt = horizontalLines.begin(); secondIt != horizontalLines.end(); ++secondIt) { + if(((firstIt->lineType_ == Line::LineType::HORIZONTAL_LEFT_LANE) && (secondIt->lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE)) || + ((firstIt->lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE) && (secondIt->lineType_ == Line::LineType::HORIZONTAL_LEFT_LANE))) { + if(abs(firstIt->wMid_.x - secondIt->wMid_.x) < 0.2) { + firstIt->lineType_ = Line::LineType::START_LINE; + secondIt->lineType_ = Line::LineType::START_LINE; + } + } + } + } + // Find stop line + for(auto firstIt = horizontalLines.begin(); firstIt != horizontalLines.end(); ++firstIt) { + if(firstIt->lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE) { + bool foundRightOuterBound = false, foundLeftOuterBound = false; + for(auto secondIt = horizontalLines.begin(); secondIt != horizontalLines.end(); ++secondIt) { + if(secondIt->lineType_ == Line::LineType::HORIZONTAL_OUTER_LEFT) + foundLeftOuterBound = true; + else if(secondIt->lineType_ == Line::LineType::HORIZONTAL_OUTER_RIGHT) + foundRightOuterBound = true; + } + if(foundLeftOuterBound && foundRightOuterBound) { + firstIt->lineType_ = Line::LineType::STOP_LINE; +// ROS_INFO_STREAM("Found a stop line"); + } + } + } + +#ifdef PUBLISH_DEBUG +// drawAndPublishDebugLines(middleLines); + drawAndPublishDebugLines(rightLines); + drawAndPublishDebugLines(leftLines); + drawAndPublishDebugLines(horizontalLines); +#endif + if(middleLines.empty()/* && leftLines.empty() && rightLines.empty()*/) { // TODO: this should not happen. maybe set state to sth. like RECOVER - ROS_WARN_STREAM("No lane markings found"); - currentGuess_.clear(); + ROS_WARN_STREAM("No middle line found"); return false; } @@ -278,8 +312,8 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { // convert points imagePoints.clear(); image_operator_.worldToImage(newMidLinePts, imagePoints); - // create new guess and middle line - currentGuess_.clear(); + // create the new middle line + currentMiddleLine_.clear(); for(size_t i = 1; i < imagePoints.size(); i++) { auto a = imagePoints.at(i - 1); auto b = imagePoints.at(i); @@ -289,58 +323,51 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { if(b.y > imgHeight_) b.y = imgHeight_ - 1; - currentGuess_.push_back(Line(a, b, newMidLinePts.at(i - 1), newMidLinePts.at(i))); + currentMiddleLine_.push_back(Line(a, b, newMidLinePts.at(i - 1), newMidLinePts.at(i), Line::LineType::MIDDLE_LINE)); } // validate our new guess - if(!currentGuess_.empty()) { - if(currentGuess_.at(currentGuess_.size() - 1).wP2_.x < 0.5) { + if(!currentMiddleLine_.empty()) { + if(currentMiddleLine_.at(currentMiddleLine_.size() - 1).wP2_.x < 0.5) { // our guess is too short, it is better to use the default one; TODO: bad idea -> RECOVER state - currentGuess_.clear(); - } else if((currentGuess_.at(0).getAngle() < 1.0) || (currentGuess_.at(0).getAngle() > 2.6)) { + currentMiddleLine_.clear(); + } else if((currentMiddleLine_.at(0).getAngle() < 1.0) || (currentMiddleLine_.at(0).getAngle() > 2.6)) { // the angle of the first segment is weird (TODO: workaround for now, why can this happen?) // if we have more than one segment, then we just delete the first - if(currentGuess_.size() > 1) { - currentGuess_.erase(currentGuess_.begin()); + if(currentMiddleLine_.size() > 1) { + currentMiddleLine_.erase(currentMiddleLine_.begin()); } else { // otherwise throw the guess away - currentGuess_.clear(); + currentMiddleLine_.clear(); } } bool done = false; - for(size_t i = 1; i < currentGuess_.size() && !done; i++) { + for(size_t i = 1; i < currentMiddleLine_.size() && !done; i++) { // angles between two connected segments should be plausible - if(std::abs(currentGuess_.at(i - 1).getAngle() - currentGuess_.at(i).getAngle()) > 0.8) { + if(std::abs(currentMiddleLine_.at(i - 1).getAngle() - currentMiddleLine_.at(i).getAngle()) > 0.8) { // 0.8 = 45 degree // todo: clear whole line or just to this segment? - currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); + currentMiddleLine_.erase(currentMiddleLine_.begin() + i, currentMiddleLine_.end()); // ROS_INFO_STREAM("clipped line because of angle difference"); done = true; - } else if(currentGuess_.at(i).wP2_.x > maxViewRange_) { + } else if(currentMiddleLine_.at(i).wP2_.x > maxViewRange_) { // ROS_INFO_STREAM("clipped line because of maxViewRange_"); - currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); + currentMiddleLine_.erase(currentMiddleLine_.begin() + i, currentMiddleLine_.end()); done = true; - } else if((i < (currentGuess_.size() - 1)) && currentGuess_.at(i).getLength() > 0.3) { - // if a segment is longer than 0.3m, the line is probably not dashed (so no middle line) todo: line does not have to be dashed + } + else if((i < (currentMiddleLine_.size() - 1)) && currentMiddleLine_.at(i).getLength() > 0.3) { + // we build middle line segments of max 0.25m, so this is a test; TODO: overthink this // excluding the last segment, since we created this for searching forward // ROS_INFO_STREAM("clipped line because of length"); - currentGuess_.erase(currentGuess_.begin() + i, currentGuess_.end()); + currentMiddleLine_.erase(currentMiddleLine_.begin() + i, currentMiddleLine_.end()); done = true; } } - - currentMiddleLine_.clear(); - currentMiddleLine_.insert(currentMiddleLine_.begin(), currentGuess_.begin(), currentGuess_.end() - (done ? 0 : 1)); - -#ifdef PUBLISH_DEBUG - // draw our middle line (yellow) - for(auto l : currentMiddleLine_) { - cv::line(debugImg_, l.iP1_, l.iP2_, cv::Scalar(255,0,255), 2, cv::LINE_AA); - } -#endif } + drawAndPublishDebugLines(currentMiddleLine_); + // publish points to road topic // TODO: we want to publish not only the middle line, but also start line, stop lines, intersections, barred areas, ... drive_ros_msgs::RoadLine msgMidLine; @@ -358,6 +385,18 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { line_output_pub_.publish(msgMidLine); } +void LineDetection::classifyHorizontalLine(Line &line, float worldDistToMiddleLine) { + if(worldDistToMiddleLine < -lineWidth_) { + line.lineType_ = Line::LineType::HORIZONTAL_OUTER_LEFT; + } else if(worldDistToMiddleLine < 0) { + line.lineType_ = Line::LineType::HORIZONTAL_LEFT_LANE; + } else if(worldDistToMiddleLine < lineWidth_) { + line.lineType_ = Line::LineType::HORIZONTAL_RIGHT_LANE; + } else { + line.lineType_ = Line::LineType::HORIZONTAL_OUTER_RIGHT; + } +} + void LineDetection::buildBbAroundLines(std::vector ¢erPoints, std::vector &midLinePoints) { auto rect = cv::minAreaRect(centerPoints); cv::Point2f a, b; @@ -382,6 +421,27 @@ void LineDetection::buildBbAroundLines(std::vector ¢erPoints, s } } +void LineDetection::drawAndPublishDebugLines(std::vector &lines) { + for(auto it = lines.begin(); it != lines.end(); ++it) { + if(it->lineType_ == Line::LineType::MIDDLE_LINE) + cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); // Green + else if((it->lineType_ == Line::LineType::LEFT_LINE) || (it->lineType_ == Line::LineType::RIGHT_LINE)) + cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); // Blue + else if(it->lineType_ == Line::LineType::STOP_LINE) + cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(255), 2, cv::LINE_AA); // Red + else if(it->lineType_ == Line::LineType::START_LINE) + cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(0, 204, 255), 2, cv::LINE_AA); // Cyan + else if(it->lineType_ == Line::LineType::HORIZONTAL_OUTER_LEFT) + cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(255, 255), 2, cv::LINE_AA); // Yellow + else if(it->lineType_ == Line::LineType::HORIZONTAL_OUTER_RIGHT) + cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(255, 153, 51), 2, cv::LINE_AA); // Orange + else if(it->lineType_ == Line::LineType::GUESS) + cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(153, 0, 204), 2, cv::LINE_AA); // Purple + else + cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(255, 51, 204), 2, cv::LINE_AA); // Pink + } +} + /// /// \brief LineDetection::reconfigureCB /// Used by the dynamic reconfigure server From e372601eef0a0dff8028f8078e0d9a579ac62927 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Wed, 4 Jul 2018 11:46:40 +0200 Subject: [PATCH 86/94] Detect intersection --- launch/warp_image.launch | 2 +- src/line_detection.cpp | 99 +++++++++++++++++++++++++++++++--------- 2 files changed, 78 insertions(+), 23 deletions(-) diff --git a/launch/warp_image.launch b/launch/warp_image.launch index b99dff0..c27ef06 100644 --- a/launch/warp_image.launch +++ b/launch/warp_image.launch @@ -3,7 +3,7 @@ - + diff --git a/src/line_detection.cpp b/src/line_detection.cpp index 314d5c2..685e2cf 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -75,7 +75,7 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { } else if(driveState == DriveState::Parking) { // findLaneSimple(linesInImage); } else if(driveState == DriveState::Intersection) { -// findIntersectionExit(linesInImage); + findLaneAdvanced(linesInImage); } else { } @@ -150,39 +150,57 @@ void LineDetection::findLinesWithHough(CvImagePtr img, std::vector &houghL /// bool LineDetection::findLaneAdvanced(std::vector &lines) { // Last frames middle line is our new guess. Clear old middle line - std::vector currentGuess_; + std::vector currentGuess; if(!currentMiddleLine_.empty()) { - for(auto l : currentMiddleLine_) { - l.lineType_ = Line::LineType::GUESS; - currentGuess_.push_back(l); + if(driveState == DriveState::Street) { + for(auto l : currentMiddleLine_) { + l.lineType_ = Line::LineType::GUESS; + currentGuess.push_back(l); + } + currentMiddleLine_.clear(); + } else if(driveState == DriveState::Intersection) { + // TODO: as soon as we find the intersection, build lane based on middle and right lane and continue it + // or always have this available as a backup and use it whenever necessary + ROS_INFO_STREAM("currentMiddleLine_.size() = " << currentMiddleLine_.size()); + auto startPt = lines.at(0).wP1_; + auto endPt = lines.at(lines.size() - 1).wP1_; + auto dir = endPt - startPt; + auto newEndPt = startPt + (dir * (2.0 / norm(dir))); + std::vector imagePoints, worldPoints; + worldPoints.push_back(startPt); + worldPoints.push_back(newEndPt); + image_operator_.worldToImage(worldPoints, imagePoints); + currentMiddleLine_.clear(); + auto l = Line(imagePoints.at(0), imagePoints.at(1), worldPoints.at(0), worldPoints.at(1), Line::LineType::GUESS); + currentGuess.push_back(l); + currentMiddleLine_.push_back(l); } - currentMiddleLine_.clear(); } // todo: we should only do this, when the car starts in the start box. Otherwise we can get in trouble std::vector imagePoints, worldPoints; - if(currentGuess_.empty()) { + if(currentGuess.empty()) { // todo: we should have a state like "RECOVER", where we try to find the middle line again (more checks, if it is really the middle one) - worldPoints.push_back(cv::Point2f(0.24, 0.5)); - worldPoints.push_back(cv::Point2f(0.4, 0.5)); - worldPoints.push_back(cv::Point2f(0.55, 0.5)); - worldPoints.push_back(cv::Point2f(0.70, 0.5)); + worldPoints.push_back(cv::Point2f(0.24, 0.15)); + worldPoints.push_back(cv::Point2f(0.4, 0.15)); + worldPoints.push_back(cv::Point2f(0.55, 0.15)); + worldPoints.push_back(cv::Point2f(0.7, 0.15)); image_operator_.worldToImage(worldPoints, imagePoints); for(size_t i = 1; i < imagePoints.size(); i++) - currentGuess_.push_back(Line(imagePoints.at(i - 1), imagePoints.at(i), worldPoints.at(i - 1), worldPoints.at(i), Line::LineType::GUESS)); + currentGuess.push_back(Line(imagePoints.at(i - 1), imagePoints.at(i), worldPoints.at(i - 1), worldPoints.at(i), Line::LineType::GUESS)); } - drawAndPublishDebugLines(currentGuess_); + drawAndPublishDebugLines(currentGuess); // Use the guess to classify the lines std::vector leftLines, middleLines, rightLines, horizontalLines; for(Line sl : lines) { - Line segment = currentGuess_.at(0); + Line segment = currentGuess.at(0); bool isInSegment = false; // find the corresponding segment // todo: this only uses the x-coordinate right now, improve this - for(size_t i = 0; (i < currentGuess_.size()) && !isInSegment; i++) { - segment = currentGuess_.at(i); + for(size_t i = 0; (i < currentGuess.size()) && !isInSegment; i++) { + segment = currentGuess.at(i); if(sl.wMid_.x > segment.wP1_.x) { if(sl.wMid_.x < segment.wP2_.x) { isInSegment = true; @@ -210,7 +228,7 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { leftLines.push_back(sl); } } else { - classifyHorizontalLine(sl, segment.wMid_.y - sl.wMid_.y); + classifyHorizontalLine(sl, sl.wMid_.y - segment.wMid_.y); horizontalLines.push_back(sl); } } @@ -231,9 +249,11 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { } } // Find stop line + bool checkStopLineDist = false; for(auto firstIt = horizontalLines.begin(); firstIt != horizontalLines.end(); ++firstIt) { if(firstIt->lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE) { bool foundRightOuterBound = false, foundLeftOuterBound = false; + // TODO: check is outer lines are on the same height as the candidate stop line for(auto secondIt = horizontalLines.begin(); secondIt != horizontalLines.end(); ++secondIt) { if(secondIt->lineType_ == Line::LineType::HORIZONTAL_OUTER_LEFT) foundLeftOuterBound = true; @@ -242,11 +262,43 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { } if(foundLeftOuterBound && foundRightOuterBound) { firstIt->lineType_ = Line::LineType::STOP_LINE; -// ROS_INFO_STREAM("Found a stop line"); + checkStopLineDist = true; } } } + // Search for intersection exit + if(driveState == DriveState::Intersection) { + if((middleLines.size() > 0) && (leftLines.size() > 0) && (rightLines.size() > 0)) { + driveState = DriveState::Street; + } else { + return true; + } + } + + // Check if we want to switch to DriveState::Intersection + // TODO: improvement: check if all found stop lines are close to each other + if(checkStopLineDist) { +// ROS_INFO_STREAM("--------------- checkStopLineDist"); + bool foundOuterLeft = false, foundOuterRight; + float distToStopLine = 10.0f; + for(auto it = horizontalLines.begin(); it != horizontalLines.end(); ++it) { + if(it->lineType_ == Line::LineType::HORIZONTAL_OUTER_LEFT) + foundOuterLeft = true; + else if(it->lineType_ == Line::LineType::HORIZONTAL_OUTER_RIGHT) + foundOuterRight = true; + else if(it->lineType_ == Line::LineType::STOP_LINE) { +// ROS_INFO_STREAM(it->wMid_.x << "," << it->wMid_.y); + distToStopLine = std::min(distToStopLine, it->wMid_.x); + } + } +// ROS_INFO_STREAM("Dist to intersection = " << distToStopLine); + if(foundOuterLeft && foundOuterRight && (distToStopLine < 0.4)) { + driveState = DriveState::Intersection; + ROS_INFO_STREAM("DriveState changed to Intersection"); + } + } + #ifdef PUBLISH_DEBUG // drawAndPublishDebugLines(middleLines); drawAndPublishDebugLines(rightLines); @@ -254,6 +306,8 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { drawAndPublishDebugLines(horizontalLines); #endif + // TODO: project left and right line into middle - cloud also help for intersections + if(middleLines.empty()/* && leftLines.empty() && rightLines.empty()*/) { // TODO: this should not happen. maybe set state to sth. like RECOVER ROS_WARN_STREAM("No middle line found"); @@ -386,14 +440,15 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { } void LineDetection::classifyHorizontalLine(Line &line, float worldDistToMiddleLine) { + // remember the worlds origin is the vehicles rear_middle_axis and the x-axis increases to the left if(worldDistToMiddleLine < -lineWidth_) { - line.lineType_ = Line::LineType::HORIZONTAL_OUTER_LEFT; + line.lineType_ = Line::LineType::HORIZONTAL_OUTER_RIGHT; } else if(worldDistToMiddleLine < 0) { - line.lineType_ = Line::LineType::HORIZONTAL_LEFT_LANE; - } else if(worldDistToMiddleLine < lineWidth_) { line.lineType_ = Line::LineType::HORIZONTAL_RIGHT_LANE; + } else if(worldDistToMiddleLine < lineWidth_) { + line.lineType_ = Line::LineType::HORIZONTAL_LEFT_LANE; } else { - line.lineType_ = Line::LineType::HORIZONTAL_OUTER_RIGHT; + line.lineType_ = Line::LineType::HORIZONTAL_OUTER_LEFT; } } From 8f7981413d183d1817e1d3530b4b304da763fe71 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Wed, 25 Jul 2018 13:33:03 +0200 Subject: [PATCH 87/94] Handle intersections * Detect start of intersection with stop lines * WIP: find lane after intersection --- .../line_detection.h | 1 + src/line_detection.cpp | 56 +++++++++++++++++-- 2 files changed, 53 insertions(+), 4 deletions(-) diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index cf956a4..de3d7ad 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -57,6 +57,7 @@ class LineDetection { #ifdef PUBLISH_DEBUG cv::Mat debugImg_; #endif + int stopLineCount; // communication image_transport::ImageTransport imageTransport_; diff --git a/src/line_detection.cpp b/src/line_detection.cpp index 314d5c2..c1bb690 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -16,6 +16,7 @@ LineDetection::LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh , isObstaceCourse(false) , driveState(DriveState::Street) // TODO: this should be StartBox in real conditions , imageTransport_(pnh) + , stopLineCount(0) , lineWidth_(0.4) , image_operator_() , dsrv_server_() @@ -75,13 +76,17 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { } else if(driveState == DriveState::Parking) { // findLaneSimple(linesInImage); } else if(driveState == DriveState::Intersection) { -// findIntersectionExit(linesInImage); + findIntersectionExit(linesInImage); } else { } #ifdef PUBLISH_DEBUG debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, debugImg_).toImageMsg()); + + cv::namedWindow("Debug Image", CV_WINDOW_NORMAL); + cv::imshow("Debug Image", debugImg_); + cv::waitKey(1); #endif // auto t_start = std::chrono::high_resolution_clock::now(); @@ -121,7 +126,6 @@ void LineDetection::findLinesWithHough(CvImagePtr img, std::vector &houghL // Get houghlines std::vector hLinePoints; - cv::HoughLinesP(processingImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); // Extract points for houghLines and convert to world-coordinates @@ -231,6 +235,7 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { } } // Find stop line + std::vector distsToStopLine; for(auto firstIt = horizontalLines.begin(); firstIt != horizontalLines.end(); ++firstIt) { if(firstIt->lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE) { bool foundRightOuterBound = false, foundLeftOuterBound = false; @@ -242,10 +247,24 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { } if(foundLeftOuterBound && foundRightOuterBound) { firstIt->lineType_ = Line::LineType::STOP_LINE; -// ROS_INFO_STREAM("Found a stop line"); + distsToStopLine.push_back(firstIt->wMid_.x); } } } + if(distsToStopLine.size() > 2) { + auto avgDistToStopLine = std::accumulate(distsToStopLine.begin(), distsToStopLine.end(), 0.0f) / distsToStopLine.size(); + ROS_INFO_STREAM("Found a stop line; distance to it = " << avgDistToStopLine); + stopLineCount++; + if(avgDistToStopLine <= 0.6f) { // TODO: magic number + + if(stopLineCount > 5) { + driveState = DriveState::Intersection; + } + // todo: maybe return here? but do not forget to publish road points + } + } else { + stopLineCount = 0; + } #ifdef PUBLISH_DEBUG // drawAndPublishDebugLines(middleLines); @@ -256,7 +275,6 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { if(middleLines.empty()/* && leftLines.empty() && rightLines.empty()*/) { // TODO: this should not happen. maybe set state to sth. like RECOVER - ROS_WARN_STREAM("No middle line found"); return false; } @@ -370,6 +388,7 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { // publish points to road topic // TODO: we want to publish not only the middle line, but also start line, stop lines, intersections, barred areas, ... + // todo: we should do this outside this function inside the imageCallback drive_ros_msgs::RoadLine msgMidLine; msgMidLine.lineType = drive_ros_msgs::RoadLine::MIDDLE; for(auto l : currentMiddleLine_) { @@ -385,6 +404,35 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { line_output_pub_.publish(msgMidLine); } +bool LineDetection::findIntersectionExit(std::vector &lines) { + drawAndPublishDebugLines(currentMiddleLine_); + std::vector potMidLines; + + // we keep current current middle line + // todo: we should check if the middle line is plausible + ROS_INFO_STREAM("findIntersectionExit"); + if(currentMiddleLine_.size() == 0) { + ROS_WARN("No middle line"); + return false; + } + for(auto l : lines) { +// if(std::abs(l.wMid_.y - currentMiddleLine_.at(0).wMid_.y) < (lineWidth_ * 0.5)) { +// ROS_INFO_STREAM(" found potential new middle line"); + if(std::abs(l.getAngle() - currentMiddleLine_.at(0).getAngle()) < (M_PI / 8)) { + ROS_INFO_STREAM(" angle is ok"); + l.lineType_ = Line::LineType::MIDDLE_LINE; + potMidLines.push_back(l); + } +// } + } + // todo: group lines by areas (left, middle, right) to find our lane again + drawAndPublishDebugLines(potMidLines); + + // todo: ideas: 1) detect opposite stop line as end of intersection + // 2) usu odometry and knowledge about intersection size (line_width * 2) + return true; +} + void LineDetection::classifyHorizontalLine(Line &line, float worldDistToMiddleLine) { if(worldDistToMiddleLine < -lineWidth_) { line.lineType_ = Line::LineType::HORIZONTAL_OUTER_LEFT; From 00b941da56c4550b6b4e96ea5f854ae9c0a31195 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Mon, 30 Jul 2018 23:28:45 +0200 Subject: [PATCH 88/94] Project middle lines to mid and find polynomial fit * Currently only used for finding the way through an intersection without own stop line --- src/line_detection.cpp | 111 ++++++++++++++++++++++++++++++++++++++--- 1 file changed, 103 insertions(+), 8 deletions(-) diff --git a/src/line_detection.cpp b/src/line_detection.cpp index 4d3be59..d20ebdb 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -4,10 +4,36 @@ #include #include #include +#include #include "drive_ros_image_recognition/line_detection.h" #include "drive_ros_image_recognition/geometry_common.h" #include "drive_ros_msgs/RoadLine.h" +// TODO: if we use this, we have to cite it: https://github.com/kipr/opencv/blob/master/modules/contrib/src/polyfit.cpp +void polyfit(const cv::Mat& src_x, const cv::Mat& src_y, cv::Mat& dst, int order) +{ + CV_Assert((src_x.rows>0)&&(src_y.rows>0)&&(src_x.cols==1)&&(src_y.cols==1) + &&(dst.cols==1)&&(dst.rows==(order+1))&&(order>=1)); + cv::Mat X; + X = cv::Mat::zeros(src_x.rows, order+1,CV_32FC1); + cv::Mat copy; + for(int i = 0; i <=order;i++) + { + copy = src_x.clone(); + cv::pow(copy,i,copy); + cv::Mat M1 = X.col(i); + copy.col(0).copyTo(M1); + } + cv::Mat X_t, X_inv; + transpose(X,X_t); + cv::Mat temp = X_t*X; + cv::Mat temp2; + invert (temp,temp2); + cv::Mat temp3 = temp2*X_t; + cv::Mat W = temp3*src_y; + W.copyTo(dst); +} + namespace drive_ros_image_recognition { LineDetection::LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh) @@ -165,11 +191,12 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { // todo: we should only do this, when the car starts in the start box. Otherwise we can get in trouble std::vector imagePoints, worldPoints; if(currentGuess.empty()) { + ROS_WARN("Dummy guess"); // todo: we should have a state like "RECOVER", where we try to find the middle line again (more checks, if it is really the middle one) - worldPoints.push_back(cv::Point2f(0.24, 0.5)); - worldPoints.push_back(cv::Point2f(0.4, 0.5)); - worldPoints.push_back(cv::Point2f(0.55, 0.5)); - worldPoints.push_back(cv::Point2f(0.70, 0.5)); + worldPoints.push_back(cv::Point2f(0.24, 0.2)); + worldPoints.push_back(cv::Point2f(0.4, 0.2)); + worldPoints.push_back(cv::Point2f(0.55, 0.2)); + worldPoints.push_back(cv::Point2f(0.70, 0.2)); image_operator_.worldToImage(worldPoints, imagePoints); for(size_t i = 1; i < imagePoints.size(); i++) currentGuess.push_back(Line(imagePoints.at(i - 1), imagePoints.at(i), worldPoints.at(i - 1), worldPoints.at(i), Line::LineType::GUESS)); @@ -248,9 +275,77 @@ bool LineDetection::findLaneAdvanced(std::vector &lines) { publishDebugLines(horizontalLines); #endif - if(middleLines.empty()/* && leftLines.empty() && rightLines.empty()*/) { - // TODO: this should not happen. maybe set state to sth. like RECOVER - return false; + if(middleLines.empty()) { + // this can happen when approach an intersection without a stop line + ROS_INFO("------------- no middle line found"); + worldPoints.clear(); + imagePoints.clear(); + for(auto l : rightLines) { + // perpendiculare vector + Eigen::Vector2f vec(-(l.wP2_.y - l.wP1_.y), + (l.wP2_.x - l.wP1_.x)); + vec.normalize(); + vec *= lineWidth_; + worldPoints.push_back(cv::Point2f(l.wP1_.x + vec(0), l.wP1_.y + vec(1))); + worldPoints.push_back(cv::Point2f(l.wP2_.x + vec(0), l.wP2_.y + vec(1))); +// ROS_INFO_STREAM("Moved (" << l.wP1_.x << "," << l.wP1_.y << ") to (" << (l.wP1_.x + vec(0)) << "," << (l.wP1_.y + vec(1)) << ")"); + } + for(auto l : leftLines) { + // perpendiculare vector + Eigen::Vector2f vec((l.wP2_.y - l.wP1_.y), + -(l.wP2_.x - l.wP1_.x)); + vec.normalize(); + vec *= lineWidth_; + worldPoints.push_back(cv::Point2f(l.wP1_.x + vec(0), l.wP1_.y + vec(1))); + worldPoints.push_back(cv::Point2f(l.wP2_.x + vec(0), l.wP2_.y + vec(1))); +// ROS_INFO_STREAM("Moved (" << l.wP1_.x << "," << l.wP1_.y << ") to (" << (l.wP1_.x + vec(0)) << "," << (l.wP1_.y + vec(1)) << ")"); + } + + if(worldPoints.empty()) + return false; + + int order = 1; + cv::Mat srcX(worldPoints.size(), 1, CV_32FC1); + cv::Mat srcY(worldPoints.size(), 1, CV_32FC1); +// (src_x.rows>0)&&(src_y.rows>0)&&(src_x.cols==1) +// (src_y.cols==1) +// (dst.cols==1)&&(dst.rows==(order+1))&&(order>=1) + cv::Mat dst(order+1, 1, CV_32FC1); + + +// image_operator_.worldToImage(worldPoints, imagePoints); + for(int i = 0; i < worldPoints.size(); i += 2) { +// middleLines.push_back(Line(imagePoints.at(i), imagePoints.at(i+1), worldPoints.at(i), worldPoints.at(i+1), Line::LineType::MIDDLE_LINE)); + srcX.at(i, 0) = worldPoints.at(i).x; + srcX.at(i+1, 0) = worldPoints.at(i+1).x; + srcY.at(i, 0) = worldPoints.at(i).y; + srcY.at(i+1, 0) = worldPoints.at(i+1).y; + } + + ROS_INFO_STREAM("srcX dims = " << srcX.size << "; srcY dims = " << srcY.size << "; dst dims = " << dst.size); + polyfit(srcX, srcY, dst, order); + ROS_INFO_STREAM("PolyFit = " << dst); + + worldPoints.clear(); + auto a = dst.at(0,0); + auto b = dst.at(1,0); + + for(int i = 1; i < 8; i++) { + auto x = i * 0.3; + worldPoints.push_back(cv::Point2f(x, a*x + b)); + } + + imagePoints.clear(); + currentMiddleLine_.clear(); + image_operator_.worldToImage(worldPoints, imagePoints); + for(int i = 1; i < worldPoints.size(); i++) { + currentMiddleLine_.push_back(Line(imagePoints.at(i-1), imagePoints.at(i), worldPoints.at(i-1), worldPoints.at(i), Line::LineType::MIDDLE_LINE)); + ROS_INFO_STREAM("World line: (" << worldPoints.at(i-1).x << "," << worldPoints.at(i-1).y << ") - (" << + worldPoints.at(i).x << "," << worldPoints.at(i).y << ")"); + } + + publishDebugLines(currentMiddleLine_); + return true; } // ------------------------------------------------------------------------------------------------------------------------------------ @@ -453,7 +548,7 @@ void LineDetection::determineLineTypes(std::vector &lines, std::vectorwMid_.y - segment.wMid_.y); - if(absDistanceToMidLine < (lineWidth_ / 2)) { + if(absDistanceToMidLine < (lineWidth_ * 0.33)) { lineIt->lineType_ = Line::LineType::MIDDLE_LINE; } else if((lineIt->wMid_.y < segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { lineIt->lineType_ = Line::LineType::RIGHT_LINE; From 2bb8aa6c3a2dac5bab750fe203580d098cf05d92 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Tue, 20 Nov 2018 18:11:43 +0100 Subject: [PATCH 89/94] Change lane detection to segment based detection * Currently implemented for no-obstacle-course * Recognized the lane and intersections (stop and no-stop) * TODO: start line detection, use odometry, compare new lane to previous one --- CMakeLists.txt | 3 +- cfg/LineDetection.cfg | 8 +- config/homography.yaml | 2 +- .../common_image_operations.h | 27 +- include/drive_ros_image_recognition/line.h | 31 +- .../line_detection.h | 51 +- .../drive_ros_image_recognition/road_model.h | 57 + src/line_detection.cpp | 1051 +++++++++-------- src/road_model.cpp | 48 + 9 files changed, 720 insertions(+), 558 deletions(-) create mode 100644 include/drive_ros_image_recognition/road_model.h create mode 100644 src/road_model.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 04b3cdf..bdab6c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -48,7 +48,7 @@ include_directories( ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} - ) +) ################################################################################ # Warp image node @@ -98,6 +98,7 @@ install(TARGETS warp_image_nodelet add_executable(line_detection_node src/line_detection.cpp src/line_detection_node.cpp + src/road_model.cpp ) add_dependencies(line_detection_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/cfg/LineDetection.cfg b/cfg/LineDetection.cfg index 8e7b5a5..86195ad 100755 --- a/cfg/LineDetection.cfg +++ b/cfg/LineDetection.cfg @@ -8,11 +8,13 @@ gen = ParameterGenerator() gen.add("lineWidth", double_t, 0, "Line width [m]", 0.45, 0.1, 1.0) gen.add("lineVar", double_t, 0, "Line search corridor [m]", 0.1, 0.01, 0.20) gen.add("lineAngle", double_t, 0, "Max line angle (rad)", 0.7, 0.1, 3.1) -gen.add("maxSenseRange", double_t, 0, "Max view range for lines", 1.6, 0.5, 5.0) +gen.add("maxSenseRange", double_t, 0, "Max view range for lines", 2.0, 0.5, 5.0) gen.add("cannyThreshold", int_t, 0, "cannyThreshold", 30, 10, 500) gen.add("houghThreshold", int_t, 0, "houghThreshold", 20, 5, 100) -gen.add("houghMinLineLen", int_t, 0, "houghMinLineLen", 20, 5, 100) -gen.add("houghMaxLineGap", int_t, 0, "houghMaxLineGap", 10, 0, 300) +gen.add("houghMinLineLen", int_t, 0, "houghMinLineLen", 5, 1, 100) +gen.add("houghMaxLineGap", int_t, 0, "houghMaxLineGap", 3, 0, 300) +gen.add("segmentLength", double_t, 0, "Classification segment length", 0.2, 0.1, 1.0) +gen.add("ransacIterations", int_t, 0, "Max number of Ransac iterations", 200, 50, 1000) exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "LineDetection")) diff --git a/config/homography.yaml b/config/homography.yaml index 66b2425..d322600 100644 --- a/config/homography.yaml +++ b/config/homography.yaml @@ -1,2 +1,2 @@ world_size: [5.0, 5.0] -image_size: [512, 512] +image_size: [800, 800] diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 5ec11d9..089ef94 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -253,8 +253,6 @@ class TransformHelper { /// /// \brief imageToWorld /// Converts all imagePoints to the corresponding worldPoints using the cam2world matrix. - /// Important: x and y switches when converted. The origin is on the camera mount point on the ground. - /// The y-cooridnate goes to the front of the car, the x-coordinate to the left. /// \param imagePoints /// \param worldPoints /// \return true on success, false on failure. @@ -274,8 +272,6 @@ class TransformHelper { /// /// \brief worldToImage /// Converts all worldPoints to the corresponding imagePoints using the world2cam matrix. - /// Important: x and y switches when converted. The origin is on the camera mount point on the ground. - /// The y-cooridnate goes to the front of the car, the x-coordinate to the left. /// \param worldPoints /// \param imagePoints /// \return true on success, false on failure. @@ -325,6 +321,29 @@ class TransformHelper { return true; } + bool warpedImgToCamera(std::vector &warpedImgPoints, std::vector &cameraPoints) { + if(!homography_received_) { + ROS_WARN_STREAM("[warpedImgToCamera] Homography not received yet"); + return false; + } + cv::perspectiveTransform(warpedImgPoints, cameraPoints, scaledWorld2cam_); + return true; + } + + bool worldToWarpedImg(std::vector &worldPts, std::vector &warpedImgPts) { + std::vector tmp; + worldToImage(worldPts, tmp); + cameraToWarpedImg(tmp, warpedImgPts); + return true; + } + + bool warpedImgToWorld(std::vector &warpedImgPts, std::vector &worldPts) { + std::vector tmp; + warpedImgToCamera(warpedImgPts, tmp); + imageToWorld(tmp, worldPts); + return true; + } + /* * Simon says: these two methods are not working properly, since we are doing the homography twice (first use the homog matrix * and then the TF conversion. I will leave this in, in case we need the tf version later. diff --git a/include/drive_ros_image_recognition/line.h b/include/drive_ros_image_recognition/line.h index 731a568..85c185f 100644 --- a/include/drive_ros_image_recognition/line.h +++ b/include/drive_ros_image_recognition/line.h @@ -14,26 +14,11 @@ namespace drive_ros_image_recognition { /// class Line { public: - enum LineType { - UNKNOWN = 0, - LEFT_LINE = 1, - MIDDLE_LINE = 2, - RIGHT_LINE = 3, - HORIZONTAL_LEFT_LANE = 4, - HORIZONTAL_RIGHT_LANE = 5, - HORIZONTAL_OUTER_LEFT = 6, - HORIZONTAL_OUTER_RIGHT = 7, - STOP_LINE = 8, - START_LINE = 9, - GUESS = 10 - }; - cv::Point iP1_, iP2_, iMid_; - cv::Point2f wP1_, wP2_, wMid_; - LineType lineType_; + cv::Point2f iP1_, iP2_; + cv::Point2f wP1_, wP2_; - Line(cv::Point iP1, cv::Point iP2, cv::Point2f wP1, cv::Point2f wP2, LineType lineType) - : lineType_(lineType) + Line(cv::Point2i iP1, cv::Point2i iP2, cv::Point2f wP1, cv::Point2f wP2) { if(wP1.x < wP2.x) { iP1_ = iP1; @@ -46,16 +31,12 @@ class Line { wP1_ = wP2; wP2_ = wP1; } - - iMid_ = (iP1_ + iP2_) * .5; - wMid_ = (wP1_ + wP2_) * .5; } - // returns the angle of the lines between 0 and PI - inline double getAngle() { return std::abs(atan2(wP1_.x - wP2_.x, wP1_.y - wP2_.y)); } + // returns the angle of the lines between -PI and PI + inline float getAngle() { return atan2(wP2_.y - wP1_.y, wP2_.x - wP1_.x); } - float getLength() { -// return getDistanceBetweenPoints(wP2_, wP1_); + float getWorldLength() { auto xDir = wP2_.x - wP1_.x; auto yDir = wP2_.y - wP1_.y; return sqrt(xDir * xDir + yDir * yDir); diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index dd472a6..2a510ab 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -14,8 +14,7 @@ #include #include "drive_ros_msgs/RoadLane.h" #include "line.h" - -#define GUESS_RESOLUTION = 0.2 +#include "road_model.h" namespace drive_ros_image_recognition { @@ -25,39 +24,26 @@ class LineDetection { ros::NodeHandle nh_; ros::NodeHandle pnh_; - // drive state enum - enum DriveState { - StartBox = 0, - Parking = 1, - Intersection = 2, - Street = 3 - }; - - // drive mode config - bool isObstaceCourse; // true for Obstacle Evasion Course; false for Free Drive (w/o Obstacles) & Parking - DriveState driveState; - // configs - float lineWidth_; - float lineAngle_; + float laneWidthWorld_; + float lineAngle_; // not used at the moment float lineVar_; - float maxViewRange_; + float maxSenseRange_; int cannyThreshold_; int houghThresold_; int houghMinLineLen_; int houghMaxLineGap_; + float segmentLength_; + size_t maxRansacInterations_; // variables -// CvImagePtr currentImage_; -// std::vector currentGuess_; - std::vector currentMiddleLine_; + RoadModel roadModel; ImageOperator image_operator_; int imgHeight_; int imgWidth_; #ifdef PUBLISH_DEBUG cv::Mat debugImg_; #endif - int stopLineCount; // communication image_transport::ImageTransport imageTransport_; @@ -87,12 +73,8 @@ class LineDetection { dynamic_reconfigure::Server::CallbackType dsrv_cb_; // methods - void findLane(); - void findLinesWithHough(CvImagePtr img, std::vector &houghLines); - bool findLanesFromStartbox(std::vector &lines); // called at the start to initialy find the lanes - bool findLaneSimple(std::vector &lines); // finds the lane assuming there are no markings missing (e.g. for Parking area) - bool findLaneAdvanced(std::vector &lines); // finds the lane assuming some markings could be missing - bool findIntersectionExit(std::vector &lines); // find the end of an intersection (used while in intersection) + void findLinesWithHough(cv::Mat &img, std::vector &houghLines); + void findLaneMarkings(std::vector &lines); // helper functions float getDistanceBetweenPoints(const cv::Point2f a, const cv::Point2f b) { @@ -101,12 +83,19 @@ class LineDetection { return sqrt(dX * dX + dY * dY); } - void determineLineTypes(std::vector &lines, std::vector ¤tGuess); - void classifyHorizontalLine(Line *line, float worldDistToMiddleLine); - void buildBbAroundLines(std::vector ¢erPoints, std::vector &midLinePoints); + std::vector buildRegions(cv::Point2f position, float angle); + void assignLinesToRegions(std::vector *regions, std::vector &lines, + std::vector &leftMarkings, std::vector &midMarkings, + std::vector &rightMarkings, std::vector &otherMarkings); + bool lineIsInRegion(Line *line, const cv::RotatedRect *region, bool isImageCoordiante) const; + bool pointIsInRegion(cv::Point2f *pt, cv::Point2f *edges) const; + Segment findLaneWithRansac(std::vector &leftMarkings, std::vector &midMarkings, std::vector &rightMarkings, cv::Point2f pos, float prevAngle); + bool findIntersection(Segment &resultingSegment, float segmentAngle, cv::Point2f segStartWorld, + std::vector &leftMarkings, std::vector &midMarkings, std::vector &rightMarkings); + bool segmentIsPlausible(Segment *segmentToAdd, bool isFirstSegment); #ifdef PUBLISH_DEBUG - void publishDebugLines(std::vector &lines); + void drawDebugLines(std::vector &lines); #endif public: diff --git a/include/drive_ros_image_recognition/road_model.h b/include/drive_ros_image_recognition/road_model.h new file mode 100644 index 0000000..f4b9891 --- /dev/null +++ b/include/drive_ros_image_recognition/road_model.h @@ -0,0 +1,57 @@ +#ifndef ROAD_MODEL_H +#define ROAD_MODEL_H + +#include +#include +#include "drive_ros_image_recognition/line.h" +#include "drive_ros_image_recognition/common_image_operations.h" + +#define PREDICTION_LENGTH 1.0 + +namespace drive_ros_image_recognition { + +enum SegmentType { + NORMAL_ROAD, INTERSECTION_STOP, INTERSECTION_GO_STRAIGHT +}; + +struct Segment { + cv::Point2f positionWorld; + cv::Point2f positionImage; + cv::Point2f leftPosW, midPosW, rightPosW; // These points lay on the left/mid/right line marking + cv::Point2f leftPosI, midPosI, rightPosI; + float angleDiff; // The angle difference to the previous segment + float angleTotal; // The angle in which the lane is heading to + float length; // This is defined globally and hence will be the same for every segment + float probablity; // Not sure if we use this, yet + SegmentType segmentType = SegmentType::NORMAL_ROAD; + + Segment() + : probablity(0.f) + { + } + + Segment(cv::Point2f worldPos, cv::Point2f imagePos, float angleDiff_, float angleTotal_, float len, float prob) + : positionWorld(worldPos) + , positionImage(imagePos) + , angleDiff(angleDiff_) + , angleTotal(angleTotal_) + , length(len) + , probablity(prob) + { + } + + inline bool isIntersection() const { return segmentType == SegmentType::INTERSECTION_GO_STRAIGHT || + segmentType == SegmentType::INTERSECTION_STOP; }; +}; + +class RoadModel { + std::vector segments; + +public: + void addSegments(std::vector &newSegments); + void getFirstPosW(cv::Point2f &posW, float &angle) const; +}; + +} // namespace drive_ros_image_recognition + +#endif // ROAD_MODEL_H diff --git a/src/line_detection.cpp b/src/line_detection.cpp index d20ebdb..3ff520d 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -5,45 +5,22 @@ #include #include #include +#include #include "drive_ros_image_recognition/line_detection.h" #include "drive_ros_image_recognition/geometry_common.h" #include "drive_ros_msgs/RoadLine.h" -// TODO: if we use this, we have to cite it: https://github.com/kipr/opencv/blob/master/modules/contrib/src/polyfit.cpp -void polyfit(const cv::Mat& src_x, const cv::Mat& src_y, cv::Mat& dst, int order) -{ - CV_Assert((src_x.rows>0)&&(src_y.rows>0)&&(src_x.cols==1)&&(src_y.cols==1) - &&(dst.cols==1)&&(dst.rows==(order+1))&&(order>=1)); - cv::Mat X; - X = cv::Mat::zeros(src_x.rows, order+1,CV_32FC1); - cv::Mat copy; - for(int i = 0; i <=order;i++) - { - copy = src_x.clone(); - cv::pow(copy,i,copy); - cv::Mat M1 = X.col(i); - copy.col(0).copyTo(M1); - } - cv::Mat X_t, X_inv; - transpose(X,X_t); - cv::Mat temp = X_t*X; - cv::Mat temp2; - invert (temp,temp2); - cv::Mat temp3 = temp2*X_t; - cv::Mat W = temp3*src_y; - W.copyTo(dst); -} - namespace drive_ros_image_recognition { LineDetection::LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh) : nh_(nh) , pnh_(pnh) - , isObstaceCourse(false) - , driveState(DriveState::Street) // TODO: this should be StartBox in real conditions , imageTransport_(pnh) - , stopLineCount(0) - , lineWidth_(0.4) + , laneWidthWorld_(0.4) + , lineVar_(0.1) + , segmentLength_(0.2) + , maxSenseRange_(1.6) + , maxRansacInterations_(200) , image_operator_() , dsrv_server_() , dsrv_cb_(boost::bind(&LineDetection::reconfigureCB, this, _1, _2)) @@ -63,14 +40,14 @@ bool LineDetection::init() { dsrv_server_.setCallback(dsrv_cb_); //subscribe to camera image - imageSubscriber_ = imageTransport_.subscribe("/img_in", 10, &LineDetection::imageCallback, this); + imageSubscriber_ = imageTransport_.subscribe("/img_in", 3, &LineDetection::imageCallback, this); ROS_INFO_STREAM("Subscribed image transport to topic " << imageSubscriber_.getTopic()); line_output_pub_ = nh_.advertise("roadLine", 10); ROS_INFO_STREAM("Advertising road line on " << line_output_pub_.getTopic()); #ifdef PUBLISH_DEBUG - debugImgPub_ = imageTransport_.advertise("debug_image", 10); + debugImgPub_ = imageTransport_.advertise("debug_image", 3); ROS_INFO_STREAM("Publishing debug image on topic " << debugImgPub_.getTopic()); #endif @@ -91,27 +68,25 @@ bool LineDetection::init() { void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { // TODO: so far these states are only for the free drive (w/o obstacles) and probably not even complete, yet auto currentImage = convertImageMessage(imgIn); - imgHeight_ = currentImage->rows; - imgWidth_ = currentImage->cols; + cv::Mat homographedImg; + if(!image_operator_.homographImage(*currentImage, homographedImg)) { + ROS_WARN("Homographing image failed"); + return; + } + imgHeight_ = homographedImg.rows; + imgWidth_ = homographedImg.cols; + +#ifdef PUBLISH_DEBUG + cv::cvtColor(homographedImg, debugImg_, CV_GRAY2RGB); +#endif + std::vector linesInImage; - findLinesWithHough(currentImage, linesInImage); - if(driveState == DriveState::StartBox) { - // findLanesFromStartbox(linesInImage); - } else if(driveState == DriveState::Street) { - findLaneAdvanced(linesInImage); - } else if(driveState == DriveState::Parking) { - // findLaneSimple(linesInImage); - } else if(driveState == DriveState::Intersection) { - findIntersectionExit(linesInImage); - } else { + findLinesWithHough(homographedImg, linesInImage); - } + findLaneMarkings(linesInImage); #ifdef PUBLISH_DEBUG debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, debugImg_).toImageMsg()); - // cv::namedWindow("Debug Image", CV_WINDOW_NORMAL); - // cv::imshow("Debug Image", debugImg_); - // cv::waitKey(1); #endif // auto t_start = std::chrono::high_resolution_clock::now(); @@ -120,503 +95,591 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { // ROS_INFO_STREAM("Cycle time: " << (std::chrono::duration(t_end-t_start).count()) << "ms"); } -/// -/// \brief LineDetection::findLinesWithHough -/// Extract Lines from the image using Hough Lines -/// \param img A CvImagePtr to the current image where we want to search for lines -/// \param houghLines A std::vector where the Lines will be returned. Has to be empty. -/// -void LineDetection::findLinesWithHough(CvImagePtr img, std::vector &houghLines) { - cv::Mat processingImg; -#ifdef PUBLISH_DEBUG - cv::cvtColor(*img, debugImg_, CV_GRAY2RGB); -#endif +void LineDetection::findLaneMarkings(std::vector &lines) { + cv::Point2f segStartWorld(0.3f, 0.f); + float segAngle = 0.f; + float totalSegLength = 0.f; + bool findIntersectionExit = false; + std::vector foundSegments; + std::vector unusedLines, leftMarkings, midMarkings, rightMarkings, otherMarkings; + std::vector regions; + +// ROS_INFO("------------- New image ------------------"); + for(int i = 0; i < lines.size(); i++) { + unusedLines.push_back(&lines.at(i)); + } - // Blur the image and find edges with Canny - cv::GaussianBlur(*img, processingImg, cv::Size(15, 15), 0, 0); - cv::Canny(processingImg, processingImg, cannyThreshold_, cannyThreshold_ * 3, 3); + roadModel.getFirstPosW(segStartWorld, segAngle); + while((totalSegLength < maxSenseRange_) || findIntersectionExit) { + // clear the marking vectors. Otherwise the old ones stay in there. + leftMarkings.clear(); + midMarkings.clear(); + rightMarkings.clear(); + otherMarkings.clear(); + + regions = buildRegions(segStartWorld, segAngle); + assignLinesToRegions(®ions, unusedLines, leftMarkings, midMarkings, rightMarkings, otherMarkings); + auto seg = findLaneWithRansac(leftMarkings, midMarkings, rightMarkings, segStartWorld, segAngle); + if(segmentIsPlausible(&seg, foundSegments.empty())) { + foundSegments.push_back(seg); + segAngle = seg.angleTotal; // the current angle of the lane + } else { + break; + } - // Zero out the vehicle - int imgWidth = processingImg.cols; - int imgHeight = processingImg.rows; - cv::Point vehiclePolygon[1][4]; - // TODO: these factor should be in a config - vehiclePolygon[0][0] = cv::Point(imgWidth * .35, imgHeight); - vehiclePolygon[0][1] = cv::Point(imgWidth * .65, imgHeight); - vehiclePolygon[0][2] = cv::Point(imgWidth * .65, imgHeight * .8); - vehiclePolygon[0][3] = cv::Point(imgWidth * .35, imgHeight * .8); - const cv::Point* polygonStarts[1] = { vehiclePolygon[0] }; - int polygonLengths[] = { 4 }; - cv::fillPoly(processingImg, polygonStarts, polygonLengths, 1, cv::Scalar(), cv::LINE_8); + totalSegLength += seg.length; + // Move the segment start forward + segStartWorld.x = seg.positionWorld.x + cos(seg.angleTotal) * seg.length; + segStartWorld.y = seg.positionWorld.y + sin(seg.angleTotal) * seg.length; + + + // ================================ + // Search for a stop line + // ================================ + float stopLineAngle; + Segment intersectionSegment; + findIntersectionExit = false; + if(findIntersection(intersectionSegment, segAngle, segStartWorld, leftMarkings, midMarkings, rightMarkings)) { + if(segmentIsPlausible(&intersectionSegment, false)) { + foundSegments.push_back(intersectionSegment); + totalSegLength += intersectionSegment.length; + segAngle = intersectionSegment.angleTotal; // the current angle of the lane + // Move the segment start forward + segStartWorld.x = intersectionSegment.positionWorld.x + cos(intersectionSegment.angleTotal) * intersectionSegment.length; + segStartWorld.y = intersectionSegment.positionWorld.y + sin(intersectionSegment.angleTotal) * intersectionSegment.length; + findIntersectionExit = true; + } else { + ROS_WARN("Intersection segment not plausible"); + break; + } + } - // Get houghlines - std::vector hLinePoints; - cv::HoughLinesP(processingImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); + // only used unused lines for the next iteration + unusedLines = otherMarkings; + } - // Extract points for houghLines and convert to world-coordinates - std::vector imagePoints, worldPoints; - for(size_t i = 0; i < hLinePoints.size(); i++) { - cv::Vec4i currentPoints = hLinePoints.at(i); - imagePoints.push_back(cv::Point(currentPoints[0], currentPoints[1])); - imagePoints.push_back(cv::Point(currentPoints[2], currentPoints[3])); +// ROS_INFO("Detection range = %.2f", totalSegLength); + roadModel.addSegments(foundSegments); + +#ifdef PUBLISH_DEBUG + if(foundSegments.size() > 0) { + // ego lane in orange + cv::circle(debugImg_, foundSegments.at(0).positionImage, 5, cv::Scalar(255,128), 2, cv::LINE_AA); + // left lane marking in purple + cv::circle(debugImg_, foundSegments.at(0).leftPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); + // right lane marking in purple + cv::circle(debugImg_, foundSegments.at(0).rightPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); + // mid lane marking in yellow + cv::circle(debugImg_, foundSegments.at(0).midPosI, 3, cv::Scalar(255,255), 1, cv::LINE_AA); } - image_operator_.imageToWorld(imagePoints, worldPoints); - // Build lines from points to return them - for(size_t i = 0; i < worldPoints.size(); i += 2) { - houghLines.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1), Line::LineType::UNKNOWN)); + for(int i = 1; i < foundSegments.size(); i++) { + // ego lane in orange + cv::circle(debugImg_, foundSegments.at(i).positionImage, 5, cv::Scalar(255,128), 2, cv::LINE_AA); + cv::line(debugImg_, foundSegments.at(i-1).positionImage, foundSegments.at(i).positionImage, cv::Scalar(255,128), 2, cv::LINE_AA); + if(!foundSegments.at(i).isIntersection() && !foundSegments.at(i-1).isIntersection()) { + // left lane marking in purple + cv::line(debugImg_, foundSegments.at(i-1).leftPosI, foundSegments.at(i).leftPosI, cv::Scalar(153,0,204), 2, cv::LINE_AA); + cv::circle(debugImg_, foundSegments.at(i).leftPosI, 3, cv::Scalar(153, 0, 204), 1, cv::LINE_AA); + // right lane marking in purple + cv::line(debugImg_, foundSegments.at(i-1).rightPosI, foundSegments.at(i).rightPosI, cv::Scalar(153,0,204), 2, cv::LINE_AA); + cv::circle(debugImg_, foundSegments.at(i).rightPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); + // mid lane marking in yellow + cv::line(debugImg_, foundSegments.at(i-1).midPosI, foundSegments.at(i).midPosI, cv::Scalar(255,255), 2, cv::LINE_AA); + cv::circle(debugImg_, foundSegments.at(i).midPosI, 3, cv::Scalar(255,255), 1, cv::LINE_AA); + } } - return; +// for(auto m : unusedLines) { +// cv::line(debugImg_, m->iP1_, m->iP2_, cv::Scalar(0,180), 2, cv::LINE_AA); +// } +#endif } /// -/// \brief LineDetection::findLaneAdvanced -/// In this scenario lines can be missing, we could have a double middle line, intersection lines, -/// and the start line could occur. -/// \param lines -/// \return +/// \brief LineDetection::buildRegions +/// \param position Position of the segment in world coordinates +/// \param angle +/// \return the four regions in image coordinates +/// TODO: maybe return regions in world coordinates /// -bool LineDetection::findLaneAdvanced(std::vector &lines) { - // Last frames middle line is our new guess. Clear old middle line - std::vector currentGuess; - if(!currentMiddleLine_.empty()) { - for(auto l : currentMiddleLine_) { - l.lineType_ = Line::LineType::GUESS; - currentGuess.push_back(l); - } - currentMiddleLine_.clear(); +std::vector LineDetection::buildRegions(cv::Point2f positionWorld, float angle) { + cv::Vec2f dirVec(cos(angle), sin(angle)); // in world coordinates + cv::Vec2f leftVec(laneWidthWorld_ * dirVec[1], laneWidthWorld_ * dirVec[0]); // in world coordinates + std::vector regions(4); + +// ROS_INFO_STREAM("dirVec(W): " << dirVec); +// ROS_INFO_STREAM("leftVec(W): " << leftVec); + + // Convert the position to image coordinates + std::vector imgPts(1), worldPts(1); + worldPts.at(0) = positionWorld; + image_operator_.worldToWarpedImg(worldPts, imgPts); + cv::Point2f position = imgPts.at(0); + + // Calculate the region size in world coordinates and then convert to size in image coordinates + cv::Point2f tmpWorldPt(positionWorld.x + segmentLength_, positionWorld.y + laneWidthWorld_); + worldPts.at(0) = tmpWorldPt; + image_operator_.worldToWarpedImg(worldPts, imgPts); + cv::Size regionSize(imgPts.at(0).x - position.x, position.y - imgPts.at(0).y); + + // Get the region points in world coordinates + worldPts.resize(4); + imgPts.resize(4); + cv::Point2f centerR2(positionWorld.x + (0.5f * segmentLength_ * dirVec[0]) + (0.5f * leftVec[0]), + positionWorld.y + (0.5f * segmentLength_ * dirVec[1]) - (0.5f * leftVec[1])); + cv::Point2f centerR0(centerR2.x - 2*leftVec[0], centerR2.y + 2*leftVec[1]); + cv::Point2f centerR1(centerR2.x - leftVec[0], centerR2.y + leftVec[1]); + cv::Point2f centerR3(centerR2.x + leftVec[0], centerR2.y - leftVec[1]); + worldPts.at(0) = centerR0; + worldPts.at(1) = centerR1; + worldPts.at(2) = centerR2; + worldPts.at(3) = centerR3; + image_operator_.worldToWarpedImg(worldPts, imgPts); + + // Create the regions + for(int i = 0; i < 4; i++) { + regions.at(i) = cv::RotatedRect(imgPts.at(i), regionSize, (-angle * 180.f / M_PI)); } - // todo: we should only do this, when the car starts in the start box. Otherwise we can get in trouble - std::vector imagePoints, worldPoints; - if(currentGuess.empty()) { - ROS_WARN("Dummy guess"); - // todo: we should have a state like "RECOVER", where we try to find the middle line again (more checks, if it is really the middle one) - worldPoints.push_back(cv::Point2f(0.24, 0.2)); - worldPoints.push_back(cv::Point2f(0.4, 0.2)); - worldPoints.push_back(cv::Point2f(0.55, 0.2)); - worldPoints.push_back(cv::Point2f(0.70, 0.2)); - image_operator_.worldToImage(worldPoints, imagePoints); - for(size_t i = 1; i < imagePoints.size(); i++) - currentGuess.push_back(Line(imagePoints.at(i - 1), imagePoints.at(i), worldPoints.at(i - 1), worldPoints.at(i), Line::LineType::GUESS)); - } +#ifdef PUBLISH_DEBUG +// for(int i = 0; i < 3; i++) { +// auto r = regions.at(i); +// cv::Point2f edges[4]; +// r.points(edges); +// +// for(int i = 0; i < 4; i++) +// cv::line(debugImg_, edges[i], edges[(i+1)%4], cv::Scalar(255)); +// } +#endif - publishDebugLines(currentGuess); - - // Use the guess to classify the lines - determineLineTypes(lines, currentGuess); - std::vector leftLines, middleLines, rightLines, horizontalLines; - for(auto l : lines) { - if(l.lineType_ == Line::LineType::MIDDLE_LINE) - middleLines.push_back(l); - else if(l.lineType_ == Line::LineType::LEFT_LINE) - leftLines.push_back(l); - else if(l.lineType_ == Line::LineType::RIGHT_LINE) - rightLines.push_back(l); - else if(l.lineType_ == Line::LineType::HORIZONTAL_LEFT_LANE || - l.lineType_ == Line::LineType::HORIZONTAL_OUTER_LEFT || - l.lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE || - l.lineType_ == Line::LineType::HORIZONTAL_OUTER_RIGHT) - horizontalLines.push_back(l); - } + return regions; +} - // Classify the horizontal lines - // Find start line - // TODO: check if we have lines on left and right lane. in this case, set all left and right ones (in a certain x-interval to start line) - for(auto firstIt = horizontalLines.begin(); firstIt != horizontalLines.end(); ++firstIt) { - for(auto secondIt = horizontalLines.begin(); secondIt != horizontalLines.end(); ++secondIt) { - if(((firstIt->lineType_ == Line::LineType::HORIZONTAL_LEFT_LANE) && (secondIt->lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE)) || - ((firstIt->lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE) && (secondIt->lineType_ == Line::LineType::HORIZONTAL_LEFT_LANE))) { - if(abs(firstIt->wMid_.x - secondIt->wMid_.x) < 0.2) { - firstIt->lineType_ = Line::LineType::START_LINE; - secondIt->lineType_ = Line::LineType::START_LINE; - } - } +/// +/// \param regions at least 3 regions (currently in image coordinates, but could be changed -> TODO) +/// +void LineDetection::assignLinesToRegions(std::vector *regions, std::vector &lines, + std::vector &leftMarkings, std::vector &midMarkings, + std::vector &rightMarkings, std::vector &otherMarkings) { + for(auto linesIt = lines.begin(); linesIt != lines.end(); ++linesIt) { + if(lineIsInRegion(*linesIt, &(regions->at(0)), true)) { + leftMarkings.push_back(*linesIt); +// cv::line(debugImg_, linesIt->iP1_, linesIt->iP2_, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); // Green + } else if(lineIsInRegion(*linesIt, &(regions->at(1)), true)) { + midMarkings.push_back(*linesIt); +// cv::line(debugImg_, linesIt->iP1_, linesIt->iP2_, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); // Green + } else if(lineIsInRegion(*linesIt, &(regions->at(2)), true)) { + rightMarkings.push_back(*linesIt); +// cv::line(debugImg_, linesIt->iP1_, linesIt->iP2_, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); // Green + } else { + otherMarkings.push_back(*linesIt); } } +} - // Find stop line - std::vector distsToStopLine; - for(auto firstIt = horizontalLines.begin(); firstIt != horizontalLines.end(); ++firstIt) { - if(firstIt->lineType_ == Line::LineType::HORIZONTAL_RIGHT_LANE) { - bool foundRightOuterBound = false, foundLeftOuterBound = false; - for(auto secondIt = horizontalLines.begin(); secondIt != horizontalLines.end(); ++secondIt) { - if(secondIt->lineType_ == Line::LineType::HORIZONTAL_OUTER_LEFT) - foundLeftOuterBound = true; - else if(secondIt->lineType_ == Line::LineType::HORIZONTAL_OUTER_RIGHT) - foundRightOuterBound = true; - } - if(foundLeftOuterBound && foundRightOuterBound) { - firstIt->lineType_ = Line::LineType::STOP_LINE; - distsToStopLine.push_back(firstIt->wMid_.x); - } - } - } - if(distsToStopLine.size() > 2) { - auto avgDistToStopLine = std::accumulate(distsToStopLine.begin(), distsToStopLine.end(), 0.0f) / distsToStopLine.size(); - ROS_INFO_STREAM("Found a stop line; distance to it = " << avgDistToStopLine); - stopLineCount++; - if(avgDistToStopLine <= 0.6f) { // TODO: magic number - - if(stopLineCount > 5) { - driveState = DriveState::Intersection; - // todo: create intersection exit based on stop line (orthogonal) or middle line? - } - // todo: maybe return here? but do not forget to publish road points - } +/// +/// \brief LineDetection::lineIsInRegion +/// \param line +/// \param region in image coordinates +/// \param isImageCoordinate true, if region is given in image coordinates +/// \return +/// +bool LineDetection::lineIsInRegion(Line *line, const cv::RotatedRect *region, bool isImageCoordiante) const { + cv::Point2f edges[4]; + region->points(edges); // bottomLeft, topLeft, topRight, bottomRight + + if(isImageCoordiante) { + // we check if at least on of the lines ends is in the region + if(pointIsInRegion(&(line->iP1_), edges)) + return true; + else + return pointIsInRegion(&(line->iP2_), edges); } else { - stopLineCount = 0; + // we check if at least on of the lines ends is in the region + if(pointIsInRegion(&(line->wP1_), edges)) + return true; + else + return pointIsInRegion(&(line->wP2_), edges); } +} -#ifdef PUBLISH_DEBUG - publishDebugLines(rightLines); - publishDebugLines(leftLines); - publishDebugLines(horizontalLines); -#endif +bool LineDetection::pointIsInRegion(cv::Point2f *pt, cv::Point2f *edges) const { + auto u = edges[0] - edges[1]; + auto v = edges[0] - edges[3]; - if(middleLines.empty()) { - // this can happen when approach an intersection without a stop line - ROS_INFO("------------- no middle line found"); - worldPoints.clear(); - imagePoints.clear(); - for(auto l : rightLines) { - // perpendiculare vector - Eigen::Vector2f vec(-(l.wP2_.y - l.wP1_.y), - (l.wP2_.x - l.wP1_.x)); - vec.normalize(); - vec *= lineWidth_; - worldPoints.push_back(cv::Point2f(l.wP1_.x + vec(0), l.wP1_.y + vec(1))); - worldPoints.push_back(cv::Point2f(l.wP2_.x + vec(0), l.wP2_.y + vec(1))); -// ROS_INFO_STREAM("Moved (" << l.wP1_.x << "," << l.wP1_.y << ") to (" << (l.wP1_.x + vec(0)) << "," << (l.wP1_.y + vec(1)) << ")"); - } - for(auto l : leftLines) { - // perpendiculare vector - Eigen::Vector2f vec((l.wP2_.y - l.wP1_.y), - -(l.wP2_.x - l.wP1_.x)); - vec.normalize(); - vec *= lineWidth_; - worldPoints.push_back(cv::Point2f(l.wP1_.x + vec(0), l.wP1_.y + vec(1))); - worldPoints.push_back(cv::Point2f(l.wP2_.x + vec(0), l.wP2_.y + vec(1))); -// ROS_INFO_STREAM("Moved (" << l.wP1_.x << "," << l.wP1_.y << ") to (" << (l.wP1_.x + vec(0)) << "," << (l.wP1_.y + vec(1)) << ")"); - } + auto uDotPt = u.dot(*pt); + auto uDotP1 = u.dot(edges[0]); + auto uDotP2 = u.dot(edges[1]); + auto vDotPt = v.dot(*pt); + auto vDotP1 = v.dot(edges[0]); + auto vDotP4 = v.dot(edges[3]); - if(worldPoints.empty()) - return false; - - int order = 1; - cv::Mat srcX(worldPoints.size(), 1, CV_32FC1); - cv::Mat srcY(worldPoints.size(), 1, CV_32FC1); -// (src_x.rows>0)&&(src_y.rows>0)&&(src_x.cols==1) -// (src_y.cols==1) -// (dst.cols==1)&&(dst.rows==(order+1))&&(order>=1) - cv::Mat dst(order+1, 1, CV_32FC1); - - -// image_operator_.worldToImage(worldPoints, imagePoints); - for(int i = 0; i < worldPoints.size(); i += 2) { -// middleLines.push_back(Line(imagePoints.at(i), imagePoints.at(i+1), worldPoints.at(i), worldPoints.at(i+1), Line::LineType::MIDDLE_LINE)); - srcX.at(i, 0) = worldPoints.at(i).x; - srcX.at(i+1, 0) = worldPoints.at(i+1).x; - srcY.at(i, 0) = worldPoints.at(i).y; - srcY.at(i+1, 0) = worldPoints.at(i+1).y; - } + bool xOk = (uDotPt < uDotP1 && uDotPt > uDotP2) || + (uDotPt > uDotP1 && uDotPt < uDotP2); + bool yOk = (vDotPt < vDotP1 && vDotPt > vDotP4) || + (vDotPt > vDotP1 && vDotPt < vDotP4); - ROS_INFO_STREAM("srcX dims = " << srcX.size << "; srcY dims = " << srcY.size << "; dst dims = " << dst.size); - polyfit(srcX, srcY, dst, order); - ROS_INFO_STREAM("PolyFit = " << dst); + return xOk && yOk; +} - worldPoints.clear(); - auto a = dst.at(0,0); - auto b = dst.at(1,0); +/// +/// \brief LineDetection::findLaneWithRansac +/// \param leftMarkings +/// \param midMarkings +/// \param rightMarkings +/// \param pos the segments position in world coordinates +/// \param prevAngle +/// \return +/// +Segment LineDetection::findLaneWithRansac(std::vector &leftMarkings, std::vector &midMarkings, std::vector &rightMarkings, + cv::Point2f pos, float prevAngle) { + float bestAngle = prevAngle; + float bestScore = 0.f; + cv::Point2f bestLeft, bestMid, bestRight; + size_t numLines = leftMarkings.size() + midMarkings.size() + rightMarkings.size(); +// float angleVar = M_PI / 32.0f; // TODO move to config + size_t maxIterations = 200, iteration = 0; + std::vector worldPts, imgPts; + worldPts.push_back(pos); + image_operator_.worldToWarpedImg(worldPts, imgPts); + + if(numLines < 5) { +// ROS_WARN("No lines for Ransac"); + return Segment(pos, imgPts.at(0), 0.f, prevAngle, segmentLength_, 0.f); + } - for(int i = 1; i < 8; i++) { - auto x = i * 0.3; - worldPoints.push_back(cv::Point2f(x, a*x + b)); + std::default_random_engine generator; + std::uniform_int_distribution distribution(0, numLines - 1); + +// ROS_INFO_STREAM("Left markings: " << leftMarkings.size()); +// ROS_INFO_STREAM("Mid markings: " << midMarkings.size()); +// ROS_INFO_STREAM("Right markings: " << rightMarkings.size()); +// if(leftMarkings.size() < 2) +// ROS_WARN_STREAM("Only having " << leftMarkings.size() << " left markings"); +// if(midMarkings.size() < 2) +// ROS_WARN_STREAM("Only having " << midMarkings.size() << " mid markings"); +// if(rightMarkings.size() < 2) +// ROS_WARN_STREAM("Only having " << rightMarkings.size() << " right markings"); + + while(bestScore < 0.9 && (iteration++ < maxIterations)) { + // Select a random line and get its angle + // Also get a random point and move it on the expected middle line + int randomIdx = distribution(generator); + float currentAngle = prevAngle; + cv::Vec2f dirVec; + cv::Point2f randomPoint; + + if(randomIdx < leftMarkings.size()) { + currentAngle = leftMarkings.at(randomIdx)->getAngle(); + randomPoint = leftMarkings.at(randomIdx)->wP1_; + dirVec = cv::Vec2f (cos(currentAngle), sin(currentAngle)); + cv::Vec2f downVec(dirVec[1], - dirVec[0]); + randomPoint.x += downVec[0] * laneWidthWorld_; + randomPoint.y += downVec[1] * laneWidthWorld_; +// ROS_INFO_STREAM("Move left marking from " << leftMarkings.at(randomIdx)->wP1_ << " with " << (downVec * laneWidthWorld_)); + } else if((randomIdx - leftMarkings.size()) < midMarkings.size()){ + currentAngle = midMarkings.at(randomIdx - leftMarkings.size())->getAngle(); + randomPoint = midMarkings.at(randomIdx - leftMarkings.size())->wP1_; + dirVec = cv::Vec2f (cos(currentAngle), sin(currentAngle)); + // This point is already on the middle line +// ROS_INFO_STREAM("Use mid marking " << midMarkings.at(randomIdx - leftMarkings.size())->wP1_); + } else { + currentAngle = rightMarkings.at(randomIdx - leftMarkings.size() - midMarkings.size())->getAngle(); + randomPoint = rightMarkings.at(randomIdx - leftMarkings.size() - midMarkings.size())->wP1_; + dirVec = cv::Vec2f (cos(currentAngle), sin(currentAngle)); + cv::Vec2f upVec(- dirVec[1], dirVec[0]); + randomPoint.x += upVec[0] * laneWidthWorld_; + randomPoint.y += upVec[1] * laneWidthWorld_; +// ROS_INFO_STREAM("Move right marking from " << rightMarkings.at(randomIdx - leftMarkings.size() - midMarkings.size())->wP1_ << +// " with " << (upVec * laneWidthWorld_)); } - imagePoints.clear(); - currentMiddleLine_.clear(); - image_operator_.worldToImage(worldPoints, imagePoints); - for(int i = 1; i < worldPoints.size(); i++) { - currentMiddleLine_.push_back(Line(imagePoints.at(i-1), imagePoints.at(i), worldPoints.at(i-1), worldPoints.at(i), Line::LineType::MIDDLE_LINE)); - ROS_INFO_STREAM("World line: (" << worldPoints.at(i-1).x << "," << worldPoints.at(i-1).y << ") - (" << - worldPoints.at(i).x << "," << worldPoints.at(i).y << ")"); - } + // Now build 3 RotatedRects where we expect the lane markings + cv::Size2f boxSize(segmentLength_ * 2, lineVar_ * 2); +// ROS_INFO_STREAM("Box size: " << boxSize); + cv::Vec2f upVec(- dirVec[1], dirVec[0]); + cv::Point2f leftPos(randomPoint.x + upVec[0] * laneWidthWorld_, randomPoint.y + upVec[1] * laneWidthWorld_); + cv::Point2f rightPos(randomPoint.x - upVec[0] * laneWidthWorld_, randomPoint.y - upVec[1] * laneWidthWorld_); + cv::RotatedRect midBox(randomPoint, boxSize, currentAngle * 180.f / M_PI); + cv::RotatedRect leftBox(leftPos, boxSize, currentAngle * 180.f / M_PI); + cv::RotatedRect rightBox(rightPos, boxSize, currentAngle * 180.f / M_PI); + +// ROS_INFO_STREAM("center left: " << cv::Point2f(randomPoint.x + dirVec[0] * laneWidthWorld_, +// randomPoint.y + dirVec[1] * laneWidthWorld_)); +// ROS_INFO_STREAM("center mid: " << randomPoint); +// ROS_INFO_STREAM("center right: " << cv::Point2f(randomPoint.x - dirVec[0] * laneWidthWorld_, +// randomPoint.y - dirVec[1] * laneWidthWorld_)); - publishDebugLines(currentMiddleLine_); - return true; - } +#ifdef PUBLISH_DEBUG +// std::vector regions(3); +// regions.at(0) = midBox; +// regions.at(1) = leftBox; +// regions.at(2) = rightBox; +// for(auto r : regions) { +// cv::Point2f edges[4]; +// r.points(edges); +// +// std::vector imgPts, worldPts; +// worldPts.push_back(edges[0]); +// worldPts.push_back(edges[1]); +// worldPts.push_back(edges[2]); +// worldPts.push_back(edges[3]); +// worldPts.push_back(r.center); +// image_operator_.worldToWarpedImg(worldPts, imgPts); +// +// for(int i = 0; i < 4; i++) +// cv::line(debugImg_, imgPts.at(i), imgPts.at((i+1)%4), cv::Scalar(255,255,255), 2, cv::LINE_AA); +// cv::circle(debugImg_, imgPts.at(4), 3, cv::Scalar(255,128), 1, cv::LINE_AA); +// } +#endif - // ------------------------------------------------------------------------------------------------------------------------------------ - // TODO: find a better and more efficient way to do this. maybe also project left and right line into middle to use all points. - // ------------------------------------------------------------------------------------------------------------------------------------ - // in the free drive event, double lines are treated as normal dashed lines - // sort our middle lines based on wP1_.x of line - std::sort(middleLines.begin(), middleLines.end(), [](Line &a, Line &b){ return a.wP1_.x < b.wP1_.x; }); - // build bounding boxes around group of lines, where lines in group are closer than 0.25m to each other - float currentMinX = middleLines.at(0).wP1_.x; - std::vector cPts, newMidLinePts; - for(auto l : middleLines) { - if(l.wP1_.x > (currentMinX + 0.25)) { - buildBbAroundLines(cPts, newMidLinePts); - cPts.clear(); - currentMinX = l.wP1_.x; + // Check for inliers + int numInliers = 0; + numInliers += std::count_if(leftMarkings.begin(), leftMarkings.end(), + [leftBox, this](Line *l) {return lineIsInRegion(l, &leftBox, false);}); + numInliers += std::count_if(midMarkings.begin(), midMarkings.end(), + [midBox, this](Line *l) {return lineIsInRegion(l, &midBox, false);}); + numInliers += std::count_if(rightMarkings.begin(), rightMarkings.end(), + [rightBox, this](Line *l) {return lineIsInRegion(l, &rightBox, false);}); + + // compare the selected angle to all lines +// int numInliers = 0; +// numInliers += std::count_if(leftMarkings.begin(), leftMarkings.end(), +// [currentAngle, angleVar](Line *l) {return std::abs(l->getAngle() - currentAngle) < angleVar;}); +// numInliers += std::count_if(midMarkings.begin(), midMarkings.end(), +// [currentAngle, angleVar](Line *l) {return std::abs(l->getAngle() - currentAngle) < angleVar;}); +// numInliers += std::count_if(rightMarkings.begin(), rightMarkings.end(), +// [currentAngle, angleVar](Line *l) {return std::abs(l->getAngle() - currentAngle) < angleVar;}); + + float newScore = static_cast(numInliers) / static_cast(numLines); + if(newScore > bestScore) { + bestScore = newScore; + bestAngle = currentAngle; + bestLeft = leftPos; + bestMid = randomPoint; + bestRight = rightPos; } - cPts.push_back(l.wP1_); - cPts.push_back(l.wP2_); - } - - // finish the bounding box building step - if(!cPts.empty()) { - buildBbAroundLines(cPts, newMidLinePts); } - // build the middle line from out points - // extend the last line to the front - auto wPt1 = newMidLinePts.at(newMidLinePts.size() - 2); - auto wPt2 = newMidLinePts.at(newMidLinePts.size() - 1); - auto dir = wPt2 - wPt1; - auto dirLen = sqrt(dir.x * dir.x + dir.y * dir.y); - - auto newPt = wPt2 + (dir * (0.6 / dirLen)); - if(newPt.x < maxViewRange_) - newMidLinePts.push_back(newPt); - - // extend first line to the back - wPt1 = newMidLinePts.at(0); - wPt2 = newMidLinePts.at(1); - dir = wPt2 - wPt1; - dirLen = sqrt(dir.x * dir.x + dir.y * dir.y); - // the image starts 0.24m from the rear_axis_middle_ground. TODO: sould be in config - auto distToImageBoundary = wPt1.x - 0.24; - if(distToImageBoundary > 0.1) { // distance is positive - // dir, distToImageBoundary, dirLen are all positive - newPt = wPt1 - (dir * distToImageBoundary / dirLen); - newMidLinePts.push_back(newPt); - } - - // sort the world points based on the distance to the car - std::sort(newMidLinePts.begin(), newMidLinePts.end(), [](cv::Point2f &a, cv::Point2f &b) { return a.x < b.x; }); - - // convert points - imagePoints.clear(); - image_operator_.worldToImage(newMidLinePts, imagePoints); - // create the new middle line - currentMiddleLine_.clear(); - for(size_t i = 1; i < imagePoints.size(); i++) { - auto a = imagePoints.at(i - 1); - auto b = imagePoints.at(i); - // this makes the line coordinates inconsistant, but this should not be a serious problem - if(a.y > imgHeight_) - a.y = imgHeight_ - 1; - if(b.y > imgHeight_) - b.y = imgHeight_ - 1; - - currentMiddleLine_.push_back(Line(a, b, newMidLinePts.at(i - 1), newMidLinePts.at(i), Line::LineType::MIDDLE_LINE)); +// ROS_INFO_STREAM("Best score: " << bestScore << " with " << (bestScore*(float)numLines) << "/" << numLines << " lines"); + + // Calculate the segments position based on the results + // First the a point on the right lane + cv::Point2f laneMidPt = bestRight; + cv::Vec2f rightToMidVec(bestMid.x - bestRight.x, bestMid.y - bestRight.y); + laneMidPt.x += 0.5 * rightToMidVec[0]; + laneMidPt.y += 0.5 * rightToMidVec[1]; + // Move this point back so it is at the segments start +// ROS_INFO_STREAM("Old position: " << pos << " new one: " << laneMidPt); + cv::Vec2f segDir(cos(bestAngle), sin(bestAngle)); // orientation vector of segment + cv::Vec2f vecToInitPos(laneMidPt.x - pos.x, laneMidPt.y - pos.y); // vector from init position to new position + float distToInitPos = sqrt(vecToInitPos[0]*vecToInitPos[0] + vecToInitPos[1]*vecToInitPos[1]); + if(laneMidPt.x < pos.x) { + laneMidPt.x += segDir[0] * distToInitPos; + laneMidPt.y += segDir[1] * distToInitPos; + } else { + laneMidPt.x -= segDir[0] * distToInitPos; + laneMidPt.y -= segDir[1] * distToInitPos; } +// ROS_INFO_STREAM("Corrected to: " << laneMidPt); + +// auto angleBetweenVecs = acos(segDir.dot(vecToInitPos)); +// ROS_INFO_STREAM("angle between vecs = " << (angleBetweenVecs * 180.f / M_PI)); + +// ROS_INFO_STREAM("Move from " << laneMidPt << " with " << (segDir * distToInitPos) << " with goal " << pos); + +// ROS_INFO_STREAM("Ended up at " << laneMidPt); + +// float t = (laneMidPt.x - pos.x) / segDir[0]; +// laneMidPt.x += t * segDir[0]; +// laneMidPt.y += t * segDir[1]; + // Convert all needed points to image coordinates + worldPts.clear(); + imgPts.clear(); + worldPts.push_back(laneMidPt); + worldPts.push_back(bestLeft); + worldPts.push_back(bestMid); + worldPts.push_back(bestRight); + image_operator_.worldToWarpedImg(worldPts, imgPts); + + auto lengthVec = bestMid - pos; + auto segLen = sqrt(lengthVec.x*lengthVec.x + lengthVec.y*lengthVec.y); + + // Create the segment + Segment s(laneMidPt, imgPts.at(0), prevAngle - bestAngle, bestAngle, segLen, bestScore); + s.leftPosW = bestLeft; + s.midPosW = bestMid; + s.rightPosW = bestRight; + s.leftPosI = imgPts.at(1); + s.midPosI = imgPts.at(2); + s.rightPosI = imgPts.at(3); + + return s; +} - // validate our new guess - if(!currentMiddleLine_.empty()) { - if(currentMiddleLine_.at(currentMiddleLine_.size() - 1).wP2_.x < 0.5) { - // our guess is too short, it is better to use the default one; TODO: bad idea -> RECOVER state - currentMiddleLine_.clear(); - } else if((currentMiddleLine_.at(0).getAngle() < 1.0) || (currentMiddleLine_.at(0).getAngle() > 2.6)) { - // the angle of the first segment is weird (TODO: workaround for now, why can this happen?) - // if we have more than one segment, then we just delete the first - if(currentMiddleLine_.size() > 1) { - currentMiddleLine_.erase(currentMiddleLine_.begin()); - } else { - // otherwise throw the guess away - currentMiddleLine_.clear(); - } - } +bool LineDetection::findIntersection(Segment &resultingSegment, float segmentAngle, cv::Point2f segStartWorld, + std::vector &leftMarkings, std::vector &midMarkings, std::vector &rightMarkings) { + + bool foundIntersection = false; + bool middleExists = false, rightExists = false, leftExists = false; + float stopLineAngle = 0.f; + int numStopLines = 0; + + for(auto l : leftMarkings) { + if(std::abs(l->getAngle() - segmentAngle - M_PI_2) < (M_PI / 7.0f)) { + leftExists = true; + cv::line(debugImg_, l->iP1_, l->iP2_, cv::Scalar(155), 2, cv::LINE_AA); + } + } + + for(auto l : midMarkings) { + if(std::abs(l->getAngle() - segmentAngle - M_PI_2) < (M_PI / 7.0f)) { + middleExists = true; + numStopLines++; + stopLineAngle += l->getAngle(); + cv::line(debugImg_, l->iP1_, l->iP2_, cv::Scalar(57,189,37), 2, cv::LINE_AA); + } + } + + for(auto l : rightMarkings) { + if(std::abs(l->getAngle() - segmentAngle - M_PI_2) < (M_PI / 7.0f)) { + rightExists = true; + cv::line(debugImg_, l->iP1_, l->iP2_, cv::Scalar(0,255,255), 2, cv::LINE_AA); + } + } + + // DEBUG +// if(leftExists || rightExists || middleExists) { +// ROS_INFO("Left: %s middle: %s right: %s", (leftExists ? "Yes" : "No"), (middleExists ? "Yes" : "No"), (rightExists ? "Yes" : "No")); +// } + + // Build a segment for the whole intersection + Eigen::Vector2f laneDir; + SegmentType intersectionType; + float drivingDirectionAngle = segmentAngle; + if(middleExists && (leftExists || rightExists)) { + ROS_INFO("Intersection found - stop"); + foundIntersection = true; + intersectionType = SegmentType::INTERSECTION_STOP; + stopLineAngle = stopLineAngle / static_cast(numStopLines); + drivingDirectionAngle = stopLineAngle - M_PI_2; +// laneDir = Eigen::Vector2f(sin(stopLineAngle), - cos(stopLineAngle)); // direction of vector perpendicular to stop line + laneDir = Eigen::Vector2f(cos(drivingDirectionAngle), sin(drivingDirectionAngle)); + } + if(!middleExists && leftExists && rightExists) { + ROS_INFO("Intersection found - do not stop"); + foundIntersection = true; + intersectionType = SegmentType::INTERSECTION_GO_STRAIGHT; + laneDir = Eigen::Vector2f(cos(segmentAngle), sin(segmentAngle)); + } + + if(foundIntersection) { + std::vector imgPts, worldPts; + float segmentLength = laneWidthWorld_ * 2.5f; + + worldPts.push_back(segStartWorld); + image_operator_.worldToWarpedImg(worldPts, imgPts); + // Segment(cv::Point2f worldPos, cv::Point2f imagePos, float angleDiff_, float angleTotal_, float len, float prob) + resultingSegment = Segment(segStartWorld, imgPts.at(0), segmentAngle - drivingDirectionAngle, + drivingDirectionAngle, segmentLength, 1.0f); + resultingSegment.segmentType = intersectionType; + } + + return foundIntersection; +} - bool done = false; - for(size_t i = 1; i < currentMiddleLine_.size() && !done; i++) { - // angles between two connected segments should be plausible - if(std::abs(currentMiddleLine_.at(i - 1).getAngle() - currentMiddleLine_.at(i).getAngle()) > 0.8) { - // 0.8 = 45 degree - // todo: clear whole line or just to this segment? - currentMiddleLine_.erase(currentMiddleLine_.begin() + i, currentMiddleLine_.end()); - // ROS_INFO_STREAM("clipped line because of angle difference"); - done = true; - } else if(currentMiddleLine_.at(i).wP2_.x > maxViewRange_) { - // ROS_INFO_STREAM("clipped line because of maxViewRange_"); - currentMiddleLine_.erase(currentMiddleLine_.begin() + i, currentMiddleLine_.end()); - done = true; - } - else if((i < (currentMiddleLine_.size() - 1)) && currentMiddleLine_.at(i).getLength() > 0.3) { - // we build middle line segments of max 0.25m, so this is a test; TODO: overthink this - // excluding the last segment, since we created this for searching forward - // ROS_INFO_STREAM("clipped line because of length"); - currentMiddleLine_.erase(currentMiddleLine_.begin() + i, currentMiddleLine_.end()); - done = true; - } - } - } +bool LineDetection::segmentIsPlausible(Segment *segmentToAdd, bool isFirstSegment) { + if(segmentToAdd-> probablity < 0.2f) { +// ROS_INFO("Segments probability is too low"); + return false; + } + if(!isFirstSegment) { + float angleVar = M_PI / 7.0f; // TODO move to config + if(std::abs(segmentToAdd->angleDiff) > angleVar) { +// ROS_INFO_STREAM("Too much angle difference: " << (segmentToAdd->angleDiff * 180.f / M_PI) << "[deg]"); + return false; + } + } + + return true; +} - publishDebugLines(currentMiddleLine_); - - // publish points to road topic - // TODO: we want to publish not only the middle line, but also start line, stop lines, intersections, barred areas, ... - // todo: we should do this outside this function inside the imageCallback - drive_ros_msgs::RoadLine msgMidLine; - msgMidLine.lineType = drive_ros_msgs::RoadLine::MIDDLE; - for(auto l : currentMiddleLine_) { - geometry_msgs::PointStamped pt1, pt2; - pt1.point.x = l.wP1_.x; - pt1.point.y = l.wP1_.y; - msgMidLine.points.push_back(pt1); - pt2.point.x = l.wP2_.x; - pt2.point.y = l.wP2_.y; - msgMidLine.points.push_back(pt2); - } +/// +/// \brief LineDetection::findLinesWithHough +/// Extract Lines from the image using Hough Lines +/// \param img A CvImagePtr to the current image where we want to search for lines +/// \param houghLines A std::vector where the Lines will be returned. Has to be empty. +/// +void LineDetection::findLinesWithHough(cv::Mat &img, std::vector &houghLines) { + cv::Mat processingImg; - line_output_pub_.publish(msgMidLine); -} + // Blur the image and find edges with Canny + cv::GaussianBlur(img, processingImg, cv::Size(15, 15), 0, 0); + cv::Canny(processingImg, processingImg, cannyThreshold_, cannyThreshold_ * 3, 3); -bool LineDetection::findIntersectionExit(std::vector &lines) { - std::vector potMidLines; - // we keep current current middle line - // todo: we should check if the middle line is plausible - if(currentMiddleLine_.size() == 0) { - ROS_WARN("No middle line"); - return false; - } - for(auto l : lines) { - if(std::abs(l.getAngle() - currentMiddleLine_.at(0).getAngle()) < (M_PI / 8)) { - l.lineType_ = Line::LineType::UNKNOWN; - potMidLines.push_back(l); - } - } + // Get houghlines + std::vector hLinePoints; + cv::HoughLinesP(processingImg, hLinePoints, 1, CV_PI / 180, houghThresold_, houghMinLineLen_, houghMaxLineGap_); - determineLineTypes(potMidLines, currentMiddleLine_); - float closestLineDist = 10.0f; - Line *closestMiddleLine; - bool foundLeft = false, foundMiddle = false, foundRight = false; - for(auto l : potMidLines) { - foundLeft |= l.lineType_ == Line::LineType::LEFT_LINE; - foundMiddle |= l.lineType_ == Line::LineType::MIDDLE_LINE; - foundRight |= l.lineType_ == Line::LineType::RIGHT_LINE; - - if(l.wMid_.x < closestLineDist) { - closestLineDist = l.wMid_.x; - closestMiddleLine = &l; + // Extract points for houghLines and convert to world-coordinates + std::vector imagePoints, worldPoints; + for(size_t i = 0; i < hLinePoints.size(); i++) { + cv::Vec4i currentPoints = hLinePoints.at(i); + // Ignore lines very close to the car and about to be parallel to image y-axis + if(!((currentPoints[0] < 50) && (currentPoints[2] < 50))) { + imagePoints.push_back(cv::Point(currentPoints[0], currentPoints[1])); + imagePoints.push_back(cv::Point(currentPoints[2], currentPoints[3])); } } - ROS_INFO_STREAM("Closest line dist = " << closestLineDist); - ROS_INFO_STREAM("found left: " << foundLeft << "\tfound middle: " << foundMiddle << "\tfound right: " << foundRight); + image_operator_.warpedImgToWorld(imagePoints, worldPoints); - if(foundLeft && foundMiddle && foundRight) { - closestMiddleLine->lineType_ = Line::LineType::GUESS; - potMidLines.push_back(*closestMiddleLine); - driveState = DriveState::Street; + // Build lines from points + std::vector lines; + for(size_t i = 0; i < worldPoints.size(); i += 2) { + lines.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); } - publishDebugLines(currentMiddleLine_); - publishDebugLines(potMidLines); - - // todo: ideas: 1) detect opposite stop line as end of intersection - // 2) usu odometry and knowledge about intersection size (line_width * 2) - return true; -} - -void LineDetection::determineLineTypes(std::vector &lines, std::vector ¤tGuess) { - for(auto lineIt = lines.begin(); lineIt != lines.end(); ++lineIt) { - Line segment = currentGuess.at(0); - bool isInSegment = false; - // find the corresponding segment - // todo: this only uses the x-coordinate right now, improve this - for(size_t i = 0; (i < currentGuess.size()) && !isInSegment; i++) { - segment = currentGuess.at(i); - if(lineIt->wMid_.x > segment.wP1_.x) { - if(lineIt->wMid_.x < segment.wP2_.x) { - isInSegment = true; - } - } else if(lineIt->wMid_.x > segment.wP2_.x) { - isInSegment = true; - } - } - - if(isInSegment) { - // classify the line - if(std::abs(lineIt->getAngle() - segment.getAngle()) < lineAngle_) { - // lane width is 0.35 to 0.45 [m] - // todo: distance should be calculated based on orthogonal distance. - // the current approach leads to problems in curves - auto absDistanceToMidLine = std::abs(lineIt->wMid_.y - segment.wMid_.y); - if(absDistanceToMidLine < (lineWidth_ * 0.33)) { - lineIt->lineType_ = Line::LineType::MIDDLE_LINE; - } else if((lineIt->wMid_.y < segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { - lineIt->lineType_ = Line::LineType::RIGHT_LINE; - } else if((lineIt->wMid_.y > segment.wMid_.y) && (std::abs(absDistanceToMidLine - lineWidth_) < lineVar_)) { - lineIt->lineType_ = Line::LineType::LEFT_LINE; - } - } else { - classifyHorizontalLine(&*lineIt, segment.wMid_.y - lineIt->wMid_.y); // convert iterator to pointer - } - } + // Split lines which are longer than segmentLength (world coordinates) + worldPoints.clear(); + imagePoints.clear(); + for(auto it = lines.begin(); it != lines.end(); ++it) { + int splitInto = static_cast(it->getWorldLength() / segmentLength_) + 1; +// ROS_INFO_STREAM("Length = " << it->getWorldLength() << " split into " << splitInto); + + if(splitInto > 1) { + // Build normalized vector + cv::Vec2f dir = it->wP2_ - it->wP1_; + auto length = sqrt(dir[0]*dir[0] + dir[1]*dir[1]); + dir[0] = dir[0] / length; + dir[1] = dir[1] / length; + + cv::Point2f curPos = it->wP1_; + for(int i = 0; i < (splitInto - 1); i++) { + worldPoints.push_back(curPos); + curPos.x += dir[0] * segmentLength_; + curPos.y += dir[1] * segmentLength_; + worldPoints.push_back(curPos); + } + // This is the last part of the split line + worldPoints.push_back(curPos); + worldPoints.push_back(it->wP2_); + } else { + houghLines.push_back(*it); + } } -} -void LineDetection::classifyHorizontalLine(Line *line, float worldDistToMiddleLine) { - if(worldDistToMiddleLine < -lineWidth_) { - line->lineType_ = Line::LineType::HORIZONTAL_OUTER_LEFT; - } else if(worldDistToMiddleLine < 0) { - line->lineType_ = Line::LineType::HORIZONTAL_LEFT_LANE; - } else if(worldDistToMiddleLine < lineWidth_) { - line->lineType_ = Line::LineType::HORIZONTAL_RIGHT_LANE; - } else { - line->lineType_ = Line::LineType::HORIZONTAL_OUTER_RIGHT; + // Build split lines + image_operator_.worldToWarpedImg(worldPoints, imagePoints); + for(size_t i = 0; i < worldPoints.size(); i += 2) { + houghLines.push_back(Line(imagePoints.at(i), imagePoints.at(i + 1), worldPoints.at(i), worldPoints.at(i + 1))); } -} - -void LineDetection::buildBbAroundLines(std::vector ¢erPoints, std::vector &midLinePoints) { - auto rect = cv::minAreaRect(centerPoints); - cv::Point2f a, b; - cv::Point2f vertices2f[4]; - rect.points(vertices2f); - // use the long side as line - if(std::abs(vertices2f[0].x - vertices2f[1].x) > std::abs(vertices2f[1].x - vertices2f[2].x)) { - a = vertices2f[0]; - b = vertices2f[1]; - } else { - a = vertices2f[1]; - b = vertices2f[2]; - } - // ensures the correct order of the points in newMidLinePts vector - if(a.x > b.x) { - midLinePoints.push_back(b); - midLinePoints.push_back(a); - } else { - midLinePoints.push_back(a); - midLinePoints.push_back(b); - } -} +// ROS_INFO_STREAM("Started with " << lines.size() << " lines, now having " << houghLines.size()); -void LineDetection::publishDebugLines(std::vector &lines) { - for(auto it = lines.begin(); it != lines.end(); ++it) { - if(it->lineType_ == Line::LineType::MIDDLE_LINE) - cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(0, 255), 2, cv::LINE_AA); // Green - else if((it->lineType_ == Line::LineType::LEFT_LINE) || (it->lineType_ == Line::LineType::RIGHT_LINE)) - cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); // Blue - else if(it->lineType_ == Line::LineType::STOP_LINE) - cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(255), 2, cv::LINE_AA); // Red - else if(it->lineType_ == Line::LineType::START_LINE) - cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(0, 204, 255), 2, cv::LINE_AA); // Cyan - else if(it->lineType_ == Line::LineType::HORIZONTAL_OUTER_LEFT) - cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(255, 255), 2, cv::LINE_AA); // Yellow - else if(it->lineType_ == Line::LineType::HORIZONTAL_OUTER_RIGHT) - cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(255, 153, 51), 2, cv::LINE_AA); // Orange - else if(it->lineType_ == Line::LineType::GUESS) - cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(153, 0, 204), 2, cv::LINE_AA); // Purple - else - cv::line(debugImg_, it->iP1_, it->iP2_, cv::Scalar(255, 51, 204), 2, cv::LINE_AA); // Pink - } + return; } /// @@ -627,14 +690,16 @@ void LineDetection::publishDebugLines(std::vector &lines) { /// void LineDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level){ image_operator_.setConfig(config); - lineWidth_ = config.lineWidth; + laneWidthWorld_ = config.lineWidth; lineAngle_ = config.lineAngle; lineVar_ = config.lineVar; - maxViewRange_ = config.maxSenseRange; + maxSenseRange_ = config.maxSenseRange; cannyThreshold_ = config.cannyThreshold; houghThresold_ = config.houghThreshold; houghMinLineLen_ = config.houghMinLineLen; houghMaxLineGap_ = config.houghMaxLineGap; + segmentLength_ = config.segmentLength; + maxRansacInterations_ = config.ransacIterations; } } // namespace drive_ros_image_recognition diff --git a/src/road_model.cpp b/src/road_model.cpp new file mode 100644 index 0000000..c9add07 --- /dev/null +++ b/src/road_model.cpp @@ -0,0 +1,48 @@ +#include +#include "drive_ros_image_recognition/road_model.h" + +namespace drive_ros_image_recognition { + +void RoadModel::getFirstPosW(cv::Point2f &posW, float &angle) const { + for(auto s : segments) { + // Find the first segment which is in front of the car + if(s.positionWorld.x > 0.1f) { + // If the segment is too far away translate if towards the car + if(s.positionWorld.x > 0.5f) { +// ROS_WARN_STREAM("First segment is too far away: " << s.positionWorld); + float moveDistance = sqrt(s.positionWorld.x*s.positionWorld.x + s.positionWorld.y*s.positionWorld.y); + moveDistance -= 0.3f; + + posW.x = s.positionWorld.x - (moveDistance * cos(s.angleTotal)); + posW.y = s.positionWorld.y - (moveDistance * sin(s.angleTotal)); + } else { + posW = s.positionWorld; + } + + angle = s.angleTotal; + return; + } + } + + // If we have not found a suitable segment, return some default values + posW.x = 0.3f; + posW.y = 0.f; + angle = 0.f; + + return; +} + +void RoadModel::addSegments(std::vector &newSegments) { +// ROS_INFO_STREAM(segments.size() << " old segments and " << newSegments.size() << " new ones"); + for(int i = 0; (i < newSegments.size()) && (i < segments.size()); i++) { + auto deltaAngleDiff = std::abs(newSegments.at(i).angleDiff - segments.at(i).angleDiff); + auto deltaPosition = newSegments.at(i).positionWorld - segments.at(i).positionWorld; +// ROS_INFO_STREAM("(" << i << ") deltaAngleDiff = " << (deltaAngleDiff * 180.f / M_PI) << " deltaPosition = " << deltaPosition); + } + + segments.clear(); + segments.insert(segments.begin(), newSegments.begin(), newSegments.end()); +// ROS_INFO_STREAM("Num segments: " << segments.size()); +} + +} // namespace drive_ros_image_recognition From 32c2d1ea37745ff02c02aea0d4fb1c5ae32ca898 Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Thu, 22 Nov 2018 15:20:47 +0100 Subject: [PATCH 90/94] Use odometry to transform the driving line. --- .../line_detection.h | 8 +- .../drive_ros_image_recognition/road_model.h | 26 +- launch/line_detection.launch | 7 +- src/line_detection.cpp | 116 +++++--- src/road_model.cpp | 271 ++++++++++++++++-- 5 files changed, 353 insertions(+), 75 deletions(-) diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index 2a510ab..2cbd5af 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -41,6 +42,7 @@ class LineDetection { ImageOperator image_operator_; int imgHeight_; int imgWidth_; + ros::Time imgTimestamp; #ifdef PUBLISH_DEBUG cv::Mat debugImg_; #endif @@ -50,6 +52,7 @@ class LineDetection { image_transport::Subscriber imageSubscriber_; ros::Publisher line_output_pub_; ros::Subscriber homography_params_sub_; + ros::Subscriber odometrySub; #ifdef PUBLISH_DEBUG image_transport::Publisher debugImgPub_; #endif @@ -65,9 +68,13 @@ class LineDetection { // odometry components tf::TransformListener tfListener_; cv::Point2f oldPoint_; + nav_msgs::Odometry lastUsedOdometry; + nav_msgs::Odometry latestOdometry; + bool odometryInitialized = false; // callbacks void imageCallback(const sensor_msgs::ImageConstPtr& imageIn); + void odometryCallback(const nav_msgs::OdometryConstPtr &odomMsg); void reconfigureCB(drive_ros_image_recognition::LineDetectionConfig& config, uint32_t level); dynamic_reconfigure::Server dsrv_server_; dynamic_reconfigure::Server::CallbackType dsrv_cb_; @@ -92,7 +99,6 @@ class LineDetection { Segment findLaneWithRansac(std::vector &leftMarkings, std::vector &midMarkings, std::vector &rightMarkings, cv::Point2f pos, float prevAngle); bool findIntersection(Segment &resultingSegment, float segmentAngle, cv::Point2f segStartWorld, std::vector &leftMarkings, std::vector &midMarkings, std::vector &rightMarkings); - bool segmentIsPlausible(Segment *segmentToAdd, bool isFirstSegment); #ifdef PUBLISH_DEBUG void drawDebugLines(std::vector &lines); diff --git a/include/drive_ros_image_recognition/road_model.h b/include/drive_ros_image_recognition/road_model.h index f4b9891..0b54e6d 100644 --- a/include/drive_ros_image_recognition/road_model.h +++ b/include/drive_ros_image_recognition/road_model.h @@ -22,9 +22,13 @@ struct Segment { float angleDiff; // The angle difference to the previous segment float angleTotal; // The angle in which the lane is heading to float length; // This is defined globally and hence will be the same for every segment - float probablity; // Not sure if we use this, yet + float probablity; // The probability of this segment SegmentType segmentType = SegmentType::NORMAL_ROAD; + // Values for use with odometry + tf::Stamped odomPosition; + ros::Time creationTimestamp; + Segment() : probablity(0.f) { @@ -40,16 +44,26 @@ struct Segment { { } - inline bool isIntersection() const { return segmentType == SegmentType::INTERSECTION_GO_STRAIGHT || - segmentType == SegmentType::INTERSECTION_STOP; }; + inline bool isIntersection() const { return + segmentType == SegmentType::INTERSECTION_GO_STRAIGHT || + segmentType == SegmentType::INTERSECTION_STOP; + }; }; class RoadModel { - std::vector segments; + tf::TransformListener *pTfListener; + std::vector drivingLine; public: - void addSegments(std::vector &newSegments); - void getFirstPosW(cv::Point2f &posW, float &angle) const; + RoadModel(tf::TransformListener *tfListener) + : pTfListener(tfListener) + { + } + + void addSegments(std::vector &newSegments, ros::Time timestamp); + void getSegmentSearchStart(cv::Point2f &posWorld, float &angle) const; + bool segmentFitsToPrevious(Segment *previousSegment, Segment *segmentToAdd, bool isFirstSegment); + std::vector> getDrivingLinePts(); }; } // namespace drive_ros_image_recognition diff --git a/launch/line_detection.launch b/launch/line_detection.launch index da27cb7..87b3074 100644 --- a/launch/line_detection.launch +++ b/launch/line_detection.launch @@ -2,12 +2,13 @@ + - + @@ -16,6 +17,7 @@ + @@ -35,4 +37,7 @@ + + + diff --git a/src/line_detection.cpp b/src/line_detection.cpp index 3ff520d..7f7d203 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -24,6 +24,7 @@ LineDetection::LineDetection(const ros::NodeHandle nh, const ros::NodeHandle pnh , image_operator_() , dsrv_server_() , dsrv_cb_(boost::bind(&LineDetection::reconfigureCB, this, _1, _2)) + , roadModel(&tfListener_) { } @@ -43,6 +44,9 @@ bool LineDetection::init() { imageSubscriber_ = imageTransport_.subscribe("/img_in", 3, &LineDetection::imageCallback, this); ROS_INFO_STREAM("Subscribed image transport to topic " << imageSubscriber_.getTopic()); + odometrySub = pnh_.subscribe("odom_topic", 3, &LineDetection::odometryCallback, this); + ROS_INFO("Subscribing to odometry on topic '%s'", odometrySub.getTopic().c_str()); + line_output_pub_ = nh_.advertise("roadLine", 10); ROS_INFO_STREAM("Advertising road line on " << line_output_pub_.getTopic()); @@ -60,6 +64,10 @@ bool LineDetection::init() { return true; } +void LineDetection::odometryCallback(const nav_msgs::OdometryConstPtr &odomMsg) { + latestOdometry = *odomMsg; +} + /// /// \brief LineDetection::imageCallback /// Called for incoming camera image. Extracts the image from the incoming message. @@ -75,6 +83,8 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { } imgHeight_ = homographedImg.rows; imgWidth_ = homographedImg.cols; +// imgTimestamp = imgIn->header.stamp; + imgTimestamp = ros::Time::now(); #ifdef PUBLISH_DEBUG cv::cvtColor(homographedImg, debugImg_, CV_GRAY2RGB); @@ -103,13 +113,23 @@ void LineDetection::findLaneMarkings(std::vector &lines) { std::vector foundSegments; std::vector unusedLines, leftMarkings, midMarkings, rightMarkings, otherMarkings; std::vector regions; + std::vector worldPts, imgPts; // ROS_INFO("------------- New image ------------------"); for(int i = 0; i < lines.size(); i++) { unusedLines.push_back(&lines.at(i)); } - roadModel.getFirstPosW(segStartWorld, segAngle); + // ================================ + // Find lane in new image + // ================================ + roadModel.getSegmentSearchStart(segStartWorld, segAngle); + + // DEBUG + worldPts.push_back(segStartWorld); + image_operator_.worldToWarpedImg(worldPts, imgPts); + cv::circle(debugImg_, imgPts.at(0), 5, cv::Scalar(0,255), 2, cv::LINE_AA); + while((totalSegLength < maxSenseRange_) || findIntersectionExit) { // clear the marking vectors. Otherwise the old ones stay in there. leftMarkings.clear(); @@ -120,7 +140,10 @@ void LineDetection::findLaneMarkings(std::vector &lines) { regions = buildRegions(segStartWorld, segAngle); assignLinesToRegions(®ions, unusedLines, leftMarkings, midMarkings, rightMarkings, otherMarkings); auto seg = findLaneWithRansac(leftMarkings, midMarkings, rightMarkings, segStartWorld, segAngle); - if(segmentIsPlausible(&seg, foundSegments.empty())) { + // If this is the first segment we can use nullptr as argument for previousSegment + if(roadModel.segmentFitsToPrevious( + foundSegments.empty() ? nullptr : &foundSegments.back(), + &seg, foundSegments.empty())) { foundSegments.push_back(seg); segAngle = seg.angleTotal; // the current angle of the lane } else { @@ -134,13 +157,13 @@ void LineDetection::findLaneMarkings(std::vector &lines) { // ================================ - // Search for a stop line + // Search for an intersection // ================================ float stopLineAngle; Segment intersectionSegment; findIntersectionExit = false; if(findIntersection(intersectionSegment, segAngle, segStartWorld, leftMarkings, midMarkings, rightMarkings)) { - if(segmentIsPlausible(&intersectionSegment, false)) { + if(roadModel.segmentFitsToPrevious(&foundSegments.back(), &intersectionSegment, false)) { foundSegments.push_back(intersectionSegment); totalSegLength += intersectionSegment.length; segAngle = intersectionSegment.angleTotal; // the current angle of the lane @@ -159,37 +182,52 @@ void LineDetection::findLaneMarkings(std::vector &lines) { } // ROS_INFO("Detection range = %.2f", totalSegLength); - roadModel.addSegments(foundSegments); + roadModel.addSegments(foundSegments, imgTimestamp); #ifdef PUBLISH_DEBUG - if(foundSegments.size() > 0) { - // ego lane in orange - cv::circle(debugImg_, foundSegments.at(0).positionImage, 5, cv::Scalar(255,128), 2, cv::LINE_AA); - // left lane marking in purple - cv::circle(debugImg_, foundSegments.at(0).leftPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); - // right lane marking in purple - cv::circle(debugImg_, foundSegments.at(0).rightPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); - // mid lane marking in yellow - cv::circle(debugImg_, foundSegments.at(0).midPosI, 3, cv::Scalar(255,255), 1, cv::LINE_AA); - } + worldPts.clear(); + imgPts.clear(); + auto transformedLane = roadModel.getDrivingLinePts(); + if(!transformedLane.empty()) { + for(int i = 0; i < transformedLane.size(); i++) { + worldPts.push_back(cv::Point2f(transformedLane.at(i).x(), transformedLane.at(i).y())); + } + image_operator_.worldToWarpedImg(worldPts, imgPts); - for(int i = 1; i < foundSegments.size(); i++) { - // ego lane in orange - cv::circle(debugImg_, foundSegments.at(i).positionImage, 5, cv::Scalar(255,128), 2, cv::LINE_AA); - cv::line(debugImg_, foundSegments.at(i-1).positionImage, foundSegments.at(i).positionImage, cv::Scalar(255,128), 2, cv::LINE_AA); - if(!foundSegments.at(i).isIntersection() && !foundSegments.at(i-1).isIntersection()) { - // left lane marking in purple - cv::line(debugImg_, foundSegments.at(i-1).leftPosI, foundSegments.at(i).leftPosI, cv::Scalar(153,0,204), 2, cv::LINE_AA); - cv::circle(debugImg_, foundSegments.at(i).leftPosI, 3, cv::Scalar(153, 0, 204), 1, cv::LINE_AA); - // right lane marking in purple - cv::line(debugImg_, foundSegments.at(i-1).rightPosI, foundSegments.at(i).rightPosI, cv::Scalar(153,0,204), 2, cv::LINE_AA); - cv::circle(debugImg_, foundSegments.at(i).rightPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); - // mid lane marking in yellow - cv::line(debugImg_, foundSegments.at(i-1).midPosI, foundSegments.at(i).midPosI, cv::Scalar(255,255), 2, cv::LINE_AA); - cv::circle(debugImg_, foundSegments.at(i).midPosI, 3, cv::Scalar(255,255), 1, cv::LINE_AA); + for(int i = 1; i < imgPts.size(); i++) { + cv::circle(debugImg_, imgPts.at(i), 5, cv::Scalar(255,128), 2, cv::LINE_AA); + cv::line(debugImg_, imgPts.at(i-1), imgPts.at(i), cv::Scalar(255,128), 2, cv::LINE_AA); } } +// if(foundSegments.size() > 0) { +// // ego lane in orange +// cv::circle(debugImg_, foundSegments.at(0).positionImage, 5, cv::Scalar(255,128), 2, cv::LINE_AA); +// // left lane marking in purple +// cv::circle(debugImg_, foundSegments.at(0).leftPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); +// // right lane marking in purple +// cv::circle(debugImg_, foundSegments.at(0).rightPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); +// // mid lane marking in yellow +// cv::circle(debugImg_, foundSegments.at(0).midPosI, 3, cv::Scalar(255,255), 1, cv::LINE_AA); +// } +// +// for(int i = 1; i < foundSegments.size(); i++) { +// // ego lane in orange +// cv::circle(debugImg_, foundSegments.at(i).positionImage, 5, cv::Scalar(255,128), 2, cv::LINE_AA); +// cv::line(debugImg_, foundSegments.at(i-1).positionImage, foundSegments.at(i).positionImage, cv::Scalar(255,128), 2, cv::LINE_AA); +// if(!foundSegments.at(i).isIntersection() && !foundSegments.at(i-1).isIntersection()) { +// // left lane marking in purple +// cv::line(debugImg_, foundSegments.at(i-1).leftPosI, foundSegments.at(i).leftPosI, cv::Scalar(153,0,204), 2, cv::LINE_AA); +// cv::circle(debugImg_, foundSegments.at(i).leftPosI, 3, cv::Scalar(153, 0, 204), 1, cv::LINE_AA); +// // right lane marking in purple +// cv::line(debugImg_, foundSegments.at(i-1).rightPosI, foundSegments.at(i).rightPosI, cv::Scalar(153,0,204), 2, cv::LINE_AA); +// cv::circle(debugImg_, foundSegments.at(i).rightPosI, 3, cv::Scalar(153,0,204), 1, cv::LINE_AA); +// // mid lane marking in yellow +// cv::line(debugImg_, foundSegments.at(i-1).midPosI, foundSegments.at(i).midPosI, cv::Scalar(255,255), 2, cv::LINE_AA); +// cv::circle(debugImg_, foundSegments.at(i).midPosI, 3, cv::Scalar(255,255), 1, cv::LINE_AA); +// } +// } + // for(auto m : unusedLines) { // cv::line(debugImg_, m->iP1_, m->iP2_, cv::Scalar(0,180), 2, cv::LINE_AA); // } @@ -559,7 +597,7 @@ bool LineDetection::findIntersection(Segment &resultingSegment, float segmentAng SegmentType intersectionType; float drivingDirectionAngle = segmentAngle; if(middleExists && (leftExists || rightExists)) { - ROS_INFO("Intersection found - stop"); +// ROS_INFO("Intersection found - stop"); foundIntersection = true; intersectionType = SegmentType::INTERSECTION_STOP; stopLineAngle = stopLineAngle / static_cast(numStopLines); @@ -568,7 +606,7 @@ bool LineDetection::findIntersection(Segment &resultingSegment, float segmentAng laneDir = Eigen::Vector2f(cos(drivingDirectionAngle), sin(drivingDirectionAngle)); } if(!middleExists && leftExists && rightExists) { - ROS_INFO("Intersection found - do not stop"); +// ROS_INFO("Intersection found - do not stop"); foundIntersection = true; intersectionType = SegmentType::INTERSECTION_GO_STRAIGHT; laneDir = Eigen::Vector2f(cos(segmentAngle), sin(segmentAngle)); @@ -589,22 +627,6 @@ bool LineDetection::findIntersection(Segment &resultingSegment, float segmentAng return foundIntersection; } -bool LineDetection::segmentIsPlausible(Segment *segmentToAdd, bool isFirstSegment) { - if(segmentToAdd-> probablity < 0.2f) { -// ROS_INFO("Segments probability is too low"); - return false; - } - if(!isFirstSegment) { - float angleVar = M_PI / 7.0f; // TODO move to config - if(std::abs(segmentToAdd->angleDiff) > angleVar) { -// ROS_INFO_STREAM("Too much angle difference: " << (segmentToAdd->angleDiff * 180.f / M_PI) << "[deg]"); - return false; - } - } - - return true; -} - /// /// \brief LineDetection::findLinesWithHough /// Extract Lines from the image using Hough Lines diff --git a/src/road_model.cpp b/src/road_model.cpp index c9add07..8ecc229 100644 --- a/src/road_model.cpp +++ b/src/road_model.cpp @@ -3,20 +3,84 @@ namespace drive_ros_image_recognition { -void RoadModel::getFirstPosW(cv::Point2f &posW, float &angle) const { - for(auto s : segments) { +void RoadModel::getSegmentSearchStart(cv::Point2f &posWorld, float &angle) const { + // DEBUG + for(int i = 0; i < drivingLine.size(); i++) { + if(drivingLine.at(i).isIntersection()) { + ROS_INFO_STREAM("=== SearchStart: IntersectionSegment is at " << i << "/" << drivingLine.size()); + } + } + + auto intersectionSegment = std::find_if(drivingLine.begin(), drivingLine.end(), [](Segment s){ return s.isIntersection(); }); + // If an intersection exists, start searching for segments after it + if(intersectionSegment != drivingLine.end()) { + if(intersectionSegment == (drivingLine.end() - 1)) { + ROS_INFO("Create the intersection exit point"); + // We do not have any segment after the intersection + tf::Stamped p; + + try { + pTfListener->transformPoint("/rear_axis_middle_ground", intersectionSegment->odomPosition, p); + } + catch (tf::TransformException &ex){ + ROS_ERROR("%s",ex.what()); + } + + // Guess the intersection exit position + posWorld.x = p.x() + (1.f * cos(intersectionSegment->angleTotal)); + posWorld.y = p.y() + (1.f * sin(intersectionSegment->angleTotal)); + angle = intersectionSegment->angleTotal; + ROS_INFO("(%f, %f)", posWorld.x, posWorld.y); + return; + + } else { + ROS_INFO("Return segment after intersection"); + // Select the first segment after the intersection + auto searchStartSeg = intersectionSegment + 1; + + tf::Stamped p; + + try { + pTfListener->transformPoint("/rear_axis_middle_ground", searchStartSeg->odomPosition, p); + } + catch (tf::TransformException &ex){ + ROS_ERROR("%s",ex.what()); + } + + posWorld.x = p.x(); + posWorld.y = p.y(); + angle = searchStartSeg->angleTotal; + ROS_INFO("(%f, %f)", posWorld.x, posWorld.y); + return; + } + } + + // If there is no intersection select the first segment + for(auto s : drivingLine) { + tf::Stamped p; + + try { + pTfListener->transformPoint("/rear_axis_middle_ground", s.odomPosition, p); + } + catch (tf::TransformException &ex){ + ROS_ERROR("%s",ex.what()); + continue; + } + // Find the first segment which is in front of the car - if(s.positionWorld.x > 0.1f) { + if(p.x() > 0.2f && (std::abs(p.y()) < 0.3f)) { // If the segment is too far away translate if towards the car - if(s.positionWorld.x > 0.5f) { -// ROS_WARN_STREAM("First segment is too far away: " << s.positionWorld); - float moveDistance = sqrt(s.positionWorld.x*s.positionWorld.x + s.positionWorld.y*s.positionWorld.y); + if(p.x() > 0.5f) { + // ROS_WARN_STREAM("First segment is too far away: " << s.positionWorld); + float moveDistance = sqrt(p.x()*p.x() + p.y()*p.y()); moveDistance -= 0.3f; - posW.x = s.positionWorld.x - (moveDistance * cos(s.angleTotal)); - posW.y = s.positionWorld.y - (moveDistance * sin(s.angleTotal)); + posWorld.x = p.x() - (moveDistance * cos(s.angleTotal)); + posWorld.y = p.y() - (moveDistance * sin(s.angleTotal)); + } else { - posW = s.positionWorld; + posWorld.x = p.x(); + posWorld.y = p.y(); } angle = s.angleTotal; @@ -25,24 +89,191 @@ void RoadModel::getFirstPosW(cv::Point2f &posW, float &angle) const { } // If we have not found a suitable segment, return some default values - posW.x = 0.3f; - posW.y = 0.f; + posWorld.x = 0.3f; + posWorld.y = 0.f; angle = 0.f; return; } -void RoadModel::addSegments(std::vector &newSegments) { -// ROS_INFO_STREAM(segments.size() << " old segments and " << newSegments.size() << " new ones"); - for(int i = 0; (i < newSegments.size()) && (i < segments.size()); i++) { - auto deltaAngleDiff = std::abs(newSegments.at(i).angleDiff - segments.at(i).angleDiff); - auto deltaPosition = newSegments.at(i).positionWorld - segments.at(i).positionWorld; -// ROS_INFO_STREAM("(" << i << ") deltaAngleDiff = " << (deltaAngleDiff * 180.f / M_PI) << " deltaPosition = " << deltaPosition); +std::vector> RoadModel::getDrivingLinePts() { + // DEBUG + for(int i = 0; i < drivingLine.size(); i++) { + if(drivingLine.at(i).isIntersection()) { + ROS_INFO_STREAM("=== DrivingLinePts: IntersectionSegment is at " << i << "/" << drivingLine.size()); + } + } + + std::vector> drivingLinePts; + float drivingLineLength = 0.f; + + // If we found an intersection aim for the exit + // TODO: actually, we should aim for the entry (and check if we have to stop) and then aim for the exit +// auto intersectionSegment = std::find_if(drivingLine.begin(), drivingLine.end(), [](Segment s){ return s.isIntersection(); }); +// if(intersectionSegment != drivingLine.end()) { +// ROS_INFO("Intersection (%s) exists - aim for exit", +// intersectionSegment->segmentType == SegmentType::INTERSECTION_GO_STRAIGHT ? "GO" : "STOP"); +// +// if(intersectionSegment == (drivingLine.end() - 1)) { +// // We do not have any segment after the intersection +// tf::Stamped p; +// +// try { +// pTfListener->transformPoint("/rear_axis_middle_ground", intersectionSegment->odomPosition, p); +// } +// catch (tf::TransformException &ex){ +// ROS_ERROR("%s",ex.what()); +// } +// +// // Guess the intersection exit position +// tf::Stamped interpolatedPt; +// interpolatedPt.setX(p.x() + (1.f * cos(intersectionSegment->angleTotal))); +// interpolatedPt.setY(p.y() + (1.f * sin(intersectionSegment->angleTotal))); +// drivingLinePts.push_back(interpolatedPt); +// auto dist = sqrt(interpolatedPt.x()*interpolatedPt.x() + interpolatedPt.y()*interpolatedPt.y()); +// drivingLineLength = dist; +// +// } else { +// // Select the first segment after the intersection +// auto searchStartSeg = intersectionSegment + 1; +// +// tf::Stamped p; +// +// try { +// pTfListener->transformPoint("/rear_axis_middle_ground", searchStartSeg->odomPosition, p); +// } +// catch (tf::TransformException &ex){ +// ROS_ERROR("%s",ex.what()); +// } +// +// auto dist = sqrt(p.x()*p.x() + p.y()*p.y()); +// drivingLineLength = dist; +// drivingLinePts.push_back(p); +// } +// } + + // The actual driving line (if an intersection exists, we ignore segments before it) + for(auto s : drivingLine) { + tf::Stamped p; + + try { + pTfListener->transformPoint("/rear_axis_middle_ground", s.odomPosition, p); + } + catch (tf::TransformException &ex){ + ROS_ERROR("%s",ex.what()); + continue; + } + + auto dist = sqrt(p.x()*p.x() + p.y()*p.y()); + if(dist > drivingLineLength) { + drivingLinePts.push_back(p); + drivingLineLength = dist; + } + } + + return drivingLinePts; +} + +bool RoadModel::segmentFitsToPrevious(Segment *previousSegment, Segment *segmentToAdd, bool isFirstSegment) { + if(segmentToAdd->probablity < 0.2f) { +// ROS_INFO("Segments probability is too low"); + return false; + } + if(!isFirstSegment) { + float angleVar = M_PI / 7.0f; // TODO move to config + if(std::abs(previousSegment->angleTotal - segmentToAdd->angleTotal) > angleVar) { +// ROS_INFO_STREAM("Too much angle difference: " << (segmentToAdd->angleDiff * 180.f / M_PI) << "[deg]"); + return false; + } + } else { + // The first segment should points into the cars driving direction + if(std::abs(segmentToAdd->angleTotal) > M_PI / 4.0f) { + ROS_WARN("First segments angle is too big: %.1f[deg]", segmentToAdd->angleTotal * 180.f / M_PI); + return false; + } + } + + return true; +} + +void RoadModel::addSegments(std::vector &newSegments, ros::Time timestamp) { + // Transform drivingLine to /rear_axis_middle_ground frame + // TODO + + // Compare the current driving line with the new segment + // TODO + + // Add the new segments to the driving line + std::vector updatedDrivingLine; + float drivingLineLength = 0.f; + + for(auto s : newSegments) { + // The new segments already had a plausibility check + + // Transform the points to the /odom frame + tf::Stamped p; + p.frame_id_ = "/rear_axis_middle_ground"; + p.stamp_ = timestamp; + p.setX(s.positionWorld.x); + p.setY(s.positionWorld.y); + p.setZ(0.0); + +// target_frame The frame into which to transform +// source_frame The frame from which to transform +// time The time at which to transform +// timeout How long to block before failing + if(pTfListener->waitForTransform("/odom", p.frame_id_, timestamp, ros::Duration(0.1))) { + try { + pTfListener->transformPoint("/odom", p, s.odomPosition); + + } + catch (tf::TransformException &ex){ + ROS_ERROR("%s",ex.what()); + continue; + } + + updatedDrivingLine.push_back(s); + drivingLineLength = sqrt(p.x()*p.x() + p.y()*p.y()); + } else { + ROS_ERROR("waitForTransform timed out in RoadModel::addSegments"); + break; + } + } + + // If the new driving line is too short add some segments from the previous one + if(drivingLineLength < 1.0f) { + for(auto s : drivingLine) { + tf::Stamped p; + + // Transform the point from /odom to /rear_axis_middle_ground + try { + pTfListener->transformPoint("/rear_axis_middle_ground", s.odomPosition, p); + } + catch (tf::TransformException &ex){ + ROS_ERROR("%s",ex.what()); + continue; + } + + auto dist = sqrt(p.x()*p.x() + p.y()*p.y()); + if(dist > drivingLineLength) { + // Only use the previous driving line up to a certain distance + if(dist < 1.8f) { + // Check if the old segments fits to the current driving line + if(segmentFitsToPrevious( + updatedDrivingLine.empty() ? nullptr : &updatedDrivingLine.back(), + &s, updatedDrivingLine.empty())) { + updatedDrivingLine.push_back(s); + } + + } else { + // Break if the old segment is too far away + break; + } + } + } } - segments.clear(); - segments.insert(segments.begin(), newSegments.begin(), newSegments.end()); -// ROS_INFO_STREAM("Num segments: " << segments.size()); + drivingLine = updatedDrivingLine; } } // namespace drive_ros_image_recognition From f2f15b7bf845e990d3fc082f68573eb675f3a3bc Mon Sep 17 00:00:00 2001 From: Simon Weigl Date: Sat, 24 Nov 2018 14:06:03 +0100 Subject: [PATCH 91/94] Create a simple trajectory. --- cfg/LineDetection.cfg | 2 + .../line_detection.h | 6 ++- src/line_detection.cpp | 46 +++++++++++++++++-- 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/cfg/LineDetection.cfg b/cfg/LineDetection.cfg index 86195ad..f4bca4b 100755 --- a/cfg/LineDetection.cfg +++ b/cfg/LineDetection.cfg @@ -15,6 +15,8 @@ gen.add("houghMinLineLen", int_t, 0, "houghMinLineLen", 5, 1, 100) gen.add("houghMaxLineGap", int_t, 0, "houghMaxLineGap", 3, 0, 300) gen.add("segmentLength", double_t, 0, "Classification segment length", 0.2, 0.1, 1.0) gen.add("ransacIterations", int_t, 0, "Max number of Ransac iterations", 200, 50, 1000) +gen.add("trajectory_dist", double_t, 0, "Distance we want the trajectory point to be", 0.3, 0.1, 1.0) +gen.add("target_velocity", double_t, 0, "The velocity for our trajectory", 0.01, 0.00, 10.0) exit(gen.generate(PACKAGE, "drive_ros_image_recognition", "LineDetection")) diff --git a/include/drive_ros_image_recognition/line_detection.h b/include/drive_ros_image_recognition/line_detection.h index 2cbd5af..9a216fc 100644 --- a/include/drive_ros_image_recognition/line_detection.h +++ b/include/drive_ros_image_recognition/line_detection.h @@ -36,6 +36,8 @@ class LineDetection { int houghMaxLineGap_; float segmentLength_; size_t maxRansacInterations_; + float trajectoryDist; + float targetVelocity; // variables RoadModel roadModel; @@ -51,6 +53,7 @@ class LineDetection { image_transport::ImageTransport imageTransport_; image_transport::Subscriber imageSubscriber_; ros::Publisher line_output_pub_; + ros::Publisher trajectoryPub; ros::Subscriber homography_params_sub_; ros::Subscriber odometrySub; #ifdef PUBLISH_DEBUG @@ -81,7 +84,7 @@ class LineDetection { // methods void findLinesWithHough(cv::Mat &img, std::vector &houghLines); - void findLaneMarkings(std::vector &lines); + std::vector> findLaneMarkings(std::vector &lines); // helper functions float getDistanceBetweenPoints(const cv::Point2f a, const cv::Point2f b) { @@ -99,6 +102,7 @@ class LineDetection { Segment findLaneWithRansac(std::vector &leftMarkings, std::vector &midMarkings, std::vector &rightMarkings, cv::Point2f pos, float prevAngle); bool findIntersection(Segment &resultingSegment, float segmentAngle, cv::Point2f segStartWorld, std::vector &leftMarkings, std::vector &midMarkings, std::vector &rightMarkings); + cv::Point2f findTrajectoryPoint(std::vector> &drivingLine); #ifdef PUBLISH_DEBUG void drawDebugLines(std::vector &lines); diff --git a/src/line_detection.cpp b/src/line_detection.cpp index 7f7d203..a4eb70e 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -9,6 +9,7 @@ #include "drive_ros_image_recognition/line_detection.h" #include "drive_ros_image_recognition/geometry_common.h" #include "drive_ros_msgs/RoadLine.h" +#include "drive_ros_msgs/simple_trajectory.h" namespace drive_ros_image_recognition { @@ -50,6 +51,9 @@ bool LineDetection::init() { line_output_pub_ = nh_.advertise("roadLine", 10); ROS_INFO_STREAM("Advertising road line on " << line_output_pub_.getTopic()); + trajectoryPub = nh_.advertise("trajectory_point", 1); + ROS_INFO("Publish trajectory point on topic '%s'", trajectoryPub.getTopic().c_str()); + #ifdef PUBLISH_DEBUG debugImgPub_ = imageTransport_.advertise("debug_image", 3); ROS_INFO_STREAM("Publishing debug image on topic " << debugImgPub_.getTopic()); @@ -92,8 +96,16 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { std::vector linesInImage; findLinesWithHough(homographedImg, linesInImage); + auto drivingLine = findLaneMarkings(linesInImage); + - findLaneMarkings(linesInImage); + // Publish the trajectory + cv::Point2f trajPoint = findTrajectoryPoint(drivingLine); + drive_ros_msgs::simple_trajectory trajMsg; + trajMsg.v = targetVelocity; + trajMsg.phi = atan2(trajPoint.x, trajPoint.y); + + trajectoryPub.publish(trajMsg); #ifdef PUBLISH_DEBUG debugImgPub_.publish(cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, debugImg_).toImageMsg()); @@ -105,7 +117,7 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { // ROS_INFO_STREAM("Cycle time: " << (std::chrono::duration(t_end-t_start).count()) << "ms"); } -void LineDetection::findLaneMarkings(std::vector &lines) { +std::vector> LineDetection::findLaneMarkings(std::vector &lines) { cv::Point2f segStartWorld(0.3f, 0.f); float segAngle = 0.f; float totalSegLength = 0.f; @@ -184,10 +196,12 @@ void LineDetection::findLaneMarkings(std::vector &lines) { // ROS_INFO("Detection range = %.2f", totalSegLength); roadModel.addSegments(foundSegments, imgTimestamp); + auto transformedLane = roadModel.getDrivingLinePts(); + #ifdef PUBLISH_DEBUG worldPts.clear(); imgPts.clear(); - auto transformedLane = roadModel.getDrivingLinePts(); + if(!transformedLane.empty()) { for(int i = 0; i < transformedLane.size(); i++) { worldPts.push_back(cv::Point2f(transformedLane.at(i).x(), transformedLane.at(i).y())); @@ -232,6 +246,8 @@ void LineDetection::findLaneMarkings(std::vector &lines) { // cv::line(debugImg_, m->iP1_, m->iP2_, cv::Scalar(0,180), 2, cv::LINE_AA); // } #endif + + return transformedLane; } /// @@ -627,6 +643,28 @@ bool LineDetection::findIntersection(Segment &resultingSegment, float segmentAng return foundIntersection; } +cv::Point2f LineDetection::findTrajectoryPoint(std::vector> &drivingLine) { + if(drivingLine.empty()) { + return cv::Point2f(1.f, 0.f); + } + + for(int i = 1; i < drivingLine.size(); i++) { + auto fst = drivingLine.at(i-1); + auto snd = drivingLine.at(i); + if((snd.x()*snd.x() + snd.y()*snd.y()) > trajectoryDist*trajectoryDist) { + cv::Point2f direction(snd.x() - fst.x(), snd.y() - fst.y()); + direction /= (sqrt(direction.x*direction.x + direction.y*direction.y)); + float distToFst = sqrt(fst.x()*fst.x() + fst.y()*fst.y()); + float interpolationLen = trajectoryDist - distToFst; + cv::Point2f finalTrajPoint(fst.x() + direction.x*interpolationLen, fst.y() + direction.y*interpolationLen); + return finalTrajPoint; + } + } + + auto lastPt = drivingLine.back(); + return cv::Point2f(lastPt.x(), lastPt.y()); +} + /// /// \brief LineDetection::findLinesWithHough /// Extract Lines from the image using Hough Lines @@ -722,6 +760,8 @@ void LineDetection::reconfigureCB(drive_ros_image_recognition::LineDetectionConf houghMaxLineGap_ = config.houghMaxLineGap; segmentLength_ = config.segmentLength; maxRansacInterations_ = config.ransacIterations; + trajectoryDist = config.trajectory_dist; + targetVelocity = config.target_velocity; } } // namespace drive_ros_image_recognition From af7fbff5e55d4a91fcd24620a546d4e7e2dc0b93 Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sat, 24 Nov 2018 18:18:46 +0100 Subject: [PATCH 92/94] Trajectory steering angle --- src/line_detection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/line_detection.cpp b/src/line_detection.cpp index a4eb70e..aa4fc59 100644 --- a/src/line_detection.cpp +++ b/src/line_detection.cpp @@ -103,7 +103,7 @@ void LineDetection::imageCallback(const sensor_msgs::ImageConstPtr &imgIn) { cv::Point2f trajPoint = findTrajectoryPoint(drivingLine); drive_ros_msgs::simple_trajectory trajMsg; trajMsg.v = targetVelocity; - trajMsg.phi = atan2(trajPoint.x, trajPoint.y); + trajMsg.phi = atan2(trajPoint.y, trajPoint.x); trajectoryPub.publish(trajMsg); From 3d64376656ebecdb8ae276932cea0b628aa1a73f Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Mon, 3 Dec 2018 16:01:18 +0100 Subject: [PATCH 93/94] Directly use provided homography (as the published one is already scaled now) --- config/homography.yaml | 2 +- include/drive_ros_image_recognition/common_image_operations.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/config/homography.yaml b/config/homography.yaml index d322600..99ea68c 100644 --- a/config/homography.yaml +++ b/config/homography.yaml @@ -1,2 +1,2 @@ -world_size: [5.0, 5.0] +world_size: [3.0, 3.0] image_size: [800, 800] diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 089ef94..2ee0585 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -301,7 +301,7 @@ class TransformHelper { return false; } dstImg = cv::Mat::zeros( srcImg.rows, srcImg.cols, srcImg.type() ); - cv::warpPerspective(srcImg, dstImg, scaledCam2world_, transformed_size_); + cv::warpPerspective(srcImg, dstImg, cam2world_, transformed_size_); return true; } From feaa51bf509765ab53fe21ae1d7f1571611f5d8d Mon Sep 17 00:00:00 2001 From: MaslinuPoimal Date: Sun, 9 Dec 2018 12:37:29 +0100 Subject: [PATCH 94/94] Use unscaled homography --- include/drive_ros_image_recognition/common_image_operations.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/drive_ros_image_recognition/common_image_operations.h b/include/drive_ros_image_recognition/common_image_operations.h index 2ee0585..089ef94 100644 --- a/include/drive_ros_image_recognition/common_image_operations.h +++ b/include/drive_ros_image_recognition/common_image_operations.h @@ -301,7 +301,7 @@ class TransformHelper { return false; } dstImg = cv::Mat::zeros( srcImg.rows, srcImg.cols, srcImg.type() ); - cv::warpPerspective(srcImg, dstImg, cam2world_, transformed_size_); + cv::warpPerspective(srcImg, dstImg, scaledCam2world_, transformed_size_); return true; }