From b847c5001e4d9053f2de27ac1f3ff83919d79e4f Mon Sep 17 00:00:00 2001 From: mtlazaro Date: Tue, 4 Aug 2015 17:56:16 +0200 Subject: [PATCH] Fixed some bugs and changed visualization to color images --- src/thin_scan_matcher_node.cpp | 4 +++ src/tsm_core/cloud2d.cpp | 39 +++++++++------------- src/tsm_core/cloud2d.h | 4 +-- src/tsm_core/correspondence_finder2d.cpp | 42 ++++++++++++++---------- src/tsm_core/correspondence_finder2d.h | 15 +++++---- src/tsm_core/projector2d.cpp | 1 - 6 files changed, 55 insertions(+), 50 deletions(-) diff --git a/src/thin_scan_matcher_node.cpp b/src/thin_scan_matcher_node.cpp index 43dc617..d804859 100644 --- a/src/thin_scan_matcher_node.cpp +++ b/src/thin_scan_matcher_node.cpp @@ -42,6 +42,7 @@ int main(int argc, char **argv) { bool publish_odom; bool publish_tf; double min_correspondences_ratio; + int iterations; double local_map_clipping_range; double local_map_clipping_translation_threshold; @@ -70,6 +71,7 @@ int main(int argc, char **argv) { n.param("publish_odom", publish_odom, true); n.param("publish_tf", publish_tf, false); n.param("inlier_distance", inlier_distance, 0.1); + n.param("iterations", iterations, 10); n.param("frame_skip", frame_skip, 1); n.param("bpr", bpr, 0.2); n.param("min_correspondences_ratio", min_correspondences_ratio, 0.3); @@ -103,12 +105,14 @@ int main(int argc, char **argv) { std::cout << "_min_matching_range:=" << min_matching_range << std::endl; std::cout << "_num_matching_beams:=" << num_matching_beams << std::endl; std::cout << "_matching_fov:=" << matching_fov << std::endl; + std::cout << "_iterations:=" << iterations << std::endl; fflush(stdout); // Setting input parameters message_handler.setFrameSkip(frame_skip); tracker.setBpr(bpr); + tracker.setIterations(iterations); tracker.setInlierDistance(inlier_distance); tracker.setMinCorrespondencesRatio(min_correspondences_ratio); tracker.setLocalMapClippingRange(local_map_clipping_range); diff --git a/src/tsm_core/cloud2d.cpp b/src/tsm_core/cloud2d.cpp index 76e6c81..6668573 100644 --- a/src/tsm_core/cloud2d.cpp +++ b/src/tsm_core/cloud2d.cpp @@ -94,11 +94,9 @@ namespace tsm{ model = sparse_model; } - void Cloud2D::draw(UnsignedCharImage &img, bool draw_normals, Eigen::Isometry2f T, - bool draw_pose_origin) const { - std::vector pt_to_draw; - std::vector normal_to_draw; - int scale = 55; + void Cloud2D::draw(RGBImage &img, cv::Vec3b color, bool draw_normals, Eigen::Isometry2f T, + bool draw_pose_origin, float scale) const { + std::vector normal_to_draw, pt_to_draw; float max_value = std::numeric_limits::min(); for(Cloud2D::const_iterator it = begin(); it != end(); ++it) { @@ -118,44 +116,39 @@ namespace tsm{ pt_to_draw.push_back(cv::Point2i(static_cast(pt.x()), static_cast(pt.y()))); normal_to_draw.push_back(cv::Point2i(static_cast(n.x()), static_cast(n.y()))); } - + float max_dest_size = std::max(img.rows, img.cols); - float img_size = std::max(max_value * scale, max_dest_size); + int img_size = std::max(max_value * scale, max_dest_size); - UnsignedCharImage tmp = cv::Mat::zeros(img_size, img_size, CV_8UC1); - + RGBImage tmp = cv::Mat::zeros(img_size, img_size, CV_8UC3); for(size_t i = 0; i < pt_to_draw.size(); ++i) { cv::Point2i& pt = pt_to_draw[i]; - + pt.x += img_size * 0.5; pt.y += img_size * 0.5; - + cv::Point2i& normal = normal_to_draw[i]; if (draw_normals) - cv::line( - tmp, + cv::line(tmp, pt, pt + normal, 100 ); - tmp.at(pt.y, pt.x) = 255; + tmp.at(pt.y,pt.x) = color; } - + cv::circle(tmp, - cv::Point( - T.translation().x() * scale * 0.5 + img_size * 0.5, - T.translation().y() * scale * 0.5 + img_size * 0.5 - ), + cv::Point(T.translation().x() * scale * 0.5 + img_size * 0.5, + T.translation().y() * scale * 0.5 + img_size * 0.5), 3, - 150, + cv::Scalar(color[0],color[1],color[2]), CV_FILLED); - + if (img.size().area() > 0) { float max_cols = std::max(img.cols, tmp.cols); - cv::Rect roi = cv::Rect( - (tmp.cols - img.cols) * 0.5, + cv::Rect roi = cv::Rect((tmp.cols - img.cols) * 0.5, (tmp.rows - img.rows) * 0.5, img.cols, img.rows); diff --git a/src/tsm_core/cloud2d.h b/src/tsm_core/cloud2d.h index 3132b06..adcd9f2 100644 --- a/src/tsm_core/cloud2d.h +++ b/src/tsm_core/cloud2d.h @@ -68,9 +68,9 @@ namespace tsm { void voxelize(Cloud2D& model, float res); //! draws a cloud, by applying the provided transformation, with normals and origin pose - void draw(UnsignedCharImage& img, bool draw_normals = false, + void draw(RGBImage& img, cv::Vec3b color, bool draw_normals = false, Eigen::Isometry2f T = Eigen::Isometry2f::Identity(), - bool draw_pose_origin = false) const; + bool draw_pose_origin = false, float scale = 20) const; virtual ~Cloud2D(); }; diff --git a/src/tsm_core/correspondence_finder2d.cpp b/src/tsm_core/correspondence_finder2d.cpp index cb3a091..0aceda0 100644 --- a/src/tsm_core/correspondence_finder2d.cpp +++ b/src/tsm_core/correspondence_finder2d.cpp @@ -2,6 +2,15 @@ #include namespace tsm { + + CorrespondenceFinder2D::CorrespondenceFinder2D(Projector2D* projector, Solver2D* solver) { + _correspondences.clear(); + _projector = projector; + _solver = solver; + _max_squared_distance=0.3*0.3; + _min_normal_cos=cos(M_PI/8); + } + void CorrespondenceFinder2D::init() { _correspondences.clear(); _indices_current.clear(); @@ -43,40 +52,37 @@ namespace tsm { continue; RichPoint2D pt_curr = (*_solver->current())[idx1]; + pt_curr.transformInPlace(_solver->T()); RichPoint2D pt_ref = (*_solver->reference())[idx2]; - if((pt_curr.point() - pt_ref.point()).squaredNorm() < 0.09 && - pt_curr.normal().dot(pt_ref.normal()) > 0.8) { + if((pt_curr.point() - pt_ref.point()).squaredNorm() < _max_squared_distance && + pt_curr.normal().dot(pt_ref.normal()) > _min_normal_cos) { _correspondences.push_back(i); } } } - void CorrespondenceFinder2D::drawCorrespondences(UnsignedCharImage& img) const { - UnsignedCharImage tmp; - - _solver->current()->draw(tmp, true); - _solver->reference()->draw(tmp, true); - + void CorrespondenceFinder2D::drawCorrespondences(RGBImage &img, Eigen::Isometry2f T, float scale) const { + _solver->current()->draw(img, cv::Vec3b(0,255,0), false, T, false, scale); + _solver->reference()->draw(img, cv::Vec3b(0,0,255), false, Eigen::Isometry2f::Identity(), false, scale); + + T.linear()*=scale; for (size_t i = 0; i < _correspondences.size(); ++i) { int idx1 = _indices_current[_correspondences[i]]; int idx2 = _indices_reference[_correspondences[i]]; - Eigen::Vector2f pt_curr = (*_solver->current())[idx1].point() * 10; - Eigen::Vector2f pt_ref = (*_solver->reference())[idx2].point() * 10; - - float center_x = tmp.cols * 0.5; - float center_y = tmp.rows * 0.5; + Eigen::Vector2f pt_curr = T*(*_solver->current())[idx1].point() * 0.5; + Eigen::Vector2f pt_ref = T*(*_solver->reference())[idx2].point() * 0.5; - cv::line( - tmp, + float center_x = img.cols * 0.5; + float center_y = img.rows * 0.5; + cv::line(img, cv::Point(pt_curr.x() + center_x, pt_curr.y() + center_y), cv::Point(pt_ref.x() + center_x, pt_ref.y() + center_y), - 255 + cv::Scalar(255, 0, 0) ); } - - img = tmp; } + } diff --git a/src/tsm_core/correspondence_finder2d.h b/src/tsm_core/correspondence_finder2d.h index 737bee3..51dbc4f 100644 --- a/src/tsm_core/correspondence_finder2d.h +++ b/src/tsm_core/correspondence_finder2d.h @@ -7,18 +7,14 @@ namespace tsm { class CorrespondenceFinder2D { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; - CorrespondenceFinder2D(Projector2D* projector = NULL, Solver2D* solver = NULL) { - _correspondences.clear(); - _projector = projector; - _solver = solver; - } + CorrespondenceFinder2D(Projector2D* projector = NULL, Solver2D* solver = NULL); ~CorrespondenceFinder2D() {}; void compute(); void init(); inline const std::vector& correspondences() const { return _correspondences; } inline const IntVector& indicesCurrent() const { return _indices_current; } inline const IntVector& indicesReference() const { return _indices_reference; } - void drawCorrespondences(UnsignedCharImage& img) const; + void drawCorrespondences(RGBImage& img, Eigen::Isometry2f T, float scale=20) const; // setter & getter inline void setProjector(const Projector2D* projector) { @@ -27,12 +23,19 @@ namespace tsm { inline void setSolver(const Solver2D* solver) { _solver = solver; } + inline float minNormalCos() const {return _min_normal_cos;} + inline void setMinNormalCos(float mnc) {_min_normal_cos=mnc;} + inline float maxSquaredDistance() const {return _max_squared_distance;} + inline void setMaxSquaredDistance(float msd) {_max_squared_distance=msd;} + private: const Projector2D* _projector; const Solver2D* _solver; IntVector _indices_current, _indices_reference; FloatVector _projected_current_ranges, _projected_reference_ranges; + float _max_squared_distance; + float _min_normal_cos; std::vector _correspondences; }; } diff --git a/src/tsm_core/projector2d.cpp b/src/tsm_core/projector2d.cpp index 8bb0df3..acb3604 100644 --- a/src/tsm_core/projector2d.cpp +++ b/src/tsm_core/projector2d.cpp @@ -204,7 +204,6 @@ namespace tsm { if (p0.dot(n) > 0) n = -n; - Eigen::Vector2f t(n.y(), -n.x()); model[i]._normal = n; } }