From 83017e3e218009e9565bc38d76c3bfa6a85fe26b Mon Sep 17 00:00:00 2001 From: gestom Date: Mon, 28 Sep 2015 04:31:43 +0100 Subject: [PATCH 01/13] 3D transforms added. README now contains WhyCon versions overview. Thicker circle descriptions. --- CMakeLists.txt | 3 +- README.md | 2 +- include/whycon/circle_detector.h | 2 +- include/whycon/localization_system.h | 8 +- src/lib/circle_detector.cpp | 145 +++++------ src/lib/localization_system.cpp | 362 +++++++++++++++------------ src/ros/set_axis.cpp | 4 +- src/ros/whycon_ros.cpp | 99 +++++--- src/ros/whycon_ros.h | 2 +- 9 files changed, 344 insertions(+), 283 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f9adc8d..37f97c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 2.8) project(whycon) # set some flags -add_definitions(-std=c++11) +#add_definitions(-std=c++11) +add_definitions(-std=gnu++0x) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_RELEASE} -Wall -O4 -march=native -Wfatal-errors") #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_RELEASE} -Wall -O0 -g -Wfatal-errors") set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-configs") diff --git a/README.md b/README.md index 75b5a66..162da02 100644 --- a/README.md +++ b/README.md @@ -56,7 +56,7 @@ For the latest development version (which should also work and may contain new e ### ROS -The main directory should be placed inside a catkin workspace source-space (e.g.: ~catkin_ws/src). +The main directory should be placed inside a catkin workspace source-space (e.g.: ~/catkin_ws/src). It can then be compiled simply by: catkin_make diff --git a/include/whycon/circle_detector.h b/include/whycon/circle_detector.h index 86ea7c8..c374383 100644 --- a/include/whycon/circle_detector.h +++ b/include/whycon/circle_detector.h @@ -80,7 +80,7 @@ namespace cv { void write(cv::FileStorage& fs) const; void read(const cv::FileNode& node); - void draw(cv::Mat& image, const std::string& text = std::string(), cv::Vec3b color = cv::Vec3b(0,255,0), float thickness = 1) const; + void draw(cv::Mat& image, const std::string& text1 = std::string(), const std::string& text2 = std::string(), cv::Vec3b color = cv::Vec3b(0,255,0), float thickness = 1) const; }; class Context { diff --git a/include/whycon/localization_system.h b/include/whycon/localization_system.h index 6519e07..0668b21 100644 --- a/include/whycon/localization_system.h +++ b/include/whycon/localization_system.h @@ -28,14 +28,18 @@ namespace cv { Pose get_pose(const CircleDetector::Circle& circle) const; const CircleDetector::Circle& get_circle(int id); - Pose get_transformed_pose(int id) const; - Pose get_transformed_pose(const CircleDetector::Circle& circle) const; + Pose get_transformed_pose_2d(int id) const; + Pose get_transformed_pose_3d(int id) const; + Pose get_transformed_pose_2d(const CircleDetector::Circle& circle) const; + Pose get_transformed_pose_3d(const CircleDetector::Circle& circle) const; static void load_matlab_calibration(const std::string& calib_file, cv::Mat& K, cv::Mat& dist_coeff); static void load_opencv_calibration(const std::string& calib_file, cv::Mat& K, cv::Mat& dist_coeff); CircleDetector::Circle origin_circles[4]; // center, X, Y + cv::Matx33f coordinates_transform_3d_rot; + cv::Vec3f coordinates_transform_3d_trn; cv::Matx33f coordinates_transform; ManyCircleDetector detector; diff --git a/src/lib/circle_detector.cpp b/src/lib/circle_detector.cpp index 32dec4e..7847234 100644 --- a/src/lib/circle_detector.cpp +++ b/src/lib/circle_detector.cpp @@ -13,28 +13,28 @@ cv::CircleDetector::CircleDetector(int _width, int _height, Context* _context, f context(_context) { minSize = 10; - maxSize = 100*100; // TODO: test! + maxSize = 100*100; // TODO: test! centerDistanceToleranceRatio = 0.1; centerDistanceToleranceAbs = 5; circularTolerance = 0.3; ratioTolerance = 1.0; - + //initialization - fixed params width = _width; height = _height; len = width*height; siz = len*3; - diameterRatio = _diameter_ratio; + diameterRatio = _diameter_ratio; float areaRatioInner_Outer = diameterRatio*diameterRatio; outerAreaRatio = M_PI*(1.0-areaRatioInner_Outer)/4; innerAreaRatio = M_PI/4.0; areasRatio = (1.0-areaRatioInner_Outer)/areaRatioInner_Outer; - threshold = (3 * 256) / 2; - threshold_counter = 0; + threshold = (3 * 256) / 2; + threshold_counter = 0; - use_local_window = false; - local_window_multiplier = 2.5; + use_local_window = false; + local_window_multiplier = 2.5; } cv::CircleDetector::~CircleDetector() @@ -74,21 +74,21 @@ inline int cv::CircleDetector::threshold_pixel(uchar* ptr) bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector::Circle& circle, int ii, float areaRatio, bool search_in_window) { - //int64_t ticks = cv::getTickCount(); - // get shorter names for elements in Context + //int64_t ticks = cv::getTickCount(); + // get shorter names for elements in Context vector& buffer = context->buffer; - vector& queue = context->queue; + vector& queue = context->queue; - int vx,vy; + int vx,vy; queueOldStart = queueStart; int position = 0; int pos; bool result = false; int type = buffer[ii]; int maxx,maxy,minx,miny; - int pixel_class; + int pixel_class; - WHYCON_DEBUG("examine (type " << type << ") at " << ii / width << "," << ii % width << " (numseg " << context->total_segments << ")"); + WHYCON_DEBUG("examine (type " << type << ") at " << ii / width << "," << ii % width << " (numseg " << context->total_segments << ")"); int segment_id = context->total_segments++; buffer[ii] = segment_id; @@ -106,59 +106,59 @@ bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector: position = queue[queueStart++]; //search neighbours - int position_x = position % width; - int position_y = position / width; - - if ((search_in_window && position_x + 1 < min(local_window_x + local_window_width, width)) || - (!search_in_window && position_x + 1 < width)) - { - pos = position + 1; - pixel_class = buffer[pos]; - if (is_unclassified(pixel_class)) { - uchar* ptr = &image.data[pos*3]; - pixel_class = threshold_pixel(ptr); - if (pixel_class != type) buffer[pos] = pixel_class; - } - if (pixel_class == type) { - queue[queueEnd++] = pos; - maxx = max(maxx,pos%width); - buffer[pos] = segment_id; - } - } - - if ((search_in_window && position_x - 1 >= local_window_x) || - (!search_in_window && position_x - 1 >= 0)) - { - pos = position-1; - pixel_class = buffer[pos]; - if (is_unclassified(pixel_class)) { - uchar* ptr = &image.data[pos*3]; - pixel_class = threshold_pixel(ptr); - if (pixel_class != type) buffer[pos] = pixel_class; - } - if (pixel_class == type) { - queue[queueEnd++] = pos; - minx = min(minx,pos%width); - buffer[pos] = segment_id; - } - } + int position_x = position % width; + int position_y = position / width; - if ((search_in_window && position_y - 1 >= local_window_y) || - (!search_in_window && position_y - 1 >= 0)) - { - pos = position-width; - pixel_class = buffer[pos]; - if (is_unclassified(pixel_class)) { - uchar* ptr = &image.data[pos*3]; - pixel_class = threshold_pixel(ptr); - if (pixel_class != type) buffer[pos] = pixel_class; - } - if (pixel_class == type) { - queue[queueEnd++] = pos; - miny = min(miny,pos/width); - buffer[pos] = segment_id; - } - } + if ((search_in_window && position_x + 1 < min(local_window_x + local_window_width, width)) || + (!search_in_window && position_x + 1 < width)) + { + pos = position + 1; + pixel_class = buffer[pos]; + if (is_unclassified(pixel_class)) { + uchar* ptr = &image.data[pos*3]; + pixel_class = threshold_pixel(ptr); + if (pixel_class != type) buffer[pos] = pixel_class; + } + if (pixel_class == type) { + queue[queueEnd++] = pos; + maxx = max(maxx,pos%width); + buffer[pos] = segment_id; + } + } + + if ((search_in_window && position_x - 1 >= local_window_x) || + (!search_in_window && position_x - 1 >= 0)) + { + pos = position-1; + pixel_class = buffer[pos]; + if (is_unclassified(pixel_class)) { + uchar* ptr = &image.data[pos*3]; + pixel_class = threshold_pixel(ptr); + if (pixel_class != type) buffer[pos] = pixel_class; + } + if (pixel_class == type) { + queue[queueEnd++] = pos; + minx = min(minx,pos%width); + buffer[pos] = segment_id; + } + } + + if ((search_in_window && position_y - 1 >= local_window_y) || + (!search_in_window && position_y - 1 >= 0)) + { + pos = position-width; + pixel_class = buffer[pos]; + if (is_unclassified(pixel_class)) { + uchar* ptr = &image.data[pos*3]; + pixel_class = threshold_pixel(ptr); + if (pixel_class != type) buffer[pos] = pixel_class; + } + if (pixel_class == type) { + queue[queueEnd++] = pos; + miny = min(miny,pos/width); + buffer[pos] = segment_id; + } + } if ((search_in_window && position_y + 1 < min(local_window_y + local_window_height, height)) || (!search_in_window && position_y + 1 < height)) @@ -177,8 +177,8 @@ bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector: } } - //if (queueEnd-queueOldStart > maxSize) return false; - } + //if (queueEnd-queueOldStart > maxSize) return false; + } //once the queue is empty, i.e. segment is complete, we compute its size circle.size = queueEnd-queueOldStart; @@ -200,7 +200,7 @@ bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector: //if its round, we compute yet another properties circle.round = true; - // TODO: mean computation could be delayed until the inner ring also satisfies above condition, right? + // TODO: mean computation could be delayed until the inner ring also satisfies above condition, right? circle.mean = 0; for (int p = queueOldStart;p(0,0); - fc[1] = K.at(1,1); - cc[0] = K.at(0,2); - cc[1] = K.at(1,2); - - kc[0] = 1; - for (int i = 0; i < 5; i++) kc[i + 1] = dist_coeff.at(i); - - coordinates_transform = cv::Matx33f(1, 0, 0, 0, 1, 0, 0, 0, 1); - precompute_undistort_map(); - - cout.precision(30); + _K.copyTo(K); + _dist_coeff.copyTo(dist_coeff); + + fc[0] = K.at(0,0); + fc[1] = K.at(1,1); + cc[0] = K.at(0,2); + cc[1] = K.at(1,2); + + kc[0] = 1; + for (int i = 0; i < 5; i++) kc[i + 1] = dist_coeff.at(i); + + coordinates_transform = cv::Matx33f(1, 0, 0, 0, 1, 0, 0, 0, 1); + precompute_undistort_map(); + + cout.precision(30); } bool cv::LocalizationSystem::localize(const cv::Mat& image, bool reset, int attempts, int max_refine) { return detector.detect(image, reset, attempts, max_refine); } -cv::LocalizationSystem::Pose cv::LocalizationSystem::get_pose(const cv::CircleDetector::Circle& circle) const { - Pose result; - double x,y,x1,x2,y1,y2,sx1,sx2,sy1,sy2,major,minor,v0,v1; - - //transform the center +cv::LocalizationSystem::Pose cv::LocalizationSystem::get_pose(const cv::CircleDetector::Circle& circle) const +{ + Pose result; + double x,y,x1,x2,y1,y2,sx1,sx2,sy1,sy2,major,minor,v0,v1; + + //transform the center transform(circle.x,circle.y, x, y); - - //calculate the major axis + + //calculate the major axis //endpoints in image coords sx1 = circle.x + circle.v0 * circle.m0 * 2; sx2 = circle.x - circle.v0 * circle.m0 * 2; sy1 = circle.y + circle.v1 * circle.m0 * 2; sy2 = circle.y - circle.v1 * circle.m0 * 2; - //endpoints in camera coords + //endpoints in camera coords transform(sx1, sy1, x1, y1); transform(sx2, sy2, x2, y2); - //semiaxis length + //semiaxis length major = sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))/2.0; - + v0 = (x2-x1)/major/2.0; v1 = (y2-y1)/major/2.0; @@ -68,7 +69,7 @@ cv::LocalizationSystem::Pose cv::LocalizationSystem::get_pose(const cv::CircleDe sx2 = circle.x - circle.v1 * circle.m1 * 2; sy1 = circle.y - circle.v0 * circle.m1 * 2; sy2 = circle.y + circle.v0 * circle.m1 * 2; - + //endpoints in camera coords transform(sx1, sy1, x1, y1); transform(sx2, sy2, x2, y2); @@ -85,8 +86,8 @@ cv::LocalizationSystem::Pose cv::LocalizationSystem::get_pose(const cv::CircleDe e = (-y*c-b*x); f = (a*x*x+c*y*y+2*b*x*y-1); cv::Matx33d data(a,b,d, - b,c,e, - d,e,f); + b,c,e, + d,e,f); // compute conic eigenvalues and eigenvectors cv::Vec3d eigenvalues; @@ -117,7 +118,7 @@ cv::LocalizationSystem::Pose cv::LocalizationSystem::get_pose(const cv::CircleDe result.rot(2) = 0; /* not recoverable */ /* TODO: to be checked */ - return result; + return result; } const cv::CircleDetector::Circle& cv::LocalizationSystem::get_circle(int id) @@ -130,11 +131,16 @@ cv::LocalizationSystem::Pose cv::LocalizationSystem::get_pose(int id) const return get_pose(detector.circles[id]); } -cv::LocalizationSystem::Pose cv::LocalizationSystem::get_transformed_pose(int id) const { - return get_transformed_pose(detector.circles[id]); + +cv::LocalizationSystem::Pose cv::LocalizationSystem::get_transformed_pose_2d(int id) const { + return get_transformed_pose_2d(detector.circles[id]); } -cv::LocalizationSystem::Pose cv::LocalizationSystem::get_transformed_pose(const cv::CircleDetector::Circle& circle) const +cv::LocalizationSystem::Pose cv::LocalizationSystem::get_transformed_pose_3d(int id) const { + return get_transformed_pose_3d(detector.circles[id]); +} + +cv::LocalizationSystem::Pose cv::LocalizationSystem::get_transformed_pose_2d(const cv::CircleDetector::Circle& circle) const { Pose pose; pose.pos = coordinates_transform * get_pose(circle).pos; @@ -144,93 +150,115 @@ cv::LocalizationSystem::Pose cv::LocalizationSystem::get_transformed_pose(const return pose; } +cv::LocalizationSystem::Pose cv::LocalizationSystem::get_transformed_pose_3d(const cv::CircleDetector::Circle& circle) const +{ + Pose pose; + pose.pos = coordinates_transform_3d_rot * (get_pose(circle).pos-coordinates_transform_3d_trn); + return pose; +} + // TODO: allow user to choose calibration circles, now the circles are read in the order of detection bool cv::LocalizationSystem::set_axis(const cv::Mat& image, int max_attempts, int refine_steps, const std::string& file) { - ManyCircleDetector axis_detector(4, width, height); - if (!axis_detector.detect(image, true, max_attempts, refine_steps)) return false; - - Pose circle_poses[4]; - for (int i = 0; i < 4; i++) { - origin_circles[i] = axis_detector.circles[i]; - circle_poses[i] = get_pose(axis_detector.circles[i]); - } - - cv::Vec3f vecs[3]; - for (int i = 0; i < 3; i++) { - vecs[i] = circle_poses[i + 1].pos - circle_poses[0].pos; - cout << "vec " << i+1 << "->0 " << vecs[i] << endl; - } - int min_prod_i = 0; - float min_prod = numeric_limits::max(); - for (int i = 0; i < 3; i++) { - float prod = fabsf(vecs[(i + 2) % 3].dot(vecs[i])); - cout << "prod: " << ((i + 2) % 3 + 1) << " " << i + 1 << " " << vecs[(i + 2) % 3] << " " << vecs[i] << " " << prod << endl; - if (prod < min_prod) { min_prod = prod; min_prod_i = i; } - } - int axis1_i = (((min_prod_i + 2) % 3) + 1); - int axis2_i = (min_prod_i + 1); - if (fabsf(circle_poses[axis1_i].pos(0)) < fabsf(circle_poses[axis2_i].pos(0))) std::swap(axis1_i, axis2_i); - int xy_i = 0; - for (int i = 1; i <= 3; i++) if (i != axis1_i && i != axis2_i) { xy_i = i; break; } - cout << "axis ids: " << axis1_i << " " << axis2_i << " " << xy_i << endl; - - CircleDetector::Circle origin_circles_reordered[4]; - origin_circles_reordered[0] = origin_circles[0]; - origin_circles_reordered[1] = origin_circles[axis1_i]; - origin_circles_reordered[2] = origin_circles[axis2_i]; - origin_circles_reordered[3] = origin_circles[xy_i]; - for (int i = 0; i < 4; i++) { - origin_circles[i] = origin_circles_reordered[i]; - circle_poses[i] = get_pose(origin_circles[i]); - cout << "original poses: " << circle_poses[i].pos << endl; - } - - float dim_x = 1.0; - float dim_y = 1.0; - cv::Vec2f targets[4] = { cv::Vec2f(0,0), cv::Vec2f(dim_x, 0), cv::Vec2f(0, dim_y), cv::Vec2f(dim_x, dim_y) }; - - // build matrix of coefficients and independent term for linear eq. system - cv::Mat A(8, 8, CV_64FC1), b(8, 1, CV_64FC1), x(8, 1, CV_64FC1); - - cv::Vec2f tmp[4]; - for (int i = 0; i < 4; i++) tmp[i] = cv::Vec2f(circle_poses[i].pos(0), circle_poses[i].pos(1)) / circle_poses[i].pos(2); - for (int i = 0; i < 4; i++) { - cv::Mat r_even = (cv::Mat_(1, 8) << -tmp[i](0), -tmp[i](1), -1, 0, 0, 0, targets[i](0) * tmp[i](0), targets[i](0) * tmp[i](1)); - cv::Mat r_odd = (cv::Mat_(1, 8) << 0, 0, 0, -tmp[i](0), -tmp[i](1), -1, targets[i](1) * tmp[i](0), targets[i](1) * tmp[i](1)); - r_even.copyTo(A.row(2 * i)); - r_odd.copyTo(A.row(2 * i + 1)); - b.at(2 * i) = -targets[i](0); - b.at(2 * i + 1) = -targets[i](1); - } - - // solve linear system and obtain transformation - cv::solve(A, b, x); - x.push_back(1.0); - coordinates_transform = x.reshape(1, 3); - cout << "H " << coordinates_transform << endl; - - // TODO: compare H obtained by OpenCV with the hand approach - std::vector src(4), dsts(4); - for (int i = 0; i < 4; i++) { - src[i] = tmp[i]; - dsts[i] = targets[i]; - cout << tmp[i] << " -> " << targets[i] << endl; - } - - /*cv::Matx33f H = cv::findHomography(src, dsts, CV_LMEDS); - cout << "OpenCV H " << H << endl;*/ - - if (!file.empty()) { - cv::FileStorage fs(file, cv::FileStorage::WRITE); - fs << "H" << cv::Mat(cv::Matx33d(coordinates_transform)); // store as double to get more decimals - fs << "c0"; origin_circles[0].write(fs); - fs << "c1"; origin_circles[1].write(fs); - fs << "c2"; origin_circles[2].write(fs); - fs << "c3"; origin_circles[3].write(fs); - } - axis_set = true; - return true; + ManyCircleDetector axis_detector(4, width, height); + if (!axis_detector.detect(image, true, max_attempts, refine_steps)) return false; + + Pose circle_poses[4]; + for (int i = 0; i < 4; i++) { + origin_circles[i] = axis_detector.circles[i]; + circle_poses[i] = get_pose(axis_detector.circles[i]); + } + + cv::Vec3f vecs[3]; + for (int i = 0; i < 3; i++) { + vecs[i] = circle_poses[i + 1].pos - circle_poses[0].pos; + cout << "vec " << i+1 << "->0 " << vecs[i] << endl; + } + int min_prod_i = 0; + float min_prod = numeric_limits::max(); + for (int i = 0; i < 3; i++) { + float prod = fabsf(vecs[(i + 2) % 3].dot(vecs[i])); + cout << "prod: " << ((i + 2) % 3 + 1) << " " << i + 1 << " " << vecs[(i + 2) % 3] << " " << vecs[i] << " " << prod << endl; + if (prod < min_prod) { min_prod = prod; min_prod_i = i; } + } + int axis1_i = (((min_prod_i + 2) % 3) + 1); + int axis2_i = (min_prod_i + 1); + if (fabsf(circle_poses[axis1_i].pos(0)) < fabsf(circle_poses[axis2_i].pos(0))) std::swap(axis1_i, axis2_i); + int xy_i = 0; + for (int i = 1; i <= 3; i++) if (i != axis1_i && i != axis2_i) { xy_i = i; break; } + cout << "axis ids: " << axis1_i << " " << axis2_i << " " << xy_i << endl; + + CircleDetector::Circle origin_circles_reordered[4]; + origin_circles_reordered[0] = origin_circles[0]; + origin_circles_reordered[1] = origin_circles[axis1_i]; + origin_circles_reordered[2] = origin_circles[axis2_i]; + origin_circles_reordered[3] = origin_circles[xy_i]; + for (int i = 0; i < 4; i++) { + origin_circles[i] = origin_circles_reordered[i]; + circle_poses[i] = get_pose(origin_circles[i]); + cout << "original poses: " << circle_poses[i].pos << endl; + } + + float dim_x = 1.0; + float dim_y = 1.0; + cv::Vec2f targets[4] = { cv::Vec2f(0,0), cv::Vec2f(dim_x, 0), cv::Vec2f(0, dim_y), cv::Vec2f(dim_x, dim_y) }; + + //building the homography matrix for 3D->2D transformation + // build matrix of coefficients and independent term for linear eq. system + cv::Mat A(8, 8, CV_64FC1), b(8, 1, CV_64FC1), x(8, 1, CV_64FC1); + + cv::Vec2f tmp[4]; + for (int i = 0; i < 4; i++) tmp[i] = cv::Vec2f(circle_poses[i].pos(0), circle_poses[i].pos(1)) / circle_poses[i].pos(2); + for (int i = 0; i < 4; i++) { + cv::Mat r_even = (cv::Mat_(1, 8) << -tmp[i](0), -tmp[i](1), -1, 0, 0, 0, targets[i](0) * tmp[i](0), targets[i](0) * tmp[i](1)); + cv::Mat r_odd = (cv::Mat_(1, 8) << 0, 0, 0, -tmp[i](0), -tmp[i](1), -1, targets[i](1) * tmp[i](0), targets[i](1) * tmp[i](1)); + r_even.copyTo(A.row(2 * i)); + r_odd.copyTo(A.row(2 * i + 1)); + b.at(2 * i) = -targets[i](0); + b.at(2 * i + 1) = -targets[i](1); + } + + // solve linear system and obtain transformation + cv::solve(A, b, x); + x.push_back(1.0); + coordinates_transform = x.reshape(1, 3); + cout << "H " << coordinates_transform << endl; + + // TODO: compare H obtained by OpenCV with the hand approach + std::vector src(4), dsts(4); + for (int i = 0; i < 4; i++) { + src[i] = tmp[i]; + dsts[i] = targets[i]; + cout << tmp[i] << " -> " << targets[i] << endl; + } + + /*cv::Matx33f H = cv::findHomography(src, dsts, CV_LMEDS); + cout << "OpenCV H " << H << endl;*/ + + //recovering translation vector and rotation matrix for 3D->3D transformation + + //get axis X,Y from the three circles and calculate Z by their cross-product + Vec3f v[3]; + v[0] = circle_poses[1].pos-circle_poses[0].pos; + v[1] = circle_poses[2].pos-circle_poses[0].pos; + v[2] = v[0].cross(v[1]); + for (int i = 0;i<3;i++) v[i] = v[i]/norm(v[i]); + coordinates_transform_3d_trn = circle_poses[0].pos; + for (int i = 0;i<9;i++) coordinates_transform_3d_rot(i%3,i/3) = v[i/3][i%3]; + coordinates_transform_3d_rot = coordinates_transform_3d_rot.inv(); + + if (!file.empty()) { + cv::FileStorage fs(file, cv::FileStorage::WRITE); + fs << "H" << cv::Mat(cv::Matx33d(coordinates_transform)); // store as double to get more decimals + fs << "c0"; origin_circles[0].write(fs); + fs << "c1"; origin_circles[1].write(fs); + fs << "c2"; origin_circles[2].write(fs); + fs << "c3"; origin_circles[3].write(fs); + fs << "coordinates_transform_3d_trn" << cv::Mat(coordinates_transform_3d_trn); + fs << "coordinates_transform_3d_rot" << cv::Mat(cv::Matx33d(coordinates_transform_3d_rot)); // store as double to get more decimals + } + axis_set = true; + return true; } void cv::LocalizationSystem::read_axis(const std::string& file, float _xscale, float _yscale) { @@ -243,12 +271,19 @@ void cv::LocalizationSystem::read_axis(const std::string& file, float _xscale, f origin_circles[1].read(fs["c1"]); origin_circles[2].read(fs["c2"]); origin_circles[3].read(fs["c3"]); + fs["coordinates_transform_3d_trn"] >> m; + coordinates_transform_3d_trn = cv::Vec3f(m.col(0)); + + fs["coordinates_transform_3d_rot"] >> m; + coordinates_transform_3d_rot = cv::Matx33f(m); xscale = _xscale; yscale = _yscale; axis_set = true; - WHYCON_DEBUG("transformation: " << coordinates_transform); + WHYCON_DEBUG("transformation 2D: " << coordinates_transform); + WHYCON_DEBUG("transformation 3D - trans: " << coordinates_transform_3d_trn); + WHYCON_DEBUG("transformation 3D - rotat: " << coordinates_transform_3d_rot); } void cv::LocalizationSystem::draw_axis(cv::Mat& image) @@ -257,64 +292,65 @@ void cv::LocalizationSystem::draw_axis(cv::Mat& image) for (int i = 0; i < 4; i++) { std::ostringstream ostr; //ostr << std::fixed << std::setprecision(5) << names[i] << endl << get_pose(origin_circles[i]).pos; - origin_circles[i].draw(image, /*ostr.str()*/names[i], cv::Vec3b((i == 0 || i == 3 ? 255 : 0), (i == 1 ? 255 : 0), (i == 2 || i == 3 ? 255 : 0))); + origin_circles[i].draw(image, /*ostr.str()*/names[i],"", cv::Vec3b((i == 0 || i == 3 ? 255 : 0), (i == 1 ? 255 : 0), (i == 2 || i == 3 ? 255 : 0))); } } /* normalize coordinates: move from image to canonical and remove distortion */ void cv::LocalizationSystem::transform(double x_in, double y_in, double& x_out, double& y_out) const { - #if defined(ENABLE_FULL_UNDISTORT) - x_out = (x_in-cc[0])/fc[0]; - y_out = (y_in-cc[1])/fc[1]; - #else - std::vector src(1, cv::Vec2d(x_in, y_in)); - std::vector dst(1); - cv::undistortPoints(src, dst, K, dist_coeff); - x_out = dst[0](0); y_out = dst[0](1); - #endif +#if defined(ENABLE_FULL_UNDISTORT) + x_out = (x_in-cc[0])/fc[0]; + y_out = (y_in-cc[1])/fc[1]; +#else + std::vector src(1, cv::Vec2d(x_in, y_in)); + std::vector dst(1); + cv::undistortPoints(src, dst, K, dist_coeff); + x_out = dst[0](0); y_out = dst[0](1); +#endif } void cv::LocalizationSystem::load_matlab_calibration(const std::string& calib_file, cv::Mat& K, cv::Mat& dist_coeff) { - std::ifstream file(calib_file.c_str()); - if (!file) throw std::runtime_error("calibration file not found"); - std::string line; - - K = cv::Mat::eye(3, 3, CV_64FC1); - dist_coeff.create(5, 1, CV_64FC1); - dist_coeff = cv::Scalar(0); - - while (std::getline(file, line)) { - std::string s; - std::istringstream istr(line); - istr >> s; - if (s == "fc") { - istr >> s >> s; - istr >> K.at(0,0) >> s; - istr >> K.at(1,1); - } - else if (s == "cc") { - istr >> s >> s >> K.at(0,2); - istr >> s >> K.at(1,2); - } - else if (s == "kc") { - istr >> s >> s; - int i = 0; - do { - istr >> dist_coeff.at(i) >> s; - i++; - } while (s != "];"); - } - } + std::ifstream file(calib_file.c_str()); + if (!file) throw std::runtime_error("calibration file not found"); + std::string line; + + K = cv::Mat::eye(3, 3, CV_64FC1); + dist_coeff.create(5, 1, CV_64FC1); + dist_coeff = cv::Scalar(0); + + while (std::getline(file, line)) { + std::string s; + std::istringstream istr(line); + istr >> s; + if (s == "fc") { + istr >> s >> s; + istr >> K.at(0,0) >> s; + istr >> K.at(1,1); + } + else if (s == "cc") { + istr >> s >> s >> K.at(0,2); + istr >> s >> K.at(1,2); + } + else if (s == "kc") { + istr >> s >> s; + int i = 0; + do { + istr >> dist_coeff.at(i) >> s; + i++; + } while (s != "];"); + } + } } -void cv::LocalizationSystem::load_opencv_calibration(const std::string& calib_file, cv::Mat& K, cv::Mat& dist_coeff) { - cv::FileStorage file(calib_file, cv::FileStorage::READ); - if (!file.isOpened()) throw std::runtime_error("calibration file not found"); - - file["K"] >> K; - file["dist"] >> dist_coeff; +void cv::LocalizationSystem::load_opencv_calibration(const std::string& calib_file, cv::Mat& K, cv::Mat& dist_coeff) +{ + cv::FileStorage file(calib_file, cv::FileStorage::READ); + if (!file.isOpened()) throw std::runtime_error("calibration file not found"); + + file["K"] >> K; + file["dist"] >> dist_coeff; } void cv::LocalizationSystem::precompute_undistort_map(void) diff --git a/src/ros/set_axis.cpp b/src/ros/set_axis.cpp index 0f5b35e..d987a70 100644 --- a/src/ros/set_axis.cpp +++ b/src/ros/set_axis.cpp @@ -48,7 +48,7 @@ void whycon::AxisSetter::on_image(const sensor_msgs::ImageConstPtr& img_msg, con if (!circle.valid) continue; //cv::LocalizationSystem::Pose pose = system->get_pose(circle); - cv::LocalizationSystem::Pose trans_pose = system->get_transformed_pose(circle); + cv::LocalizationSystem::Pose trans_pose = system->get_transformed_pose_3d(circle); //cv::Vec3f coord = pose.pos; cv::Vec3f coord_trans = trans_pose.pos; @@ -56,7 +56,7 @@ void whycon::AxisSetter::on_image(const sensor_msgs::ImageConstPtr& img_msg, con std::ostringstream ostr; ostr << std::fixed << std::setprecision(2); ostr << coord_trans << " " << i; - circle.draw(image, ostr.str(), cv::Vec3b(0,255,255)); + circle.draw(image, ostr.str(), "", cv::Vec3b(0,255,255)); } } img_pub.publish(cv_ptr); diff --git a/src/ros/whycon_ros.cpp b/src/ros/whycon_ros.cpp index bdb223b..dffeb83 100644 --- a/src/ros/whycon_ros.cpp +++ b/src/ros/whycon_ros.cpp @@ -24,11 +24,12 @@ whycon::WhyConROS::WhyConROS(ros::NodeHandle& n) : is_tracking(false), should_re n.param("input_queue_size", input_queue_size, input_queue_size); cam_sub = it.subscribeCamera("/camera/image_rect_color", input_queue_size, boost::bind(&WhyConROS::on_image, this, _1, _2)); - image_pub = n.advertise("image_out", 1); + image_pub = n.advertise("image_output", 1); viz_pub = n.advertise("visualization_marker", 1); points_pub = n.advertise("points", 1); poses_pub = n.advertise("poses", 1); - trans_poses_pub = n.advertise("trans_poses", 1); + trans_poses_2d_pub = n.advertise("trans_poses_2d", 1); + trans_poses_3d_pub = n.advertise("trans_poses_3d", 1); context_pub = n.advertise("context", 1); reset_service = n.advertiseService("reset", &WhyConROS::reset, this); @@ -38,7 +39,6 @@ void whycon::WhyConROS::on_image(const sensor_msgs::ImageConstPtr& image_msg, co { camera_model.fromCameraInfo(info_msg); if (camera_model.fullResolution().width == 0) { ROS_ERROR_STREAM("camera is not calibrated!"); return; } - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "rgb8"); const cv::Mat& image = cv_ptr->image; @@ -80,9 +80,10 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv bool publish_viz = (viz_pub.getNumSubscribers() != 0); bool publish_points = (points_pub.getNumSubscribers() != 0); bool publish_poses = (poses_pub.getNumSubscribers() != 0); - bool publish_trans_poses = (trans_poses_pub.getNumSubscribers() != 0); + bool publish_trans_2d_poses = (trans_poses_2d_pub.getNumSubscribers() != 0); + bool publish_trans_3d_poses = (trans_poses_3d_pub.getNumSubscribers() != 0); - if (!publish_images && !publish_viz && !publish_points && !publish_poses && !publish_trans_poses) return; + if (!publish_images && !publish_viz && !publish_points && !publish_poses && !publish_trans_2d_poses && !publish_trans_3d_poses) return; // prepare particle markers visualization_msgs::Marker marker, transformed_marker; @@ -117,55 +118,67 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv if (publish_images) output_image = cv_ptr->image.clone(); - geometry_msgs::PoseArray pose_array, trans_pose_array; + geometry_msgs::PoseArray pose_array, trans_pose_array_2d, trans_pose_array_3d; whycon::PointArray point_array; // go through detected targets for (int i = 0; i < system->targets; i++) { const cv::CircleDetector::Circle& circle = system->get_circle(i); cv::LocalizationSystem::Pose pose = system->get_pose(circle); - cv::LocalizationSystem::Pose trans_pose = system->get_transformed_pose(circle); + cv::LocalizationSystem::Pose trans_pose_2d = system->get_transformed_pose_2d(circle); + cv::LocalizationSystem::Pose trans_pose_3d = system->get_transformed_pose_3d(circle); cv::Vec3f coord = pose.pos; - cv::Vec3f coord_trans = trans_pose.pos; + cv::Vec3f coord_trans_2d = trans_pose_2d.pos; + cv::Vec3f coord_trans_3d = trans_pose_3d.pos; // draw each target if (publish_images) { - std::ostringstream ostr; - ostr << std::fixed << std::setprecision(2); - ostr << coord_trans << " " << i; - circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,255)); + std::ostringstream ostr1,ostr2; + ostr1 << std::fixed << std::setprecision(2); + ostr2 << std::fixed << std::setprecision(2); + ostr1 << i << " 2D: [" << coord_trans_2d[0] << " " << coord_trans_2d[1] << "]"; + ostr2 << "3D: " << coord_trans_3d; + circle.draw(output_image, ostr1.str(), ostr2.str(), cv::Vec3b(255,0,255),3); } if (publish_viz) { - geometry_msgs::Point marker_point; - marker_point.x = coord(0); - marker_point.y = coord(1); - marker_point.z = coord(2); - marker.points.push_back(marker_point); - if (system->axis_set) { - marker_point.x = coord_trans(0); - marker_point.y = coord_trans(1); - marker_point.z = coord_trans(2); - transformed_marker.points.push_back(marker_point); - } + geometry_msgs::Point marker_point; + marker_point.x = coord(0); + marker_point.y = coord(1); + marker_point.z = coord(2); + marker.points.push_back(marker_point); + if (system->axis_set) { + marker_point.x = coord_trans_3d(0); + marker_point.y = coord_trans_3d(1); + marker_point.z = coord_trans_3d(2); + transformed_marker.points.push_back(marker_point); + } } - if (publish_trans_poses) { - geometry_msgs::Pose p; - p.position.x = trans_pose.pos(0); - p.position.y = trans_pose.pos(1); - p.position.z = trans_pose.pos(2); - p.orientation = tf::createQuaternionMsgFromRollPitchYaw(trans_pose.rot(0), trans_pose.rot(1), trans_pose.rot(2)); - trans_pose_array.poses.push_back(p); + if (publish_trans_2d_poses) { + geometry_msgs::Pose p; + p.position.x = trans_pose_2d.pos(0); + p.position.y = trans_pose_2d.pos(1); + p.position.z = trans_pose_2d.pos(2); + p.orientation = tf::createQuaternionMsgFromRollPitchYaw(trans_pose_2d.rot(0), trans_pose_2d.rot(1), trans_pose_2d.rot(2)); + trans_pose_array_2d.poses.push_back(p); + } + if (publish_trans_3d_poses) { + geometry_msgs::Pose p; + p.position.x = trans_pose_3d.pos(0); + p.position.y = trans_pose_3d.pos(1); + p.position.z = trans_pose_3d.pos(2); + p.orientation = tf::createQuaternionMsgFromRollPitchYaw(trans_pose_3d.rot(0), trans_pose_3d.rot(1), trans_pose_3d.rot(2)); + trans_pose_array_3d.poses.push_back(p); } if (publish_poses) { - geometry_msgs::Pose p; - p.position.x = pose.pos(0); - p.position.y = pose.pos(1); - p.position.z = pose.pos(2); - p.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, pose.rot(0), pose.rot(1)); - pose_array.poses.push_back(p); + geometry_msgs::Pose p; + p.position.x = pose.pos(0); + p.position.y = pose.pos(1); + p.position.z = pose.pos(2); + p.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, pose.rot(0), pose.rot(1)); + pose_array.poses.push_back(p); } if (publish_points) { @@ -194,10 +207,16 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv poses_pub.publish(pose_array); } - if (publish_trans_poses) { - trans_pose_array.header = header; - trans_pose_array.header.frame_id = "/base_link"; - trans_poses_pub.publish(trans_pose_array); + if (publish_trans_2d_poses) { + trans_pose_array_2d.header = header; + trans_pose_array_2d.header.frame_id = "/base_link"; + trans_poses_2d_pub.publish(trans_pose_array_2d); + } + + if (publish_trans_3d_poses) { + trans_pose_array_3d.header = header; + trans_pose_array_3d.header.frame_id = "/base_link"; + trans_poses_3d_pub.publish(trans_pose_array_3d); } if (publish_points) { diff --git a/src/ros/whycon_ros.h b/src/ros/whycon_ros.h index e4b4e9a..3b6c955 100644 --- a/src/ros/whycon_ros.h +++ b/src/ros/whycon_ros.h @@ -31,7 +31,7 @@ namespace whycon { image_transport::CameraSubscriber cam_sub; ros::ServiceServer reset_service; - ros::Publisher image_pub, viz_pub, poses_pub, trans_poses_pub, points_pub, context_pub; + ros::Publisher image_pub, viz_pub, poses_pub, trans_poses_2d_pub, trans_poses_3d_pub, points_pub, context_pub; image_geometry::PinholeCameraModel camera_model; }; From 99850932080c9ce0ef554d2fadb4a183e4177d5b Mon Sep 17 00:00:00 2001 From: gestom Date: Mon, 28 Sep 2015 04:41:14 +0100 Subject: [PATCH 02/13] Readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 162da02..8e20b26 100644 --- a/README.md +++ b/README.md @@ -85,7 +85,7 @@ Note the default CMake location is `/usr/local`, but you can redefine this by in ## Using WhyCon -Please refer to the [wiki](https://github.com/lrse/whycon/wiki). +For a step-by-step tutorial, please refer to the [wiki](wiki/Tutorial). ---- From f9d8bf0c2c8165f1f77b8aa8c91744bb658cfee2 Mon Sep 17 00:00:00 2001 From: gestom Date: Mon, 28 Sep 2015 04:43:31 +0100 Subject: [PATCH 03/13] Readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 8e20b26..44af9fa 100644 --- a/README.md +++ b/README.md @@ -85,7 +85,7 @@ Note the default CMake location is `/usr/local`, but you can redefine this by in ## Using WhyCon -For a step-by-step tutorial, please refer to the [wiki](wiki/Tutorial). +For a step-by-step tutorial, please refer to the [wiki](../../wiki/Tutorial). ---- From 389b9ac8cb26c0e25d36f95596c20d3b36f9cd2a Mon Sep 17 00:00:00 2001 From: gestom Date: Mon, 28 Sep 2015 04:46:18 +0100 Subject: [PATCH 04/13] Readme --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 44af9fa..a054002 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ The main contributors were [Matias Nitsche](https://scholar.google.co.uk/citatio | WhyCon version | Application | Main features | Maintainer| | --------------- | ----------- | ------ | ----- | | [WhyCon-ROS](https://github.com/lrse/whycon) | general | 2D, 3D, ROS + lightweight | [Matias Nitsche](https://scholar.google.co.uk/citations?user=Z0hQoRUAAAAJ&hl=en&oi=ao) | -| [WhyCon-Mini](http://labe.felk.cvut.cz/~tkrajnik/circle_detector/) | general | 2D, 3D, lightweight, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao)| +| [WhyCon-orig](http://labe.felk.cvut.cz/~tkrajnik/circle_detector/) | general | 2D, 3D, lightweight, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao)| | [SwarmCon](https://github.com/gestom/CosPhi/tree/master/Localization) | μ-swarms | 2D, individual IDs, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao) | | [Caspa-WhyCon](http://robotics.fel.cvut.cz/faigl/caspa/) | UAVs | embedded, open HW-SW solution | [Jan Faigl](https://scholar.google.co.uk/citations?user=-finD_sAAAAJ&hl=en) | | [Social-card](https://github.com/strands-project/strands_social/tree/hydro-devel/social_card_reader) | HRI | orientation translated to commands | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao) | @@ -85,7 +85,8 @@ Note the default CMake location is `/usr/local`, but you can redefine this by in ## Using WhyCon -For a step-by-step tutorial, please refer to the [wiki](../../wiki/Tutorial). +For a step-by-step tutorial, please refer to the [wiki tutorial](../../wiki/Tutorial). +For additional information, please check the [wiki home](../../wiki). ---- From 64ba77664ec0455a412d8dd21e60e8dabedea25a Mon Sep 17 00:00:00 2001 From: gestom Date: Mon, 28 Sep 2015 05:06:28 +0100 Subject: [PATCH 05/13] Readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a054002..b5930c1 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ The main contributors were [Matias Nitsche](https://scholar.google.co.uk/citatio | WhyCon version | Application | Main features | Maintainer| | --------------- | ----------- | ------ | ----- | -| [WhyCon-ROS](https://github.com/lrse/whycon) | general | 2D, 3D, ROS + lightweight | [Matias Nitsche](https://scholar.google.co.uk/citations?user=Z0hQoRUAAAAJ&hl=en&oi=ao) | +| [WhyCon-ROS](.) | general | 2D, 3D, ROS + lightweight | [Matias Nitsche](https://scholar.google.co.uk/citations?user=Z0hQoRUAAAAJ&hl=en&oi=ao) | | [WhyCon-orig](http://labe.felk.cvut.cz/~tkrajnik/circle_detector/) | general | 2D, 3D, lightweight, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao)| | [SwarmCon](https://github.com/gestom/CosPhi/tree/master/Localization) | μ-swarms | 2D, individual IDs, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao) | | [Caspa-WhyCon](http://robotics.fel.cvut.cz/faigl/caspa/) | UAVs | embedded, open HW-SW solution | [Jan Faigl](https://scholar.google.co.uk/citations?user=-finD_sAAAAJ&hl=en) | From 8c1c0ff615dd20d3499871f406dbd39ed8aba01c Mon Sep 17 00:00:00 2001 From: gestom Date: Mon, 28 Sep 2015 05:07:04 +0100 Subject: [PATCH 06/13] Readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b5930c1..7c08269 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ The main contributors were [Matias Nitsche](https://scholar.google.co.uk/citatio | WhyCon version | Application | Main features | Maintainer| | --------------- | ----------- | ------ | ----- | -| [WhyCon-ROS](.) | general | 2D, 3D, ROS + lightweight | [Matias Nitsche](https://scholar.google.co.uk/citations?user=Z0hQoRUAAAAJ&hl=en&oi=ao) | +| [WhyCon-ROS](../../) | general | 2D, 3D, ROS + lightweight | [Matias Nitsche](https://scholar.google.co.uk/citations?user=Z0hQoRUAAAAJ&hl=en&oi=ao) | | [WhyCon-orig](http://labe.felk.cvut.cz/~tkrajnik/circle_detector/) | general | 2D, 3D, lightweight, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao)| | [SwarmCon](https://github.com/gestom/CosPhi/tree/master/Localization) | μ-swarms | 2D, individual IDs, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao) | | [Caspa-WhyCon](http://robotics.fel.cvut.cz/faigl/caspa/) | UAVs | embedded, open HW-SW solution | [Jan Faigl](https://scholar.google.co.uk/citations?user=-finD_sAAAAJ&hl=en) | From a3de29074726242795f6668a7b6520aa0084fee3 Mon Sep 17 00:00:00 2001 From: gestom Date: Mon, 28 Sep 2015 05:08:00 +0100 Subject: [PATCH 07/13] Readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 7c08269..95c1abd 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ The main contributors were [Matias Nitsche](https://scholar.google.co.uk/citatio | WhyCon version | Application | Main features | Maintainer| | --------------- | ----------- | ------ | ----- | | [WhyCon-ROS](../../) | general | 2D, 3D, ROS + lightweight | [Matias Nitsche](https://scholar.google.co.uk/citations?user=Z0hQoRUAAAAJ&hl=en&oi=ao) | -| [WhyCon-orig](http://labe.felk.cvut.cz/~tkrajnik/circle_detector/) | general | 2D, 3D, lightweight, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao)| +| [WhyCon-orig](https://github.com/gestom/whycon-orig/) | general | 2D, 3D, lightweight, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao)| | [SwarmCon](https://github.com/gestom/CosPhi/tree/master/Localization) | μ-swarms | 2D, individual IDs, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao) | | [Caspa-WhyCon](http://robotics.fel.cvut.cz/faigl/caspa/) | UAVs | embedded, open HW-SW solution | [Jan Faigl](https://scholar.google.co.uk/citations?user=-finD_sAAAAJ&hl=en) | | [Social-card](https://github.com/strands-project/strands_social/tree/hydro-devel/social_card_reader) | HRI | orientation translated to commands | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao) | From 6b74c595e9e6435c84f9ae328e05d2282401b766 Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Mon, 12 Oct 2015 14:03:58 +0100 Subject: [PATCH 08/13] LCAS maintainers --- package.xml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index c827153..8ce349f 100644 --- a/package.xml +++ b/package.xml @@ -6,9 +6,15 @@ WhyCon localization system Matias Nitsche - Matias Nitsche + + + Marc Hanheide + Jaime Pulido Fentanes + Tom Krajnik + https://github.com/lrse/whycon https://github.com/lrse/whycon + LGPLv3 catkin From be4512984c6cf7870d2204477ed6006ef52e3efd Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Mon, 12 Oct 2015 14:21:33 +0100 Subject: [PATCH 09/13] fixed email --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 8ce349f..d8b994a 100644 --- a/package.xml +++ b/package.xml @@ -9,7 +9,7 @@ Marc Hanheide - Jaime Pulido Fentanes + Jaime Pulido Fentanes Tom Krajnik https://github.com/lrse/whycon From ef0c52d5134437597b024960e25c08beb01a8f7a Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Mon, 12 Oct 2015 14:22:03 +0100 Subject: [PATCH 10/13] fixed email --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index d8b994a..46fb535 100644 --- a/package.xml +++ b/package.xml @@ -10,7 +10,7 @@ Marc Hanheide Jaime Pulido Fentanes - Tom Krajnik + Tom Krajnik https://github.com/lrse/whycon https://github.com/lrse/whycon From ded0b2fd65cd06080b264c94ad8b011077746f76 Mon Sep 17 00:00:00 2001 From: STRANDS Date: Mon, 12 Oct 2015 15:09:45 +0000 Subject: [PATCH 11/13] added changelog --- CHANGELOG.rst | 363 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 363 insertions(+) create mode 100644 CHANGELOG.rst diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..4b5f3e0 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,363 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package whycon +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* fixed email +* fixed email +* LCAS maintainers +* Merge branch 'master' of github.com:lrse/whycon +* cleanup launch files +* insist on settings axis until success +* Merge pull request `#2 `_ from gestom/master + Fancy Readme +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* Readme +* Readme +* Readme +* README.md - cite link +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme improved. +* Merge branch 'master' of robotica.dc.uba.ar:lrse/whycon +* launchers +* fixes +* Merge branch 'master' of robotica.dc.uba.ar:lrse/whycon +* Merge branch 'master' of robotica.dc.uba.ar:lrse/whycon +* cambios en conf +* angles, although probably not useful +* Merge branch 'master' of robotica.dc.uba.ar:lrse/whycon +* keep mono.launch a general example +* quick fix for monochrome cameras; will actually make this better by taking advantage from it +* fix del robot state publisher +* changes +* config not correctly used +* switch some calculations to double, just in case +* disable ros flag missing in readme +* README again +* use /camera/image_rect_color by default +* readme update +* big change: detectors need not cleanup between detections and they ignore each other's circles for free; global cleanup is used only once at the end + since it is quite fast; all console printing is now turned off and can be turned on during compilation +* improvements (better resetting); robot_pose_publisher +* e-mail update +* Merge branch 'master' into unstable +* ignore +* readme +* ignore +* launch files +* use cv::eigen +* set release flags +* indigo fixes +* add debug info +* README, again +* bug in local window search fixed +* improved readme +* fixed some warnings +* GSL is no longer a dependency: switched to OpenCV's eigen() +* fix circle refinement +* fix package.xml +* package is now both ROS and non-ROS. everything in a single repo +* make into ROS compatible package +* fix compile error +* delete leftover MAVLink stuff +* after tracking loss, allow for inspecting only a local window around last detected area (default behaviuor); report partial results when not all circles are localized +* calibrator: remove leftover stereo crap; allow calibration from pre-recorded images +* raise exception when axis file is not found +* removed old files +* remove mavlink/mavconn/pcl support +* oops +* change citation key +* citation updates +* enable defines in doxyfile +* whycon installation and cmake files +* Doxyfile and little update in readme +* parameters for setting camera resolution +* inner/outer-diameter clashes +* Merge branch 'master' into unstable +* bug fix for live camera input +* fixed MAVCONN. separated viewer on own executable using MAVCONN +* little argument parsing fix +* disable usage of two cameras by camera calibrator (does not work) +* simplify readme +* output help into sections +* report time for whole localization also +* number frames from start of capture +* initialize transform to identity +* many changes + * more robust working for circles that disappear, explicit initialize step is gone. + * randomized threshold and full undistort available as compile time options +* undistort map precomputed. not yet used nor tested +* simplified code +* fix drawing of ellipses (looks uglier but it is correct). remove old code +* fixed wrong (inverse) application of distortion model. interface is not final +* allow refinments to be made when not using camera. removed commented out tbb code +* debugging facilities +* drawing fix! +* allow specifying diameters on command line. do not require axis for tracking +* missing files. ignore updated +* more work for mavconn +* moved +* initial support for MAVCONN (not finished, but compiles). rearranged files +* removed old code +* re-enabled output writing +* separated executable in two modes: axis setting and tracking. added comments in circle_detector.cpp +* missing localization viewer files +* initial (not finished) support for 3D visualization using PCL (optional) +* removed some warnings +* typo +* support for more robust command line options handling +* circle was regenerated on inkscape and is now provided in SVG/PDF +* fixed readme +* citation +* change link order for some strange compilers +* fix for numerical problem when circle is aligned with optical axis +* lot of stuff commented out (couts). fixed problem with TBB headers +* make a shared library of the main sources +* mirrored XY circle pos (to follow pixel coords). auto detection of correct axis order (assuming first circle as 0,0). establish_error.rb script to measure error +* start circle search where previous valid circle was found. speeds up a bit +* Merge branch 'many' + Conflicts: + many_circle_detector.cpp +* faster drawing (and only during init) +* timings +* working version +* do not tag white pixels on main loop, solves obscure bug. also, paint white, to speedup ignoring other circles +* fast buffer cleanup +* add comments and remove segmentArray, great memory reduction +* nothing important +* better font sizes, reduce memory requirements a bit +* executable now takes calibration file as parameter +* pleace =b +* README +* add circle pattern to repo +* rename +* big rename, makes for sense +* cleanups, disabled ellipse improving since that needs testing +* localization system working, simple tests performed. needs accuracy report yet +* homography based computation implemented, needs further testing +* missing file +* readme +* fixes and ellipse improvement +* cleanup gui +* support for similarit transform +* more friendly output and fixed problem when not detecting circles +* make N attempts on every frame (currently 50) and fix little bug +* disable tbb for now +* fix, old code was in the way +* latest changes by tom integrated. to be tested +* save axis transform +* save axis pose, fix ellipse display +* calibration by opencv +* save frames when clicking, allow setting real world scale (NOTE: ratio was set to 6:5 for X,Y) +* Tested and working! +* localization system 99% complete +* localizer code (for many circles) using TBB/serial +* first working version with images +* Contributors: Marc Hanheide, Matias N., Thomas Fischer, Tom Krajnik, v01d + +* fixed email +* fixed email +* LCAS maintainers +* Merge branch 'master' of github.com:lrse/whycon +* cleanup launch files +* insist on settings axis until success +* Merge pull request `#2 `_ from gestom/master + Fancy Readme +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* WhyCon versions +* Readme +* Readme +* Readme +* README.md - cite link +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme +* Readme improved. +* Merge branch 'master' of robotica.dc.uba.ar:lrse/whycon +* launchers +* fixes +* Merge branch 'master' of robotica.dc.uba.ar:lrse/whycon +* Merge branch 'master' of robotica.dc.uba.ar:lrse/whycon +* cambios en conf +* angles, although probably not useful +* Merge branch 'master' of robotica.dc.uba.ar:lrse/whycon +* keep mono.launch a general example +* quick fix for monochrome cameras; will actually make this better by taking advantage from it +* fix del robot state publisher +* changes +* config not correctly used +* switch some calculations to double, just in case +* disable ros flag missing in readme +* README again +* use /camera/image_rect_color by default +* readme update +* big change: detectors need not cleanup between detections and they ignore each other's circles for free; global cleanup is used only once at the end + since it is quite fast; all console printing is now turned off and can be turned on during compilation +* improvements (better resetting); robot_pose_publisher +* e-mail update +* Merge branch 'master' into unstable +* ignore +* readme +* ignore +* launch files +* use cv::eigen +* set release flags +* indigo fixes +* add debug info +* README, again +* bug in local window search fixed +* improved readme +* fixed some warnings +* GSL is no longer a dependency: switched to OpenCV's eigen() +* fix circle refinement +* fix package.xml +* package is now both ROS and non-ROS. everything in a single repo +* make into ROS compatible package +* fix compile error +* delete leftover MAVLink stuff +* after tracking loss, allow for inspecting only a local window around last detected area (default behaviuor); report partial results when not all circles are localized +* calibrator: remove leftover stereo crap; allow calibration from pre-recorded images +* raise exception when axis file is not found +* removed old files +* remove mavlink/mavconn/pcl support +* oops +* change citation key +* citation updates +* enable defines in doxyfile +* whycon installation and cmake files +* Doxyfile and little update in readme +* parameters for setting camera resolution +* inner/outer-diameter clashes +* Merge branch 'master' into unstable +* bug fix for live camera input +* fixed MAVCONN. separated viewer on own executable using MAVCONN +* little argument parsing fix +* disable usage of two cameras by camera calibrator (does not work) +* simplify readme +* output help into sections +* report time for whole localization also +* number frames from start of capture +* initialize transform to identity +* many changes + * more robust working for circles that disappear, explicit initialize step is gone. + * randomized threshold and full undistort available as compile time options +* undistort map precomputed. not yet used nor tested +* simplified code +* fix drawing of ellipses (looks uglier but it is correct). remove old code +* fixed wrong (inverse) application of distortion model. interface is not final +* allow refinments to be made when not using camera. removed commented out tbb code +* debugging facilities +* drawing fix! +* allow specifying diameters on command line. do not require axis for tracking +* missing files. ignore updated +* more work for mavconn +* moved +* initial support for MAVCONN (not finished, but compiles). rearranged files +* removed old code +* re-enabled output writing +* separated executable in two modes: axis setting and tracking. added comments in circle_detector.cpp +* missing localization viewer files +* initial (not finished) support for 3D visualization using PCL (optional) +* removed some warnings +* typo +* support for more robust command line options handling +* circle was regenerated on inkscape and is now provided in SVG/PDF +* fixed readme +* citation +* change link order for some strange compilers +* fix for numerical problem when circle is aligned with optical axis +* lot of stuff commented out (couts). fixed problem with TBB headers +* make a shared library of the main sources +* mirrored XY circle pos (to follow pixel coords). auto detection of correct axis order (assuming first circle as 0,0). establish_error.rb script to measure error +* start circle search where previous valid circle was found. speeds up a bit +* Merge branch 'many' + Conflicts: + many_circle_detector.cpp +* faster drawing (and only during init) +* timings +* working version +* do not tag white pixels on main loop, solves obscure bug. also, paint white, to speedup ignoring other circles +* fast buffer cleanup +* add comments and remove segmentArray, great memory reduction +* nothing important +* better font sizes, reduce memory requirements a bit +* executable now takes calibration file as parameter +* pleace =b +* README +* add circle pattern to repo +* rename +* big rename, makes for sense +* cleanups, disabled ellipse improving since that needs testing +* localization system working, simple tests performed. needs accuracy report yet +* homography based computation implemented, needs further testing +* missing file +* readme +* fixes and ellipse improvement +* cleanup gui +* support for similarit transform +* more friendly output and fixed problem when not detecting circles +* make N attempts on every frame (currently 50) and fix little bug +* disable tbb for now +* fix, old code was in the way +* latest changes by tom integrated. to be tested +* save axis transform +* save axis pose, fix ellipse display +* calibration by opencv +* save frames when clicking, allow setting real world scale (NOTE: ratio was set to 6:5 for X,Y) +* Tested and working! +* localization system 99% complete +* localizer code (for many circles) using TBB/serial +* first working version with images +* Contributors: Marc Hanheide, Matias N., Thomas Fischer, Tom Krajnik, v01d From f6913bbcf9183497a6413f1dc71d4674f6b53e50 Mon Sep 17 00:00:00 2001 From: STRANDS Date: Mon, 12 Oct 2015 15:10:13 +0000 Subject: [PATCH 12/13] 1.2.1 --- CHANGELOG.rst | 4 ++-- package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4b5f3e0..aad541a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package whycon ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.1 (2015-10-12) +------------------ * fixed email * fixed email * LCAS maintainers diff --git a/package.xml b/package.xml index 46fb535..15e8225 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ whycon - 1.2.0 + 1.2.1 WhyCon localization system From 18e2f93101f407e36cdcee40abdc77f7f8d5354d Mon Sep 17 00:00:00 2001 From: Tom Krajnik Date: Mon, 12 Oct 2015 16:55:27 +0100 Subject: [PATCH 13/13] Camera info manager added to package list --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index 15e8225..bc5a61e 100644 --- a/package.xml +++ b/package.xml @@ -24,10 +24,12 @@ roscpp sensor_msgs std_msgs + camera_info_manager message_runtime cv_bridge roscpp sensor_msgs std_msgs + camera_info_manager