From f8182e2907923268440c5f96dc391512e9302e15 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Tue, 21 Mar 2017 17:22:08 -0500 Subject: [PATCH] removed build of monoxcal, added dynamic reconfigure to grid finder, fixed issues with opencv3 #89 --- industrial_extrinsic_cal/CMakeLists.txt | 14 +- .../cfg/circle_grid_finder.cfg | 19 + .../circle_detector.hpp | 18 +- .../ros_camera_observer.h | 19 +- .../src/camera_yaml_parser.cpp | 2 +- industrial_extrinsic_cal/src/ceres_blocks.cpp | 3 + .../src/circle_detector.cpp | 473 +++++++++--------- .../src/nodes/calibration_service.cpp | 2 +- .../src/ros_camera_observer.cpp | 160 +++--- 9 files changed, 388 insertions(+), 322 deletions(-) create mode 100755 industrial_extrinsic_cal/cfg/circle_grid_finder.cfg diff --git a/industrial_extrinsic_cal/CMakeLists.txt b/industrial_extrinsic_cal/CMakeLists.txt index ee05b40b..1f5981ff 100644 --- a/industrial_extrinsic_cal/CMakeLists.txt +++ b/industrial_extrinsic_cal/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS std_srvs tf tf_conversions + dynamic_reconfigure ) find_package(Boost REQUIRED) @@ -29,6 +30,9 @@ message("-- Found Ceres version ${CERES_VERSION}: ${CERES_INCLUDE_DIRS}") find_package(OpenCV REQUIRED) message("-- Found OpenCV version ${OpenCV_VERSION}: ${OpenCV_INCLUDE_DIRS}") + + + find_package(PkgConfig REQUIRED) pkg_check_modules(yaml_cpp REQUIRED yaml-cpp) if(NOT ${yaml_cpp_VERSION} VERSION_LESS "0.5") @@ -83,6 +87,10 @@ generate_messages( std_msgs ) +generate_dynamic_reconfigure_options( + cfg/circle_grid_finder.cfg +) + catkin_package( LIBRARIES @@ -142,7 +150,8 @@ add_library( src/target.cpp src/targets_yaml_parser.cpp ) -add_dependencies(industrial_extrinsic_cal ${PROJECT_NAME}_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) +add_dependencies(industrial_extrinsic_cal ${PROJECT_NAME}_generate_messages_cpp ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) + target_link_libraries( industrial_extrinsic_cal @@ -156,7 +165,6 @@ target_link_libraries( # targets: other nodes add_executable(camera_observer_scene_trigger src/nodes/camera_observer_scene_trigger.cpp) add_executable(manual_calt_adjust src/nodes/manual_calt_adjuster.cpp) -add_executable(mono_ex_cal src/nodes/mono_ex_cal.cpp) add_executable(mutable_joint_state_publisher src/nodes/mutable_joint_state_publisher.cpp) add_executable(nist_analysis src/nodes/nist_analysis.cpp) add_executable(ros_robot_trigger_action_service src/nodes/ros_robot_scene_trigger_action_server.cpp) @@ -171,7 +179,6 @@ add_dependencies(trigger_service ${catkin_EXPORTED_TARGETS} ${i target_link_libraries(camera_observer_scene_trigger industrial_extrinsic_cal ${catkin_LIBRARIES} ${yaml_cpp_LIBRARY} ${CERES_LIBRARIES}) target_link_libraries(manual_calt_adjust industrial_extrinsic_cal ${catkin_LIBRARIES}) -target_link_libraries(mono_ex_cal ${catkin_LIBRARIES} ${CERES_LIBRARIES}) target_link_libraries(mutable_joint_state_publisher ${catkin_LIBRARIES} ${yaml_cpp_LIBRARY}) target_link_libraries(nist_analysis industrial_extrinsic_cal ${catkin_LIBRARIES} ${CERES_LIBRARIES}) target_link_libraries(ros_robot_trigger_action_service ${catkin_LIBRARIES}) @@ -185,7 +192,6 @@ install( camera_observer_scene_trigger industrial_extrinsic_cal manual_calt_adjust - mono_ex_cal mutable_joint_state_publisher nist_analysis ros_robot_trigger_action_service diff --git a/industrial_extrinsic_cal/cfg/circle_grid_finder.cfg b/industrial_extrinsic_cal/cfg/circle_grid_finder.cfg new file mode 100755 index 00000000..7e6cee1f --- /dev/null +++ b/industrial_extrinsic_cal/cfg/circle_grid_finder.cfg @@ -0,0 +1,19 @@ +#!/usr/bin/env python +PACKAGE = "industrial_extrinsic_cal" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + + +gen.add("filter_by_area", bool_t, 0, "filter blobs by their area", True) +gen.add("filter_by_circularity", bool_t, 0, "filter blobs by their circularity", False) +gen.add("min_area", int_t, 0, "Minimum area of a blob", 1, 25, 5000) +gen.add("max_area", int_t, 0, "Maximum area of a blob", 1, 5000, 10000) +gen.add("min_circularity", double_t, 0, "The min aspect ratio necessary to be a circlular blob", .1, .8, 3.402823e38) +gen.add("max_circularity", double_t, 0, "The max aspect ratio necessary to be a circlular blob", .1, 3.402823e38, 3.402823e38) +gen.add("min_threshold", int_t, 0, "The contrast minimum threshold to be a blob", 1, 50, 1000) +gen.add("max_threshold", int_t, 0, "The contrast maximum threshold to be a blob", 1, 500, 1000) +gen.add("min_distance", double_t, 0, "The min distance between blobs", 1.0, 5.0, 500) + +exit(gen.generate(PACKAGE, "industrial_extrinsic_cal_node", "circle_grid_finder")) diff --git a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/circle_detector.hpp b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/circle_detector.hpp index 4322068f..d8e70347 100644 --- a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/circle_detector.hpp +++ b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/circle_detector.hpp @@ -86,23 +86,9 @@ class CV_EXPORTS_W CircleDetector : public FeatureDetector void write( FileStorage& fs ) const; }; - CV_WRAP CircleDetector(const CircleDetector::Params ¶meters = CircleDetector::Params()); + CV_WRAP static Ptr + create(const CircleDetector::Params ¶meters = CircleDetector::Params()); - virtual void read( const FileNode& fn ); - virtual void write( FileStorage& fs ) const; - -protected: - struct CV_EXPORTS Center - { - Point2d location; - double radius; - double confidence; - }; - - virtual void detectImpl( const Mat& image, vector& keypoints, const Mat& mask=Mat() ) const; - virtual void findCircles(const Mat &image, const Mat &binaryImage, vector
¢ers) const; - - Params params; }; } #endif diff --git a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ros_camera_observer.h b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ros_camera_observer.h index d8494481..db5bcf29 100644 --- a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ros_camera_observer.h +++ b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ros_camera_observer.h @@ -22,6 +22,8 @@ #include #include #include +#include +#include #include #include @@ -41,7 +43,7 @@ #include #include #include - +#include /** * @brief enumerator containing three options for the type of pattern to detect @@ -208,7 +210,12 @@ namespace industrial_extrinsic_cal /** * @brief circle_detector_ptr_ is a custom blob detector which localizes circles better than simple blob detection */ - cv::Ptr circle_detector_ptr_; + cv::Ptr circle_detector_ptr_; + + /** + * @brief blob_detector_ptr_ is a simple blob detector + */ + cv::Ptr blob_detector_ptr_; /** * @brief new_image_collected, set after the trigger is done @@ -291,15 +298,23 @@ namespace industrial_extrinsic_cal bool pullCameraInfo(double &fx, double &fy, double &cx, double &cy, double &k1, double &k2, double &k3, double &p1, double &p2, int &width, int &height); + void dynReConfCallBack(industrial_extrinsic_cal::circle_grid_finderConfig &config, uint32_t level); + private: + int image_number_; /**< a counter of images recieved */ cv::Mat last_raw_image_; /**< the image last received */ bool use_circle_detector_; bool white_blobs_; ros::ServiceClient client_; sensor_msgs::SetCameraInfo srv_; + ros::NodeHandle *rnh_; + dynamic_reconfigure::Server *server_; + }; + } //end industrial_extrinsic_cal namespace + #endif /* ROS_CAMERA_OBSERVER_H_ */ diff --git a/industrial_extrinsic_cal/src/camera_yaml_parser.cpp b/industrial_extrinsic_cal/src/camera_yaml_parser.cpp index 24ac0d9f..6df55c04 100644 --- a/industrial_extrinsic_cal/src/camera_yaml_parser.cpp +++ b/industrial_extrinsic_cal/src/camera_yaml_parser.cpp @@ -107,7 +107,7 @@ namespace industrial_extrinsic_cal { else{ temp_camera->pullTransform(); } - temp_camera->camera_observer_ = boost::make_shared(temp_topic); + temp_camera->camera_observer_ = boost::make_shared(temp_topic, temp_name); } catch (YAML::ParserException& e){ ROS_INFO_STREAM("Failed to read in moving cameras from yaml file "); diff --git a/industrial_extrinsic_cal/src/ceres_blocks.cpp b/industrial_extrinsic_cal/src/ceres_blocks.cpp index 7893113d..0141f570 100644 --- a/industrial_extrinsic_cal/src/ceres_blocks.cpp +++ b/industrial_extrinsic_cal/src/ceres_blocks.cpp @@ -453,6 +453,9 @@ bool CeresBlocks::writeAllStaticTransforms(string filePath) return false; }//end if writing to file outputFile << "\n"; + outputFile << "# this file might be empty\n"; + outputFile << "# It might contain static transform publishers that were updated through calibration\n"; + outputFile << "# Often, transform interfaces use some other mechanism to update themselves\n"; outputFile.close(); bool rtn = true; diff --git a/industrial_extrinsic_cal/src/circle_detector.cpp b/industrial_extrinsic_cal/src/circle_detector.cpp index 6e67c21d..ee6eb387 100644 --- a/industrial_extrinsic_cal/src/circle_detector.cpp +++ b/industrial_extrinsic_cal/src/circle_detector.cpp @@ -46,7 +46,24 @@ ** to provide the location of the circle **** ********************************************************************************************/ +#include "opencv2/features2d.hpp" +#include "opencv2/imgproc.hpp" + +#include "opencv2/core/utility.hpp" +//#include "opencv2/core/private.hpp" +#include "opencv2/core/ocl.hpp" +#include "opencv2/core/hal/hal.hpp" + +#include + +#ifdef HAVE_TEGRA_OPTIMIZATION +#include "opencv2/features2d/features2d_tegra.hpp" +#endif + #include "opencv2/opencv.hpp" + + + #include #include @@ -61,7 +78,31 @@ # endif #endif -using namespace cv; +namespace cv +{ + +class CV_EXPORTS_W CircleDetectorImpl : public CircleDetector +{ +public: + + explicit CircleDetectorImpl(const CircleDetector::Params ¶meters = CircleDetector::Params()); + + virtual void read( const FileNode& fn ); + virtual void write( FileStorage& fs ) const; + +protected: + struct CV_EXPORTS Center + { + Point2d location; + double radius; + double confidence; + }; + + virtual void detect( InputArray image, std::vector& keypoints, InputArray mask=noArray() ); + virtual void findCircles(InputArray image, InputArray binaryImage, std::vector
¢ers) const; + + Params params; +}; /* * CircleDetector @@ -87,12 +128,10 @@ CircleDetector::Params::Params() maxCircularity = std::numeric_limits::max(); filterByInertia = true; - //minInertiaRatio = 0.6; minInertiaRatio = 0.1f; maxInertiaRatio = std::numeric_limits::max(); filterByConvexity = true; - //minConvexity = 0.8; minConvexity = 0.95f; maxConvexity = std::numeric_limits::max(); } @@ -155,274 +194,242 @@ void CircleDetector::Params::write(cv::FileStorage& fs) const fs << "maxConvexity" << maxConvexity; } -CircleDetector::CircleDetector(const CircleDetector::Params ¶meters) : +CircleDetectorImpl::CircleDetectorImpl(const CircleDetector::Params ¶meters) : params(parameters) { } -void CircleDetector::read( const cv::FileNode& fn ) +void CircleDetectorImpl::read( const cv::FileNode& fn ) { params.read(fn); } -void CircleDetector::write( cv::FileStorage& fs ) const +void CircleDetectorImpl::write( cv::FileStorage& fs ) const { - params.write(fs); + writeFormat(fs); + params.write(fs); } -void CircleDetector::findCircles(const cv::Mat &image, const cv::Mat &binaryImage, vector
¢ers) const +void CircleDetectorImpl::findCircles(InputArray _image, InputArray _binaryImage, std::vector
¢ers) const { - (void)image; - centers.clear(); - - vector < vector > contours; - Mat tmpBinaryImage = binaryImage.clone(); - findContours(tmpBinaryImage, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); - - // loop on all contours - for (size_t contourIdx = 0; contourIdx < contours.size(); contourIdx++) + // CV_INSTRUMENT_REGION() + + Mat image = _image.getMat(); // Oh so much cleaner this way :( + Mat binaryImage = _binaryImage.getMat(); + + (void)image; + centers.clear(); + + vector < vector > contours; + Mat tmpBinaryImage = binaryImage.clone(); + findContours(tmpBinaryImage, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); + + // loop on all contours + for (size_t contourIdx = 0; contourIdx < contours.size(); contourIdx++) + { + // each if statement may eliminate a contour through the continue function + // some if statements may also set the confidence whose default is 1.0 + Center center; + center.confidence = 1; + Moments moms = moments(Mat(contours[contourIdx])); + if (params.filterByArea) { - // each if statement may eliminate a contour through the continue function - // some if statements may also set the confidence whose default is 1.0 - Center center; - center.confidence = 1; - Moments moms = moments(Mat(contours[contourIdx])); - if (params.filterByArea) - { - double area = moms.m00; - if (area < params.minArea || area >= params.maxArea) - continue; - } - - if (params.filterByCircularity) - { - double area = moms.m00; - double perimeter = arcLength(Mat(contours[contourIdx]), true); - double ratio = 4 * CV_PI * area / (perimeter * perimeter); - if (ratio < params.minCircularity || ratio >= params.maxCircularity) - continue; - } - - if (params.filterByInertia) - { - double denominator = sqrt(pow(2 * moms.mu11, 2) + pow(moms.mu20 - moms.mu02, 2)); - const double eps = 1e-2; - double ratio; - if (denominator > eps) - { - double cosmin = (moms.mu20 - moms.mu02) / denominator; - double sinmin = 2 * moms.mu11 / denominator; - double cosmax = -cosmin; - double sinmax = -sinmin; - - double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin; - double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax; - ratio = imin / imax; - } - else - { - ratio = 1; - } - - if (ratio < params.minInertiaRatio || ratio >= params.maxInertiaRatio) - continue; - - center.confidence = ratio * ratio; - } - - if (params.filterByConvexity) - { - vector < Point > hull; - convexHull(Mat(contours[contourIdx]), hull); - double area = contourArea(Mat(contours[contourIdx])); - double hullArea = contourArea(Mat(hull)); - double ratio = area / hullArea; - if (ratio < params.minConvexity || ratio >= params.maxConvexity) - continue; - } - Mat pointsf; - Mat(contours[contourIdx]).convertTo(pointsf, CV_32F); - if(pointsf.rows<5) continue; - RotatedRect box = fitEllipse(pointsf); - - - // find center - //center.location = Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00); - center.location = box.center; - - - // one more filter by color of central pixel - if (params.filterByColor) - { - if (binaryImage.at (cvRound(center.location.y), cvRound(center.location.x)) != params.circleColor) - continue; - } - - //compute circle radius - // { - // vector dists; - // for (size_t pointIdx = 0; pointIdx < contours[contourIdx].size(); pointIdx++) - // { - // Point2d pt = contours[contourIdx][pointIdx]; - // dists.push_back(norm(center.location - pt)); - // } - // std::sort(dists.begin(), dists.end()); - // center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.; - //} - center.radius = (box.size.height+box.size.width)/4.0; - centers.push_back(center); - + double area = moms.m00; + if (area < params.minArea || area >= params.maxArea) + continue; + } + + if (params.filterByCircularity) + { + double area = moms.m00; + double perimeter = arcLength(Mat(contours[contourIdx]), true); + double ratio = 4 * CV_PI * area / (perimeter * perimeter); + if (ratio < params.minCircularity || ratio >= params.maxCircularity) + continue; + } + + if (params.filterByInertia) + { + double denominator = sqrt(pow(2 * moms.mu11, 2) + pow(moms.mu20 - moms.mu02, 2)); + const double eps = 1e-2; + double ratio; + if (denominator > eps) + { + double cosmin = (moms.mu20 - moms.mu02) / denominator; + double sinmin = 2 * moms.mu11 / denominator; + double cosmax = -cosmin; + double sinmax = -sinmin; + + double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin; + double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax; + ratio = imin / imax; + } + else + { + ratio = 1; + } + + if (ratio < params.minInertiaRatio || ratio >= params.maxInertiaRatio) + continue; + + center.confidence = ratio * ratio; + } + + if (params.filterByConvexity) + { + vector < Point > hull; + convexHull(Mat(contours[contourIdx]), hull); + double area = contourArea(Mat(contours[contourIdx])); + double hullArea = contourArea(Mat(hull)); + double ratio = area / hullArea; + if (ratio < params.minConvexity || ratio >= params.maxConvexity) + continue; + } + Mat pointsf; + Mat(contours[contourIdx]).convertTo(pointsf, CV_32F); + if(pointsf.rows<5) continue; + RotatedRect box = fitEllipse(pointsf); + + + // find center + //center.location = Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00); + center.location = box.center; + + + // one more filter by color of central pixel + if (params.filterByColor) + { + if (binaryImage.at (cvRound(center.location.y), cvRound(center.location.x)) != params.circleColor) + continue; + } + + //compute circle radius + // { + // vector dists; + // for (size_t pointIdx = 0; pointIdx < contours[contourIdx].size(); pointIdx++) + // { + // Point2d pt = contours[contourIdx][pointIdx]; + // dists.push_back(norm(center.location - pt)); + // } + // std::sort(dists.begin(), dists.end()); + // center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.; + //} + center.radius = (box.size.height+box.size.width)/4.0; + centers.push_back(center); + #ifdef DEBUG_CIRCLE_DETECTOR - // circle( keypointsImage, center.location, 1, Scalar(0,0,255), 1 ); + // circle( keypointsImage, center.location, 1, Scalar(0,0,255), 1 ); #endif - } + } #ifdef DEBUG_CIRCLE_DETECTOR - // imshow("bk", keypointsImage ); - // waitKey(); + // imshow("bk", keypointsImage ); + // waitKey(); #endif } -void CircleDetector::detectImpl(const cv::Mat& image, std::vector& keypoints, const cv::Mat&) const +void CircleDetectorImpl::detect( InputArray _image, std::vector& keypoints, InputArray mask ) { - //TODO: support mask - keypoints.clear(); - Mat grayscaleImage; - if (image.channels() == 3) - cvtColor(image, grayscaleImage, CV_BGR2GRAY); - else - grayscaleImage = image; - - vector < vector
> centers; - for (double thresh = params.minThreshold; thresh < params.maxThreshold; thresh += params.thresholdStep) - { - Mat binarizedImage; - threshold(grayscaleImage, binarizedImage, thresh, 255, THRESH_BINARY); -#ifdef DEBUG_CIRCLE_DETECTOR - // Mat keypointsImage; - // cvtColor( binarizedImage, keypointsImage, CV_GRAY2RGB ); -#endif + Mat image = _image.getMat(); + + // CV_INSTRUMENT_REGION() + + //TODO: support mask + keypoints.clear(); + Mat grayscaleImage; + if (image.channels() == 3) + cvtColor(image, grayscaleImage, CV_BGR2GRAY); + else + grayscaleImage = image; + + vector < vector
> centers; + for (double thresh = params.minThreshold; thresh < params.maxThreshold; thresh += params.thresholdStep) + { + Mat binarizedImage; + threshold(grayscaleImage, binarizedImage, thresh, 255, THRESH_BINARY); - vector < Center > curCenters; - findCircles(grayscaleImage, binarizedImage, curCenters); - vector < vector
> newCenters; - for (size_t i = 0; i < curCenters.size(); i++) - { #ifdef DEBUG_CIRCLE_DETECTOR - // circle(keypointsImage, curCenters[i].location, curCenters[i].radius, Scalar(0,0,255),-1); + // Mat keypointsImage; + // cvtColor( binarizedImage, keypointsImage, CV_GRAY2RGB ); #endif - bool isNew = true; - for (size_t j = 0; j < centers.size(); j++) - { - double dist = norm(centers[j][ centers[j].size() / 2 ].location - curCenters[i].location); - // isNew = dist >= params.minDistBetweenCircles && dist >= centers[j][ centers[j].size() / 2 ].radius && dist >= curCenters[i].radius; - double rad_diff = fabs(centers[j][ centers[j].size() / 2 ].radius - curCenters[i].radius); - isNew = dist >= params.minDistBetweenCircles || rad_diff >= params.minRadiusDiff; - if (!isNew) - { - centers[j].push_back(curCenters[i]); - - size_t k = centers[j].size() - 1; - while( k > 0 && centers[j][k].radius < centers[j][k-1].radius ) - { - centers[j][k] = centers[j][k-1]; - k--; - } - centers[j][k] = curCenters[i]; - - break; - } - } - if (isNew) - { - newCenters.push_back(vector
(1, curCenters[i])); - //centers.push_back(vector
(1, curCenters[i])); - } - } - std::copy(newCenters.begin(), newCenters.end(), std::back_inserter(centers)); - + vector < Center > curCenters; + findCircles(grayscaleImage, binarizedImage, curCenters); + vector < vector
> newCenters; + for (size_t i = 0; i < curCenters.size(); i++) + { #ifdef DEBUG_CIRCLE_DETECTOR - // imshow("binarized", keypointsImage ); - //waitKey(); + // circle(keypointsImage, curCenters[i].location, curCenters[i].radius, Scalar(0,0,255),-1); #endif - } - for (size_t i = 0; i < centers.size(); i++) - { - if (centers[i].size() < params.minRepeatability) - continue; - Point2d sumPoint(0, 0); - double normalizer = 0; - for (size_t j = 0; j < centers[i].size(); j++) + bool isNew = true; + for (size_t j = 0; j < centers.size(); j++) + { + double dist = norm(centers[j][ centers[j].size() / 2 ].location - curCenters[i].location); + // isNew = dist >= params.minDistBetweenCircles && dist >= centers[j][ centers[j].size() / 2 ].radius && dist >= curCenters[i].radius; + double rad_diff = fabs(centers[j][ centers[j].size() / 2 ].radius - curCenters[i].radius); + isNew = dist >= params.minDistBetweenCircles || rad_diff >= params.minRadiusDiff; + if (!isNew) { - sumPoint += centers[i][j].confidence * centers[i][j].location; - normalizer += centers[i][j].confidence; + centers[j].push_back(curCenters[i]); + + size_t k = centers[j].size() - 1; + while( k > 0 && centers[j][k].radius < centers[j][k-1].radius ) + { + centers[j][k] = centers[j][k-1]; + k--; + } + centers[j][k] = curCenters[i]; + + break; } - sumPoint *= (1. / normalizer); - KeyPoint kpt(sumPoint, (float)(centers[i][centers[i].size() / 2].radius*2.0)); - keypoints.push_back(kpt); + } + if (isNew) + { + newCenters.push_back(vector
(1, curCenters[i])); + //centers.push_back(vector
(1, curCenters[i])); + } } + std::copy(newCenters.begin(), newCenters.end(), std::back_inserter(centers)); #ifdef DEBUG_CIRCLE_DETECTOR - namedWindow("keypoints", CV_WINDOW_NORMAL); - Mat outImg = image.clone(); - for(size_t i=0; i -#include - -using namespace boost::python; - -struct World -{ - World(std::string msg): msg(msg) {} - void set(std::string msg) { this->msg = msg; } - std::string greet() { return msg; } - std::string msg; -}; -BOOST_PYTHON_MODULE(libcircle_detector) + Ptr CircleDetector::create(const CircleDetector::Params& params) { - class_("World", init()) - .def("greet", &World::greet) - .def("set", &World::set) - ; - class_("CircleDetector", init()) - .def("read", &CircleDetector::read) - .def("write", &CircleDetector::write) - ; - class_("CircleDetectorParams", init()) - .def_readwrite("thresholdStep",&CircleDetector::Params::thresholdStep) - .def_readwrite("minThreshold",&CircleDetector::Params::minThreshold) - .def_readwrite("maxThreshold",&CircleDetector::Params::maxThreshold) - .def_readwrite("minRepeatability",&CircleDetector::Params::minRepeatability) - .def_readwrite("minDistBetweenCircles",&CircleDetector::Params::minDistBetweenCircles) - .def_readwrite("minRadiusDiff",&CircleDetector::Params::minRadiusDiff) - .def_readwrite("filterByColor",&CircleDetector::Params::filterByColor) - .def_readwrite("circleColor",&CircleDetector::Params::circleColor) - .def_readwrite("filterByArea",&CircleDetector::Params::filterByArea) - .def_readwrite("minArea",&CircleDetector::Params::minArea) - .def_readwrite("maxArea",&CircleDetector::Params::maxArea) - .def_readwrite("filterByCircularity",&CircleDetector::Params::filterByCircularity) - .def_readwrite("minCircularity",&CircleDetector::Params::minCircularity) - .def_readwrite("maxCircularity",&CircleDetector::Params::maxCircularity) - .def_readwrite("filterByInertia",&CircleDetector::Params::filterByInertia) - .def_readwrite("minInertiaRatio",&CircleDetector::Params::minInertiaRatio) - .def_readwrite("maxInertiaRatio",&CircleDetector::Params::maxInertiaRatio) - .def_readwrite("filterByConvexity",&CircleDetector::Params::filterByConvexity) - .def_readwrite("minConvexity",&CircleDetector::Params::minConvexity) - .def_readwrite("maxConvexity",&CircleDetector::Params::maxConvexity) - ; - + return makePtr(params); } -*/ +}// end namespace cv diff --git a/industrial_extrinsic_cal/src/nodes/calibration_service.cpp b/industrial_extrinsic_cal/src/nodes/calibration_service.cpp index a41021d5..cd7557aa 100644 --- a/industrial_extrinsic_cal/src/nodes/calibration_service.cpp +++ b/industrial_extrinsic_cal/src/nodes/calibration_service.cpp @@ -51,7 +51,7 @@ class CalibrationServiceNode priv_nh.getParam("post_proc_on", post_proc_on); priv_nh.getParam("observation_data_file", observation_data_file); if(!priv_nh.getParam("results_file", results_file_)){ - results_file_ = yaml_file_path + "/results.txt"; + results_file_ = yaml_file_path + "results.launch"; } ROS_INFO("yaml_file_path: %s",yaml_file_path.c_str()); ROS_INFO("camera_file: %s",camera_file.c_str()); diff --git a/industrial_extrinsic_cal/src/ros_camera_observer.cpp b/industrial_extrinsic_cal/src/ros_camera_observer.cpp index 9692aaf3..d50c9afa 100644 --- a/industrial_extrinsic_cal/src/ros_camera_observer.cpp +++ b/industrial_extrinsic_cal/src/ros_camera_observer.cpp @@ -41,71 +41,27 @@ namespace industrial_extrinsic_cal pnh.getParam("store_observation_images", store_observation_images_); pnh.getParam("load_observation_images", load_observation_images_); - // set up the circle detector - CircleDetector::Params circle_params; - circle_params.thresholdStep = 10; - circle_params.minThreshold = 50; - circle_params.maxThreshold = 220; - circle_params.minRepeatability = 2; - circle_params.minDistBetweenCircles = 2.0; - circle_params.minRadiusDiff = 10; - - circle_params.filterByColor = false; - circle_params.circleColor = 0; - - circle_params.filterByArea = false; - circle_params.minArea = 25; - circle_params.maxArea = 5000; - - circle_params.filterByCircularity = false; - circle_params.minCircularity = 0.8f; - circle_params.maxCircularity = std::numeric_limits::max(); - - circle_params.filterByInertia = false; - circle_params.minInertiaRatio = 0.1f; - circle_params.maxInertiaRatio = std::numeric_limits::max(); - - circle_params.filterByConvexity = false; - circle_params.minConvexity = 0.95f; - circle_params.maxConvexity = std::numeric_limits::max(); - - // set up and create the detector using the parameters + // set up blob/circle detectors parameters are dynamic cv::SimpleBlobDetector::Params simple_blob_params; if(!pnh.getParam("white_blobs", white_blobs_)){ white_blobs_ = false; } - if(white_blobs_){ - simple_blob_params.minThreshold = 40; - simple_blob_params.maxThreshold = 60; - simple_blob_params.thresholdStep = 5; - simple_blob_params.minArea = 100; - simple_blob_params.minConvexity = 0.3; - simple_blob_params.maxConvexity = 0.3;//circularity ($\frac4*\pi*Area* perimeter$) . - simple_blob_params.minInertiaRatio = 0.01; - simple_blob_params.maxInertiaRatio = 0.01; - simple_blob_params.minArea = 30.0; //float - simple_blob_params.maxArea = 8000.0; - simple_blob_params.maxConvexity = 10; - simple_blob_params.filterByColor = false; - simple_blob_params.blobColor = (uchar) 200; // 255=light 0=dark blobs - simple_blob_params.filterByCircularity = true; - simple_blob_params.minCircularity= 0.8; // float - simple_blob_params.maxCircularity= 1.0; //float - simple_blob_params.minDistBetweenBlobs = 10; // float - simple_blob_params.minRepeatability = (size_t) 128; // don't know what it means - } - if(!pnh.getParam("use_circle_detector", use_circle_detector_)){ use_circle_detector_ = false; } - if(use_circle_detector_){ - circle_detector_ptr_ = new cv::CircleDetector(circle_params); - } - else{ - // FIXME - circle_detector_ptr_ = cv::SimpleBlobDetector::create(simple_blob_params); - } + circle_detector_ptr_ = cv::CircleDetector::create(); + blob_detector_ptr_ = cv::SimpleBlobDetector::create(simple_blob_params); + + + std::string recon_node_name = "~/" + camera_name; + rnh_ = new ros::NodeHandle(recon_node_name.c_str()); + server_ = new dynamic_reconfigure::Server (*rnh_); + + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&ROSCameraObserver::dynReConfCallBack, this, _1, _2); + server_->setCallback(f); } @@ -215,6 +171,7 @@ int ROSCameraObserver::getObservations(CameraObservations &cam_obs) if (sym_circle_) // symetric circle grid { ROS_DEBUG_STREAM("Finding Circles in grid, symmetric..."); + if(use_circle_detector_){ successful_find = cv::findCirclesGrid(image_roi_, pattern_size, observation_pts_, cv::CALIB_CB_SYMMETRIC_GRID, circle_detector_ptr_); @@ -292,7 +249,10 @@ int ROSCameraObserver::getObservations(CameraObservations &cam_obs) // Should OpenCV change their method, the keypoint locations may not match, this has a risk of failing with // updates to OpenCV std::vector keypoints; - circle_detector_ptr_->detect(image_roi_, keypoints); + + if(use_circle_detector_)circle_detector_ptr_->detect(image_roi_, keypoints); + if(!use_circle_detector_)blob_detector_ptr_->detect(image_roi_, keypoints); + ROS_DEBUG("found %d keypoints", (int) keypoints.size()); // if a flipped pattern is found, flip the rows/columns @@ -504,7 +464,8 @@ int ROSCameraObserver::getObservations(CameraObservations &cam_obs) dilate( green_binary_image, green_binary_image, dilation_element); std::vector centers; std::vector keypoints; - circle_detector_ptr_->detect(red_binary_image, keypoints); + if(!use_circle_detector_)blob_detector_ptr_->detect(red_binary_image, keypoints); + if( use_circle_detector_)circle_detector_ptr_->detect(red_binary_image, keypoints); ROS_ERROR("Red keypoints: %d", (int) keypoints.size()); if(keypoints.size() == 1 ){ observation_pts_.push_back(keypoints[0].pt); @@ -514,7 +475,8 @@ int ROSCameraObserver::getObservations(CameraObservations &cam_obs) else{ ROS_ERROR("found %d red blobs, expected one", (int) keypoints.size()); } - circle_detector_ptr_->detect(green_binary_image, keypoints); + if(!use_circle_detector_)blob_detector_ptr_->detect(green_binary_image, keypoints); + if( use_circle_detector_)circle_detector_ptr_->detect(green_binary_image, keypoints); ROS_ERROR("Green keypoints: %d",(int)keypoints.size()); if(keypoints.size() == 1){ observation_pts_.push_back(keypoints[0].pt); @@ -522,7 +484,8 @@ int ROSCameraObserver::getObservations(CameraObservations &cam_obs) else{ ROS_ERROR("found %d green blobs, expected one", (int) keypoints.size()); } - circle_detector_ptr_->detect(yellow_binary_image, keypoints); + if(!use_circle_detector_)blob_detector_ptr_->detect(yellow_binary_image, keypoints); + if( use_circle_detector_)circle_detector_ptr_->detect(yellow_binary_image, keypoints); ROS_ERROR("Blue keypoints: %d", (int)keypoints.size()); if(keypoints.size() == 1){ observation_pts_.push_back(keypoints[0].pt); @@ -816,10 +779,10 @@ bool ROSCameraObserver::pullCameraInfo(double &fx, double &fy, std::string camera_info_topic = "/" + camera_name_ + "/camera_info"; const sensor_msgs::CameraInfoConstPtr& info_msg = ros::topic::waitForMessage(camera_info_topic, ros::Duration(10.0)); if(info_msg == NULL) - { - ROS_ERROR("camera info message not available for camera %s on topic %s", camera_name_.c_str(), camera_info_topic.c_str()); - return(false); - } + { + ROS_ERROR("camera info message not available for camera %s on topic %s", camera_name_.c_str(), camera_info_topic.c_str()); + return(false); + } if(info_msg->K[0] ==0){ ROS_ERROR("camera info message not correct for camera %s on topic %s", camera_name_.c_str(), camera_info_topic.c_str()); return(false); @@ -837,4 +800,71 @@ bool ROSCameraObserver::pullCameraInfo(double &fx, double &fy, height = info_msg->height; return(true); } + +void ROSCameraObserver::dynReConfCallBack(industrial_extrinsic_cal::circle_grid_finderConfig &config, uint32_t level) +{ + CircleDetector::Params circle_params; + circle_params.thresholdStep = 10; + circle_params.minThreshold = config.min_threshold; + circle_params.maxThreshold = config.max_threshold; + circle_params.minRepeatability = 2; + circle_params.minDistBetweenCircles = config.min_distance; + circle_params.minRadiusDiff = 10; + + circle_params.filterByColor = true; + if(white_blobs_)circle_params.circleColor = 200; + if(!white_blobs_)circle_params.circleColor = 0; + + circle_params.filterByArea = config.filter_by_area; + circle_params.minArea = config.min_area; + circle_params.maxArea = config.max_area; + + circle_params.filterByCircularity = config.filter_by_circularity; + circle_params.minCircularity = config.min_circularity; + circle_params.maxCircularity = config.max_circularity; + + circle_params.filterByInertia = false; + circle_params.minInertiaRatio = 0.1f; + circle_params.maxInertiaRatio = std::numeric_limits::max(); + + circle_params.filterByConvexity = false; + circle_params.minConvexity = 0.95f; + circle_params.maxConvexity = std::numeric_limits::max(); + + circle_detector_ptr_ = cv::CircleDetector::create(circle_params); + + cv::SimpleBlobDetector::Params blob_params; + blob_params.thresholdStep = 10; + blob_params.minThreshold = config.min_threshold; + blob_params.maxThreshold = config.max_threshold; + blob_params.minRepeatability = 2; + blob_params.minDistBetweenBlobs = config.min_distance; + + blob_params.filterByColor = true; + if(white_blobs_)blob_params.blobColor = 200; + if(!white_blobs_)blob_params.blobColor = 0; + + blob_params.filterByArea = config.filter_by_area; + blob_params.minArea = config.min_area; + blob_params.maxArea = config.max_area; + + blob_params.filterByCircularity = config.filter_by_circularity; + blob_params.minCircularity = config.min_circularity; + blob_params.maxCircularity = config.max_circularity; + + blob_params.filterByInertia = false; + blob_params.minInertiaRatio = 0.1f; + blob_params.maxInertiaRatio = std::numeric_limits::max(); + + blob_params.filterByConvexity = false; + blob_params.minConvexity = 0.95f; + blob_params.maxConvexity = std::numeric_limits::max(); + + blob_detector_ptr_ = cv::SimpleBlobDetector::create(blob_params); + + + blob_detector_ptr_ = cv::SimpleBlobDetector::create(blob_params); +} + + } //industrial_extrinsic_cal