diff --git a/CMakeLists.txt b/CMakeLists.txt index 2107f5d..07b5035 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,9 @@ find_package(catkin REQUIRED COMPONENTS tf ) + +SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) + set (CMAKE_BUILD_TYPE Release) #RASPBERRY PI 2 FLAGS if ($ENV{HOSTTYPE} MATCHES "arm") @@ -23,6 +26,20 @@ endif () FIND_PACKAGE(OpenCV) INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}) + +# OpenGL is used for drawing the models. +# Could put some conditional compilation in future releases +FIND_PACKAGE(OpenGL REQUIRED) +INCLUDE_DIRECTORIES(${OPENGL_INCLUDE}) +MESSAGE(STATUS "Compiling with OpenGL support") + +FIND_PACKAGE(Qt4 REQUIRED) +INCLUDE_DIRECTORIES(${QT_INCLUDES}) + +# For building the GUI +FIND_PACKAGE(QGLViewer REQUIRED) +INCLUDE_DIRECTORIES(${QGLVIEWER_INCLUDE_DIR}) + #FIND_PACKAGE(Eigen3 REQUIRED) #INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) INCLUDE_DIRECTORIES(/usr/include/eigen3) diff --git a/cmake_modules/FindQGLViewer.cmake b/cmake_modules/FindQGLViewer.cmake new file mode 100644 index 0000000..edce13a --- /dev/null +++ b/cmake_modules/FindQGLViewer.cmake @@ -0,0 +1,24 @@ +# Need to find both Qt4 and QGLViewer if the QQL support is to be built +FIND_PACKAGE(Qt4 COMPONENTS QtXml QtOpenGL QtGui) + +FIND_PATH(QGLVIEWER_INCLUDE_DIR qglviewer.h + /usr/include/QGLViewer + /opt/local/include/QGLViewer + /usr/local/include/QGLViewer + /sw/include/QGLViewer + ) + +FIND_LIBRARY(QGLVIEWER_LIBRARY NAMES qglviewer-qt4 QGLViewer + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + +IF(QGLVIEWER_INCLUDE_DIR AND QGLVIEWER_LIBRARY) + SET(QGLVIEWER_FOUND TRUE) +ELSE(QGLVIEWER_INCLUDE_DIR AND QGLVIEWER_LIBRARY) + SET(QGLVIEWER_FOUND FALSE) +ENDIF(QGLVIEWER_INCLUDE_DIR AND QGLVIEWER_LIBRARY) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e3f0580..db7cab1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -2,11 +2,17 @@ ADD_SUBDIRECTORY(tsm_core) add_executable(thin_scan_matcher_node - message_handler.cpp thin_scan_matcher_node.cpp ) target_link_libraries(thin_scan_matcher_node tsm_library + ${QGLVIEWER_LIBRARY} + ${QT_QTXML_LIBRARY} + ${QT_QTOPENGL_LIBRARY} + ${QT_QTGUI_LIBRARY} + ${QT_QTCORE_LIBRARY} + ${OPENGL_gl_LIBRARY} + ${OPENGL_glu_LIBRARY} ${catkin_LIBRARIES} ) diff --git a/src/message_handler.cpp b/src/message_handler.cpp deleted file mode 100644 index 2db8dfe..0000000 --- a/src/message_handler.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include "message_handler.h" - -MessageHandler::MessageHandler() { - _projector = NULL; - _frame_count = 0; - _frame_skip = 1; -} - -MessageHandler::~MessageHandler() { - if (_projector) - delete _projector; -} - -void MessageHandler::laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) { - ++_frame_count; - - if (_frame_count % _frame_skip != 0) - return; - - if (_projector == NULL) { - float fov = 0.f; - fov = msg->angle_increment * msg->ranges.size(); - _projector = new tsm::Projector2D(); - _projector->setMaxRange(msg->range_max); - _projector->setMinRange(msg->range_min); - _projector->setFov(fov); - _projector->setNumRanges(msg->ranges.size()); - } - - CloudWithTime* current = new CloudWithTime(msg->header.stamp); - _projector->unproject(*current, msg->ranges); - _clouds.push_back(current); -} - - diff --git a/src/message_handler.h b/src/message_handler.h deleted file mode 100644 index 5907342..0000000 --- a/src/message_handler.h +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -// ROS -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/CameraInfo.h" - -#include "tsm_core/cloud2d.h" -#include "tsm_core/projector2d.h" - - -struct CloudWithTime: public tsm::Cloud2D { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - CloudWithTime(const ros::Time& t){ - timestamp=t; - } - ros::Time timestamp; -}; - -class MessageHandler { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; - MessageHandler(); - ~MessageHandler(); - void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg); - - // setter & getter - inline void setFrameSkip(int frame_skip) { _frame_skip = frame_skip; } - inline int frameSkip() const { return _frame_skip; } - inline int laserFrameCount() const { return _frame_count; } - inline std::list& clouds() { return _clouds; } - inline const tsm::Projector2D* projector() const { return _projector; } - private: - tsm::Projector2D* _projector; - std::list _clouds; - int _frame_count; - int _frame_skip; -}; - diff --git a/src/thin_scan_matcher_node.cpp b/src/thin_scan_matcher_node.cpp index d804859..f03b2c1 100644 --- a/src/thin_scan_matcher_node.cpp +++ b/src/thin_scan_matcher_node.cpp @@ -6,9 +6,9 @@ #include #include #include "message_filters/sync_policies/approximate_time.h" +#include +#include #include "std_msgs/String.h" -#include "sensor_msgs/Image.h" -#include "sensor_msgs/CameraInfo.h" #include "nav_msgs/Odometry.h" #include "tf/transform_broadcaster.h" @@ -24,12 +24,96 @@ #include "tsm_core/solver2d.h" #include "tsm_core/tracker.h" -#include "message_handler.h" +#include "qapplication.h" +#include "tsm_core/tracker_viewer.h" + +using namespace std; + + +struct CloudWithTime: public tsm::Cloud2D { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + CloudWithTime(const ros::Time& t, const Eigen::Isometry2f& guess_=Eigen::Isometry2f::Identity()){ + timestamp=t; + guess=guess_; + } + ros::Time timestamp; + Eigen::Isometry2f guess; +}; + +class MessageHandler { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + MessageHandler(); + ~MessageHandler(); + void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg); + void imu_callback(const sensor_msgs::Imu::ConstPtr& msg); + + // setter & getter + inline void setFrameSkip(int frame_skip) { _frame_skip = frame_skip; } + inline int frameSkip() const { return _frame_skip; } + inline int laserFrameCount() const { return _frame_count; } + inline std::list& clouds() { return _clouds; } + inline const tsm::Projector2D* projector() const { return _projector; } + private: + tsm::Projector2D* _projector; + std::list _clouds; + sensor_msgs::Imu _last_imu_msg; + int _frame_count; + int _frame_skip; +}; + +MessageHandler::MessageHandler() { + _projector = NULL; + _frame_count = 0; + _frame_skip = 1; + _last_imu_msg.header.stamp=ros::Time(0.0d); +} + +MessageHandler::~MessageHandler() { + if (_projector) + delete _projector; +} + +void MessageHandler::laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) { + ++_frame_count; + + if (_frame_count % _frame_skip != 0) + return; + + if (_projector == NULL) { + float fov = 0.f; + fov = msg->angle_increment * msg->ranges.size(); + _projector = new tsm::Projector2D(); + _projector->setMaxRange(msg->range_max); + _projector->setMinRange(msg->range_min); + _projector->setFov(fov); + _projector->setNumRanges(msg->ranges.size()); + } + + Eigen::Isometry2f guess; + guess.setIdentity(); + if (_last_imu_msg.header.stamp.toSec()>0) { + guess.translation().setZero(); + float angle=Eigen::AngleAxisf(Eigen::Quaternionf(_last_imu_msg.orientation.x, + _last_imu_msg.orientation.y, + _last_imu_msg.orientation.z, + _last_imu_msg.orientation.w).toRotationMatrix()).angle(); + guess.linear()=Eigen::Rotation2Df(angle).toRotationMatrix(); + } +CloudWithTime* current = new CloudWithTime(msg->header.stamp, guess); + _projector->unproject(*current, msg->ranges); + _clouds.push_back(current); +} + +void MessageHandler::imu_callback(const sensor_msgs::Imu::ConstPtr& msg) { + _last_imu_msg= *msg; +} + int main(int argc, char **argv) { - ros::init(argc, argv, "tsm_test_node"); std::string laser_topic; + std::string imu_topic; std::string published_odom_topic; std::string published_tf_origin_frame_id; std::string published_tf_destination_frame_id; @@ -57,11 +141,14 @@ int main(int argc, char **argv) { tsm::Projector2D* projector = 0; tsm::Tracker tracker; tracker.setSolver(&solver); + bool use_gui; + ros::init(argc, argv, "tsm_test_node"); ros::NodeHandle n("~"); // ROS parameters setting n.param("laser_topic", laser_topic, std::string("/scan")); + n.param("imu_topic", imu_topic, std::string("")); n.param("published_odom_topic", published_odom_topic, std::string("/odom_calib")); n.param("base_link_frame_id", base_link_frame_id, std::string("/base_link")); n.param("published_tf_origin_frame_id", published_tf_origin_frame_id, std::string("/tsm_frame")); @@ -81,20 +168,22 @@ int main(int argc, char **argv) { n.param("min_matching_range", min_matching_range, 0.0); n.param("num_matching_beams", num_matching_beams, 0); n.param("matching_fov", matching_fov, 0.0); + n.param("use_gui", use_gui, false); if (frame_skip <= 0) frame_skip = 1; std::cout << "Launched with params:" << std::endl; - std::cout << "_laser_topic:=" << laser_topic.c_str() << std::endl; - std::cout << "_published_odom_topic:=" << published_odom_topic.c_str() << std::endl; + std::cout << "_laser_topic:=" << laser_topic << std::endl; + std::cout << "_imu_topic:=" << imu_topic << std::endl; + std::cout << "_published_odom_topic:=" << published_odom_topic << std::endl; std::cout << "_frame_skip:=" << frame_skip << std::endl; std::cout << "_bpr:=" << bpr << std::endl; - std::cout << "_base_link_frame_id:=" << base_link_frame_id.c_str() << std::endl; - std::cout << "_published_tf_origin_frame_id:=" << published_tf_origin_frame_id.c_str() << std::endl; - std::cout << "_published_tf_destination_frame_id:=" << published_tf_destination_frame_id.c_str() << std::endl; - std::cout << "_current_map_topic:=" << current_map_topic.c_str() << std::endl; - std::cout << "_reference_map_topic:=" << reference_map_topic.c_str() << std::endl; + std::cout << "_base_link_frame_id:=" << base_link_frame_id << std::endl; + std::cout << "_published_tf_origin_frame_id:=" << published_tf_origin_frame_id << std::endl; + std::cout << "_published_tf_destination_frame_id:=" << published_tf_destination_frame_id << std::endl; + std::cout << "_current_map_topic:=" << current_map_topic << std::endl; + std::cout << "_reference_map_topic:=" << reference_map_topic << std::endl; std::cout << "_publish_odom:=" << publish_odom << std::endl; std::cout << "_publish_tf:=" << publish_tf << std::endl; std::cout << "_inlier_distance:=" << inlier_distance << std::endl; @@ -106,6 +195,8 @@ int main(int argc, char **argv) { std::cout << "_num_matching_beams:=" << num_matching_beams << std::endl; std::cout << "_matching_fov:=" << matching_fov << std::endl; std::cout << "_iterations:=" << iterations << std::endl; + std::cout << "_use_gui:=" << (use_gui?"true":"false") << std::endl; + fflush(stdout); @@ -121,13 +212,28 @@ int main(int argc, char **argv) { std::cerr << "inlier distance from tracker: "<< tracker.inlierDistance() << std::endl; // ROS topic subscriptions ros::Subscriber laser_sub = n.subscribe(laser_topic, 100, &MessageHandler::laser_callback, &message_handler); + + ros::Subscriber imu_sub; + if (imu_topic!="") + imu_sub = n.subscribe(imu_topic, 100, &MessageHandler::imu_callback, &message_handler); + ros::Publisher odom_pub = n.advertise(published_odom_topic, 1); CloudWithTime *cloud = 0; Eigen::Isometry2f global_t; std::cerr << "Entering ROS loop" << std::endl; - + + QApplication* app=0; + tsm::TrackerViewer* viewer=0; + if (use_gui) { + app=new QApplication(argc, argv); + viewer=new tsm::TrackerViewer(&tracker); + viewer->init(); + viewer->show(); + } + Eigen::Isometry2f previous_guess; + bool has_guess=false; while (ros::ok()) { std::list& clouds = message_handler.clouds(); @@ -163,8 +269,14 @@ int main(int argc, char **argv) { ros::Time timestamp = cloud->timestamp; clouds.pop_front(); - - tracker.update(cloud); + if(has_guess) { + tracker.update(cloud, previous_guess.inverse()*cloud->guess); + } else { + tracker.update(cloud); + has_guess=true; + } + previous_guess=cloud->guess; + global_t = tracker.globalT(); Eigen::Matrix3f rot = Eigen::Matrix3f::Identity(); @@ -207,6 +319,11 @@ int main(int argc, char **argv) { } ros::spinOnce(); + if (use_gui) { + viewer->updateGL(); + app->processEvents(); + usleep(20000); + } //usleep(10000); } diff --git a/src/tsm_core/CMakeLists.txt b/src/tsm_core/CMakeLists.txt index e9abb41..adad52b 100644 --- a/src/tsm_core/CMakeLists.txt +++ b/src/tsm_core/CMakeLists.txt @@ -5,9 +5,20 @@ ADD_LIBRARY(tsm_library SHARED cloud_processor.cpp cloud_processor.h tracker.cpp tracker.h correspondence_finder2d.cpp correspondence_finder2d.h + tracker_viewer.cpp tracker_viewer.h ) -TARGET_LINK_LIBRARIES(tsm_library ${OpenCV_LIBS} ${OPENGL_gl_LIBRARY}) +TARGET_LINK_LIBRARIES( + tsm_library + ${QGLVIEWER_LIBRARY} + ${QT_QTXML_LIBRARY} + ${QT_QTOPENGL_LIBRARY} + ${QT_QTGUI_LIBRARY} + ${QT_QTCORE_LIBRARY} + ${OPENGL_gl_LIBRARY} + ${OPENGL_glu_LIBRARY} + ${OpenCV_LIBS} +) #ADD_EXECUTABLE (depth_utils_test depth_utils_test.cpp) diff --git a/src/tsm_core/cloud2d.cpp b/src/tsm_core/cloud2d.cpp index 6668573..0d738ef 100644 --- a/src/tsm_core/cloud2d.cpp +++ b/src/tsm_core/cloud2d.cpp @@ -1,6 +1,7 @@ #include "cloud2d.h" #include #include +#include namespace tsm{ void Cloud2D::add(const Cloud2D& other) { @@ -94,71 +95,121 @@ namespace tsm{ model = sparse_model; } - 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(); + // 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) { - Eigen::Vector2f pt = T * it->point(); - Eigen::Vector2f n = T.linear() * it->normal(); - float value = std::max(std::fabs(pt.x()), std::fabs(pt.y())); + // for(Cloud2D::const_iterator it = begin(); it != end(); ++it) { + // Eigen::Vector2f pt = T * it->point(); + // Eigen::Vector2f n = T.linear() * it->normal(); + // float value = std::max(std::fabs(pt.x()), std::fabs(pt.y())); - if (value > max_value) - max_value = value + 1; + // if (value > max_value) + // max_value = value + 1; - pt = pt * scale * 0.5; - n = n * scale * 0.5; + // pt = pt * scale * 0.5; + // n = n * scale * 0.5; - if (pt.x() != pt.x() || pt.y() != pt.y()) - continue; + // if (pt.x() != pt.x() || pt.y() != pt.y()) + // continue; - 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()))); - } + // 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); - int img_size = std::max(max_value * scale, max_dest_size); + // float max_dest_size = std::max(img.rows, img.cols); + // int img_size = std::max(max_value * scale, max_dest_size); - 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]; + // 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; + // pt.x += img_size * 0.5; + // pt.y += img_size * 0.5; - cv::Point2i& normal = normal_to_draw[i]; + // cv::Point2i& normal = normal_to_draw[i]; - if (draw_normals) - cv::line(tmp, - pt, - pt + normal, - 100 - ); + // if (draw_normals) + // cv::line(tmp, + // pt, + // pt + normal, + // 100 + // ); - tmp.at(pt.y,pt.x) = color; - } + // 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), - 3, - cv::Scalar(color[0],color[1],color[2]), - CV_FILLED); + // 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), + // 3, + // 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, - (tmp.rows - img.rows) * 0.5, - img.cols, - img.rows); - tmp(roi) += img; - } + // 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, + // (tmp.rows - img.rows) * 0.5, + // img.cols, + // img.rows); + // tmp(roi) += img; + // } + + // img = tmp; + // } - img = tmp; - } + Cloud2D::~Cloud2D(){} - Cloud2D::~Cloud2D(){} + void Cloud2D::draw(bool draw_normals, + Eigen::Isometry2f T, + bool draw_pose_origin, + bool use_fans) const { + glPushMatrix(); + Eigen::Vector3f t=t2v(T); + glTranslatef(t.x(), + t.y(), + 0); + glRotatef(t.z()*180.0f/M_PI,0,0,1); + + + glBegin(GL_POINTS); + for(size_t i=0; i - +#include namespace tsm { CorrespondenceFinder2D::CorrespondenceFinder2D(Projector2D* projector, Solver2D* solver) { @@ -62,27 +62,50 @@ namespace tsm { } } - 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); + // 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 = T*(*_solver->current())[idx1].point() * 0.5; + // Eigen::Vector2f pt_ref = T*(*_solver->reference())[idx2].point() * 0.5; + + // 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), + // cv::Scalar(255, 0, 0) + // ); + // } + // } - T.linear()*=scale; + + void CorrespondenceFinder2D::drawCorrespondences(Eigen::Isometry2f T) const { + glPushAttrib(GL_COLOR); + glColor3f(0,1,0); + _solver->current()->draw(false, T); + glColor3f(0,0,1); + _solver->reference()->draw(false); + glColor3f(1,0,0); + glBegin(GL_LINES); 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 = T*(*_solver->current())[idx1].point() * 0.5; - Eigen::Vector2f pt_ref = T*(*_solver->reference())[idx2].point() * 0.5; - - 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), - cv::Scalar(255, 0, 0) - ); + int idx1 = _indices_current[_correspondences[i]]; + int idx2 = _indices_reference[_correspondences[i]]; + + Eigen::Vector2f pt_curr = T*(*_solver->current())[idx1].point(); + Eigen::Vector2f pt_ref = (*_solver->reference())[idx2].point(); + glNormal3f(0,0,1); + glVertex3f(pt_ref.x(), pt_ref.y(),0); + glNormal3f(0,0,1); + glVertex3f(pt_curr.x(), pt_curr.y(),0); } + glEnd(); + glPopAttrib(); } - } diff --git a/src/tsm_core/correspondence_finder2d.h b/src/tsm_core/correspondence_finder2d.h index 51dbc4f..ea95601 100644 --- a/src/tsm_core/correspondence_finder2d.h +++ b/src/tsm_core/correspondence_finder2d.h @@ -14,8 +14,8 @@ namespace tsm { 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(RGBImage& img, Eigen::Isometry2f T, float scale=20) const; - + //void drawCorrespondences(RGBImage& img, Eigen::Isometry2f T, float scale=20) const; + void drawCorrespondences(Eigen::Isometry2f T) const; // setter & getter inline void setProjector(const Projector2D* projector) { _projector = projector; diff --git a/src/tsm_core/tracker.cpp b/src/tsm_core/tracker.cpp index 7cd47d6..71b0eb4 100644 --- a/src/tsm_core/tracker.cpp +++ b/src/tsm_core/tracker.cpp @@ -48,6 +48,12 @@ namespace tsm { void Tracker::update(Cloud2D* cloud_, const Eigen::Isometry2f& initial_guess) { + + if (_current && _reference && _reference != _current) { + delete _current; + _current = 0; + } + if (cloud_) setCurrent(cloud_); double init = getTime(); @@ -79,10 +85,10 @@ namespace tsm { for (int i = 0; i < _iterations; ++i) { _correspondence_finder.compute(); _solver->optimize( - _correspondence_finder.correspondences(), - _correspondence_finder.indicesCurrent(), - _correspondence_finder.indicesReference() - ); + _correspondence_finder.correspondences(), + _correspondence_finder.indicesCurrent(), + _correspondence_finder.indicesReference() + ); } _current->transformInPlace(_solver->T()); @@ -130,17 +136,12 @@ namespace tsm { if (current_bad_points_ratio <= _bpr) { _cloud_processor.merge(reference_ranges, reference_indices, *_reference, - current_ranges, current_indices, *_current, - 1.f, 0.5f); + current_ranges, current_indices, *_current, + 1.f, 0.5f); _global_t = _global_t * _solver->T(); //_reference->voxelize(*_reference, 2); _reference->transformInPlace(_solver->T().inverse()); - - if (_current && _reference != _current) { - delete _current; - _current = 0; - } } else { _last_clipped_pose = _global_t; diff --git a/src/tsm_core/tracker_viewer.cpp b/src/tsm_core/tracker_viewer.cpp new file mode 100644 index 0000000..9945f4a --- /dev/null +++ b/src/tsm_core/tracker_viewer.cpp @@ -0,0 +1,109 @@ +#include "tracker_viewer.h" +namespace tsm { + using namespace std; + using namespace Eigen; + + class StandardCamera: public qglviewer::Camera { + public: + StandardCamera(): _standard(true) {} + + float zNear() const { + if(_standard) { return 0.001f; } + else { return Camera::zNear(); } + } + + float zFar() const { + if(_standard) { return 10000.0f; } + else { return Camera::zFar(); } + } + + bool standard() const { return _standard; } + void setStandard(bool s) { _standard = s; } + + protected: + bool _standard; + }; + + + QKeyEvent* TrackerViewer::lastKeyEvent() { + if (_last_key_event_processed) + return 0; + return &_last_key_event; + } + + void TrackerViewer::keyEventProcessed() { + _last_key_event_processed = true; + } + TrackerViewer::TrackerViewer(Tracker* tracker_) : + _last_key_event(QEvent::None, 0, Qt::NoModifier){ + _last_key_event_processed = true; + _tracker=tracker_; + } + + void TrackerViewer::keyPressEvent(QKeyEvent *e) { + QGLViewer::keyPressEvent(e); + _last_key_event = *e; + _last_key_event_processed=false; + } + + void TrackerViewer::init() { + // Init QGLViewer. + QGLViewer::init(); + // Set background color light yellow. + // setBackgroundColor(QColor::fromRgb(255, 255, 194)); + + + // Set background color white. + setBackgroundColor(QColor::fromRgb(255, 255, 255)); + + // Set some default settings. + glEnable(GL_LINE_SMOOTH); + glEnable(GL_BLEND); + glEnable(GL_DEPTH_TEST); + glEnable(GL_NORMALIZE); + glShadeModel(GL_FLAT); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + // Don't save state. + setStateFileName(QString::null); + + // Mouse bindings. + setMouseBinding(Qt::RightButton, CAMERA, ZOOM); + setMouseBinding(Qt::MidButton, CAMERA, TRANSLATE); + setMouseBinding(Qt::ControlModifier, Qt::LeftButton, RAP_FROM_PIXEL); + + // Replace camera. + qglviewer::Camera *oldcam = camera(); + qglviewer::Camera *cam = new StandardCamera(); + setCamera(cam); + cam->setPosition(qglviewer::Vec(0.0f, 0.0f, 10.0f)); + cam->setUpVector(qglviewer::Vec(1.0f, 0.0f, 0.0f)); + cam->lookAt(qglviewer::Vec(0.0f, 0.0f, 0.0f)); + delete oldcam; + } + + void TrackerViewer::draw(){ + if (!_tracker) + return; + Eigen::Vector3f t=t2v(_tracker->globalT()); + glPushMatrix(); + glTranslatef(t.x(), t.y(), 0); + glRotatef(180.0f/M_PI*t.z(), 0,0,1); + + glPushAttrib(GL_COLOR|GL_POINT_SIZE); + glPointSize(1); + if (_tracker->reference()) { + glColor3f(1,0,0); + _tracker->reference()->draw(); + } + + glPointSize(3); + if (_tracker->current()) { + glColor3f(0,0,1); + _tracker->current()->draw(false, _tracker->solver()->T().inverse(),false,true); + } + glPopAttrib(); + glPopMatrix(); + } + +} diff --git a/src/tsm_core/tracker_viewer.h b/src/tsm_core/tracker_viewer.h new file mode 100644 index 0000000..1dd2832 --- /dev/null +++ b/src/tsm_core/tracker_viewer.h @@ -0,0 +1,32 @@ +#pragma once +#include "tracker.h" +#include "qglviewer.h" +#include + +namespace tsm { + + class TrackerViewer: public QGLViewer{ + public: + //! ctor + TrackerViewer(Tracker* tracker_); + //! init method, opens the gl viewport and sets the key bindings + void init(); + //! callback invoked by the application on new key event. It saves the last event in + //! a member variable + virtual void keyPressEvent(QKeyEvent *e); + + //! returns the last key pressed since invoking keyEventProcessed(); + QKeyEvent* lastKeyEvent(); + + //! call this to clear the events, after processing them + void keyEventProcessed(); + + void draw(); + protected: + QKeyEvent _last_key_event; + bool _last_key_event_processed; + Tracker* _tracker; + }; + + +}