From b19a82ef34c24da8ccf28d184f84e95a81ea6d9a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 16 Nov 2019 21:27:45 -0500 Subject: [PATCH 01/10] obstacles_detection: added warning if input cloud is indicated to be not dense but only has 1 row --- src/nodelets/obstacles_detection.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 8576529f..69dac3a3 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -52,7 +52,8 @@ public: ObstaclesDetection() : frameId_("base_link"), waitForTransform_(false), - mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()) + mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()), + warned_(false) {} virtual ~ObstaclesDetection() @@ -109,6 +110,9 @@ private: ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); + int queueSize = 10; pnh.param("queue_size", queueSize, queueSize); pnh.param("frame_id", frameId_, frameId_); @@ -295,6 +299,17 @@ private: std::vector indices; pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices); } + else if(!inputCloud->is_dense && inputCloud->height == 1) + { + if(!warned_) + { + NODELET_WARN("Detected possible wrong format of point cloud \"%s\", it is " + "indicated that it is not dense, but there is only one row. " + "Assuming it is dense... This message will only appear once.", cloudSub_.getTopic().c_str()); + warned_ = true; + } + inputCloud->is_dense = true; + } //Common variables for all strategies pcl::IndicesPtr ground, obstacles; @@ -425,6 +440,7 @@ private: rtabmap::OccupancyGrid grid_; bool mapFrameProjection_; + bool warned_; tf::TransformListener tfListener_; From 26b8fb5027dfc25af990a15fe358d7fdd8699cd5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 Nov 2019 08:53:02 -0500 Subject: [PATCH 02/10] pointcloud_to_depthimage: Handling the case of empty input cloud to avoid an assert #364 --- src/nodelets/pointcloud_to_depthimage.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/nodelets/pointcloud_to_depthimage.cpp b/src/nodelets/pointcloud_to_depthimage.cpp index f6b823dd..1890962d 100644 --- a/src/nodelets/pointcloud_to_depthimage.cpp +++ b/src/nodelets/pointcloud_to_depthimage.cpp @@ -200,13 +200,22 @@ private: pcl_conversions::toPCL(*pointCloud2Msg, *cloud); cv_bridge::CvImage depthImage; - depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform()); - if(fillHolesSize_ > 0 && fillIterations_ > 0) + if(cloud->data.empty()) { - for(int i=0; i 0 && fillIterations_ > 0) { - depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_); + for(int i=0; i Date: Thu, 21 Nov 2019 10:47:44 -0500 Subject: [PATCH 03/10] :lipstick: --- src/nodelets/point_cloud_assembler.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/nodelets/point_cloud_assembler.cpp b/src/nodelets/point_cloud_assembler.cpp index 36560576..b7b9265e 100644 --- a/src/nodelets/point_cloud_assembler.cpp +++ b/src/nodelets/point_cloud_assembler.cpp @@ -107,7 +107,7 @@ private: pnh.param("range_min", rangeMin_, rangeMin_); pnh.param("range_max", rangeMax_, rangeMax_); pnh.param("voxel_size", voxelSize_, voxelSize_); - ROS_ASSERT(maxClouds_>=0 && assemblingTime_ >=0); + ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0); cloudsSkipped_ = skipClouds_; @@ -168,9 +168,9 @@ private: *cpy = *cloudMsg; clouds_.push_back(cpy); - if( (int)clouds_.size() >= maxClouds_ && maxClouds_ != 0 - || - (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ != 0.0 ) + if( (int)clouds_.size() >= maxClouds_ && maxClouds_ > 0 + || + (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ > 0.0 ) { pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*clouds_.back(), *assembled); @@ -205,7 +205,7 @@ private: pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp); } else - { + { sensor_msgs::PointCloud2 output; pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output); pcl::PCLPointCloud2 output2; From 272568a0d9307b92c8bb8125f2e66eb71fe5fada Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 23 Nov 2019 22:00:23 -0500 Subject: [PATCH 04/10] rtabmap: added subscribe_inter_odom_info option (sync odometry and info msgs for intermediate nodes) --- include/rtabmap_ros/CoreWrapper.h | 9 +++- src/CoreWrapper.cpp | 88 ++++++++++++++++++++++++++----- 2 files changed, 83 insertions(+), 14 deletions(-) diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index 5bd6681c..bc264483 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_ros/Goal.h" #include "rtabmap_ros/GetPlan.h" #include "rtabmap_ros/CommonDataSubscriber.h" +#include "rtabmap_ros/OdomInfo.h" #include "MapsManager.h" @@ -144,6 +145,7 @@ private: #endif void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections); void interOdomCallback(const nav_msgs::OdometryConstPtr & msg); + void interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2); void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg); @@ -320,8 +322,13 @@ private: std::map tags_; ros::Subscriber imuSub_; std::map imus_; + ros::Subscriber interOdomSub_; - std::list interOdoms_; + std::list> interOdoms_; + message_filters::Subscriber interOdomSyncSub_; + message_filters::Subscriber interOdomInfoSyncSub_; + typedef message_filters::sync_policies::ExactTime MyExactInterOdomSyncPolicy; + message_filters::Synchronizer * interOdomSync_; bool stereoToDepth_; bool odomSensorSync_; diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 61f7b607..111a1c90 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -112,6 +112,7 @@ CoreWrapper::CoreWrapper() : transformThread_(0), tfThreadRunning_(false), stereoToDepth_(false), + interOdomSync_(0), odomSensorSync_(false), rate_(Parameters::defaultRtabmapDetectionRate()), createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), @@ -518,8 +519,22 @@ void CoreWrapper::onInit() NODELET_INFO("Create intermediate nodes"); if(rate_ == 0.0f) { - NODELET_INFO("Subscribe to inter odom messges"); - interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this); + bool interOdomInfo = false; + pnh.getParam("subscribe_inter_odom_info", interOdomInfo); + if(interOdomInfo) + { + NODELET_INFO("Subscribe to inter odom + info messages"); + interOdomSync_ = new message_filters::Synchronizer(MyExactInterOdomSyncPolicy(queueSize_), interOdomSyncSub_, interOdomInfoSyncSub_); + interOdomSync_->registerCallback(boost::bind(&CoreWrapper::interOdomInfoCallback, this, _1, _2)); + interOdomSyncSub_.subscribe(nh, "inter_odom", 1); + interOdomInfoSyncSub_.subscribe(nh, "inter_odom_info", 1); + } + else + { + NODELET_INFO("Subscribe to inter odom messages"); + interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this); + } + } } } @@ -742,6 +757,7 @@ CoreWrapper::~CoreWrapper() rtabmap_.close(); printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); + delete interOdomSync_; delete mbClient_; } @@ -1638,24 +1654,24 @@ void CoreWrapper::process( if(rtabmap_.isIDsGenerated() || data.id() > 0) { // Add intermediate nodes? - for(std::list::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();) + for(std::list >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();) { - if(iter->header.stamp < lastPoseStamp_) + if(iter->first.header.stamp < lastPoseStamp_) { - Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->pose.pose); + Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose); if(!interOdom.isNull()) { cv::Mat covariance; - double variance = iter->twist.covariance[0]; + double variance = iter->first.twist.covariance[0]; if(variance == BAD_COVARIANCE || variance <= 0.0f) { //use the one of the pose - covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->pose.covariance.data()).clone(); + covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.pose.covariance.data()).clone(); covariance /= 2.0; } else { - covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->twist.covariance.data()).clone(); + covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.twist.covariance.data()).clone(); } if(!uIsFinite(covariance.at(0,0)) || covariance.at(0,0)<=0.0f) { @@ -1684,18 +1700,56 @@ void CoreWrapper::process( Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0), 0, cv::Size(1,2)); - SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->header.stamp)); + SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp)); Transform gt; if(!groundTruthFrameId_.empty()) { - gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); + gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->first.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); } interData.setGroundTruth(gt); - rtabmap_.process(interData, interOdom, covariance); + + std::map externalStats; + std::vector odomVelocity; + if(iter->second.timeEstimation != 0.0f) + { + OdometryInfo info = odomInfoFromROS(iter->second); + externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", info.localBundleTime*1000.0f)); + externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints)); + externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers)); + externalStats.insert(std::make_pair("Odometry/TotalTime/ms", info.timeEstimation*1000.0f)); + externalStats.insert(std::make_pair("Odometry/Registration/ms", info.reg.totalTime*1000.0f)); + float speed = 0.0f; + if(info.interval>0.0) + speed = info.transform.x()/info.interval*3.6; + externalStats.insert(std::make_pair("Odometry/Speed/kph", speed)); + externalStats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers)); + externalStats.insert(std::make_pair("Odometry/Features/", info.features)); + externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", info.distanceTravelled)); + externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded)); + externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames)); + externalStats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize)); + externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize)); + externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage)); + + if(info.interval>0.0) + { + odomVelocity.resize(6); + float x,y,z,roll,pitch,yaw; + info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + odomVelocity[0] = x/info.interval; + odomVelocity[1] = y/info.interval; + odomVelocity[2] = z/info.interval; + odomVelocity[3] = roll/info.interval; + odomVelocity[4] = pitch/info.interval; + odomVelocity[5] = yaw/info.interval; + } + } + + rtabmap_.process(interData, interOdom, covariance, odomVelocity, externalStats); } interOdoms_.erase(iter++); } - else if(iter->header.stamp == lastPoseStamp_) + else if(iter->first.header.stamp == lastPoseStamp_) { interOdoms_.erase(iter++); break; @@ -2181,7 +2235,15 @@ void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg) { if(!paused_) { - interOdoms_.push_back(*msg); + interOdoms_.push_back(std::make_pair(*msg, rtabmap_ros::OdomInfo())); + } +} + +void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2) +{ + if(!paused_) + { + interOdoms_.push_back(std::make_pair(*msg1, *msg2)); } } From 1aa0635de6644d5d0a4553fbaced770059f7e9df Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 23 Nov 2019 23:11:48 -0500 Subject: [PATCH 05/10] fixed build error on kinetic --- include/rtabmap_ros/CoreWrapper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index bc264483..66f5da94 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -324,7 +324,7 @@ private: std::map imus_; ros::Subscriber interOdomSub_; - std::list> interOdoms_; + std::list > interOdoms_; message_filters::Subscriber interOdomSyncSub_; message_filters::Subscriber interOdomInfoSyncSub_; typedef message_filters::sync_policies::ExactTime MyExactInterOdomSyncPolicy; From 473890e0953b7cd07cf36c75fb9632a4479a4d58 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 23 Nov 2019 23:22:32 -0500 Subject: [PATCH 06/10] Added kinetic/melodic travis builds --- .travis.yml | 132 ++++++++++++++++++++++++++++++++++++++-------------- 1 file changed, 96 insertions(+), 36 deletions(-) diff --git a/.travis.yml b/.travis.yml index 80e5b32c..ed57faad 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,47 +1,107 @@ sudo: true -dist: trusty language: cpp compiler: - gcc -addons: - apt: - packages: - - cmake - - libopencv-dev - - libqt4-dev - - libsqlite3-dev +matrix: + include: -install: - - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' - - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - - sudo apt-get update - - sudo apt-get install dpkg - - sudo apt-get -y install ros-indigo-ros-base libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev ros-indigo-costmap-2d ros-indigo-octomap-msgs ros-indigo-rviz ros-indigo-cv-bridge ros-indigo-move-base-msgs ros-indigo-costmap-2d ros-indigo-image-geometry ros-indigo-message-filters ros-indigo-image-transport ros-indigo-eigen-conversions ros-indigo-stereo-msgs ros-indigo-nav-msgs ros-indigo-sensor-msgs ros-indigo-tf-conversions ros-indigo-laser-geometry ros-indigo-pcl-conversions ros-indigo-pcl-ros ros-indigo-dynamic-reconfigure ros-indigo-nodelet + - dist: trusty + install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update + - sudo apt-get install dpkg + - sudo apt-get -y install ros-indigo-rtabmap + - sudo apt-get -y remove ros-indigo-rtabmap -script: - - source /opt/ros/indigo/setup.bash - - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages - - cd .. - - mkdir -p catkin_ws/src - - cd catkin_ws/src - - catkin_init_workspace - - cd .. - - catkin_make - - cd .. - - mv rtabmap_ros catkin_ws/src/. - - git clone https://github.com/introlab/rtabmap.git - - cd rtabmap - - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi - - mkdir -p build && cd build - - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. - - make - - make install - - cd ../../catkin_ws - - source devel/setup.bash - - catkin_make - - catkin_make install + script: + - source /opt/ros/indigo/setup.bash + - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages + - cd .. + - mkdir -p catkin_ws/src + - cd catkin_ws/src + - catkin_init_workspace + - cd .. + - catkin_make + - cd .. + - mv rtabmap_ros catkin_ws/src/. + - git clone https://github.com/introlab/rtabmap.git + - cd rtabmap + - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi + - mkdir -p build && cd build + - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. + - make + - make install + - cd ../../catkin_ws + - source devel/setup.bash + - catkin_make + - catkin_make install + + - dist: xenial + install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list' + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update + - sudo apt-get install dpkg + - sudo apt-get -y install ros-kinetic-rtabmap + - sudo apt-get -y remove ros-kinetic-rtabmap + + script: + - source /opt/ros/kinetic/setup.bash + - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages + - cd .. + - mkdir -p catkin_ws/src + - cd catkin_ws/src + - catkin_init_workspace + - cd .. + - catkin_make + - cd .. + - mv rtabmap_ros catkin_ws/src/. + - git clone https://github.com/introlab/rtabmap.git + - cd rtabmap + - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi + - mkdir -p build && cd build + - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. + - make + - make install + - cd ../../catkin_ws + - source devel/setup.bash + - catkin_make + - catkin_make install + + - dist: bionic + install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list' + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update + - sudo apt-get install dpkg + - sudo apt-get -y install ros-melodic-rtabmap + - sudo apt-get -y remove ros-melodic-rtabmap + + script: + - source /opt/ros/melodic/setup.bash + - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages + - cd .. + - mkdir -p catkin_ws/src + - cd catkin_ws/src + - catkin_init_workspace + - cd .. + - catkin_make + - cd .. + - mv rtabmap_ros catkin_ws/src/. + - git clone https://github.com/introlab/rtabmap.git + - cd rtabmap + - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi + - mkdir -p build && cd build + - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. + - make + - make install + - cd ../../catkin_ws + - source devel/setup.bash + - catkin_make + - catkin_make install notifications: email: From 9249cb543db084b754ed60addd937376a77617b3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 24 Nov 2019 09:04:54 -0500 Subject: [PATCH 07/10] Update .travis.yml --- .travis.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index ed57faad..c32188fb 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,7 +13,7 @@ matrix: - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - sudo apt-get install dpkg - - sudo apt-get -y install ros-indigo-rtabmap + - sudo apt-get -y install ros-indigo-ros-base ros-indigo-rtabmap - sudo apt-get -y remove ros-indigo-rtabmap script: @@ -45,7 +45,7 @@ matrix: - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - sudo apt-get install dpkg - - sudo apt-get -y install ros-kinetic-rtabmap + - sudo apt-get -y install ros-kinetic-ros-base ros-kinetic-rtabmap - sudo apt-get -y remove ros-kinetic-rtabmap script: @@ -77,7 +77,7 @@ matrix: - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - sudo apt-get install dpkg - - sudo apt-get -y install ros-melodic-rtabmap + - sudo apt-get -y install ros-melodic-ros-base ros-melodic-rtabmap - sudo apt-get -y remove ros-melodic-rtabmap script: From 510dc55b5e0af9172c957379f857169c58b95ee4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 24 Nov 2019 09:56:56 -0500 Subject: [PATCH 08/10] Update .travis.yml --- .travis.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.travis.yml b/.travis.yml index c32188fb..d8ccb7a4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,8 +13,8 @@ matrix: - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - sudo apt-get install dpkg - - sudo apt-get -y install ros-indigo-ros-base ros-indigo-rtabmap - - sudo apt-get -y remove ros-indigo-rtabmap + - sudo apt-get -y install ros-indigo-rtabmap-ros + - sudo apt-get -y remove ros-indigo-rtabmap-ros script: - source /opt/ros/indigo/setup.bash @@ -45,8 +45,8 @@ matrix: - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - sudo apt-get install dpkg - - sudo apt-get -y install ros-kinetic-ros-base ros-kinetic-rtabmap - - sudo apt-get -y remove ros-kinetic-rtabmap + - sudo apt-get -y install ros-kinetic-rtabmap-ros + - sudo apt-get -y remove ros-kinetic-rtabmap-ros script: - source /opt/ros/kinetic/setup.bash @@ -77,8 +77,8 @@ matrix: - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - sudo apt-get install dpkg - - sudo apt-get -y install ros-melodic-ros-base ros-melodic-rtabmap - - sudo apt-get -y remove ros-melodic-rtabmap + - sudo apt-get -y install ros-melodic-rtabmap-ros + - sudo apt-get -y remove ros-melodic-rtabmap-ros script: - source /opt/ros/melodic/setup.bash From cf2f16dd38c37f63bbfe1d80d01b08ead50cd850 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 24 Nov 2019 09:58:07 -0500 Subject: [PATCH 09/10] Update .travis.yml --- .travis.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index d8ccb7a4..cc9cf53d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,7 +14,7 @@ matrix: - sudo apt-get update - sudo apt-get install dpkg - sudo apt-get -y install ros-indigo-rtabmap-ros - - sudo apt-get -y remove ros-indigo-rtabmap-ros + - sudo apt-get -y remove ros-indigo-rtabmap script: - source /opt/ros/indigo/setup.bash @@ -46,7 +46,7 @@ matrix: - sudo apt-get update - sudo apt-get install dpkg - sudo apt-get -y install ros-kinetic-rtabmap-ros - - sudo apt-get -y remove ros-kinetic-rtabmap-ros + - sudo apt-get -y remove ros-kinetic-rtabmap script: - source /opt/ros/kinetic/setup.bash @@ -78,7 +78,7 @@ matrix: - sudo apt-get update - sudo apt-get install dpkg - sudo apt-get -y install ros-melodic-rtabmap-ros - - sudo apt-get -y remove ros-melodic-rtabmap-ros + - sudo apt-get -y remove ros-melodic-rtabmap script: - source /opt/ros/melodic/setup.bash From 954836c58bced5aa5eacbb94cb8cafd0f0aa5404 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 26 Nov 2019 18:33:05 -0500 Subject: [PATCH 10/10] rtabmap: Saving more odometry statistics to database --- include/rtabmap_ros/MsgConversion.h | 1 + src/CoreWrapper.cpp | 36 ++----------------------- src/MsgConversion.cpp | 42 +++++++++++++++++++++++++++++ 3 files changed, 45 insertions(+), 34 deletions(-) diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 1b6f071f..bdff7c42 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -152,6 +152,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg); void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg); +std::map odomInfoToStatistics(const rtabmap::OdometryInfo & info); rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg); void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg); diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 111a1c90..ef4f1b76 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1713,23 +1713,7 @@ void CoreWrapper::process( if(iter->second.timeEstimation != 0.0f) { OdometryInfo info = odomInfoFromROS(iter->second); - externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", info.localBundleTime*1000.0f)); - externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints)); - externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers)); - externalStats.insert(std::make_pair("Odometry/TotalTime/ms", info.timeEstimation*1000.0f)); - externalStats.insert(std::make_pair("Odometry/Registration/ms", info.reg.totalTime*1000.0f)); - float speed = 0.0f; - if(info.interval>0.0) - speed = info.transform.x()/info.interval*3.6; - externalStats.insert(std::make_pair("Odometry/Speed/kph", speed)); - externalStats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers)); - externalStats.insert(std::make_pair("Odometry/Features/", info.features)); - externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", info.distanceTravelled)); - externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded)); - externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames)); - externalStats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize)); - externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize)); - externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage)); + externalStats = rtabmap_ros::odomInfoToStatistics(info); if(info.interval>0.0) { @@ -1876,23 +1860,7 @@ void CoreWrapper::process( std::vector odomVelocity; if(odomInfo.timeEstimation != 0.0f) { - externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f)); - externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints)); - externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers)); - externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f)); - externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f)); - float speed = 0.0f; - if(odomInfo.interval>0.0) - speed = odomInfo.transform.x()/odomInfo.interval*3.6; - externalStats.insert(std::make_pair("Odometry/Speed/kph", speed)); - externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers)); - externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features)); - externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled)); - externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded)); - externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames)); - externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize)); - externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize)); - externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", odomInfo.memoryUsage)); + externalStats = rtabmap_ros::odomInfoToStatistics(odomInfo); if(odomInfo.interval>0.0) { diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 49314c16..39c3bcd3 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -1110,6 +1110,48 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose); } +std::map odomInfoToStatistics(const rtabmap::OdometryInfo & info) +{ + std::map stats; + + stats.insert(std::make_pair("Odometry/TimeRegistration/ms", info.reg.totalTime*1000.0f)); + stats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage)); + + // Based on rtabmap/MainWindow.cpp + stats.insert(std::make_pair("Odometry/Features/", info.features)); + stats.insert(std::make_pair("Odometry/Matches/", info.reg.matches)); + stats.insert(std::make_pair("Odometry/MatchesRatio/", info.features<=0?0.0f:float(info.reg.inliers)/float(info.features))); + stats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers)); + stats.insert(std::make_pair("Odometry/InliersMeanDistance/m", info.reg.inliersMeanDistance)); + stats.insert(std::make_pair("Odometry/InliersDistribution/", info.reg.inliersDistribution)); + stats.insert(std::make_pair("Odometry/InliersRatio/", info.reg.inliers)); + stats.insert(std::make_pair("Odometry/ICPInliersRatio/", info.reg.icpInliersRatio)); + stats.insert(std::make_pair("Odometry/ICPRotation/rad", info.reg.icpRotation)); + stats.insert(std::make_pair("Odometry/ICPTranslation/m", info.reg.icpTranslation)); + stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity)); + stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at(0,0)))); + stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at(5,5)))); + stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at(0,0))); + stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at(5,5))); + stats.insert(std::make_pair("Odometry/TimeEstimation/ms", info.timeEstimation*1000.0f)); + stats.insert(std::make_pair("Odometry/TimeFiltering/ms", info.timeParticleFiltering*1000.0f)); + stats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize)); + stats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize)); + stats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames)); + stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers)); + stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints)); + stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f)); + stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f)); + stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval)); + float speed = 0.0f; + if(info.interval>0.0) + speed = info.transform.x()/info.interval*3.6; + stats.insert(std::make_pair("Odometry/Speed/kph", speed)); + stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled)); + + return stats; +} + rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) { rtabmap::OdometryInfo info;