From a824b240580bf711f3dc75e953a278079920e86a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 8 Aug 2016 14:35:39 -0400 Subject: [PATCH] Added util3d::transformLaserScan(). OdomF2F: filling local scan map field with key frame scane in odom info. DbViewer: Print database parameters in Info view. --- corelib/include/rtabmap/core/OdometryInfo.h | 1 + .../include/rtabmap/core/util3d_transforms.h | 4 ++ corelib/src/OdometryF2F.cpp | 4 ++ corelib/src/util3d.cpp | 3 +- corelib/src/util3d_transforms.cpp | 53 +++++++++++++++++++ guilib/src/DatabaseViewer.cpp | 16 ++++++ guilib/src/MainWindow.cpp | 2 +- 7 files changed, 80 insertions(+), 3 deletions(-) diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index a913c63f..3b76c71d 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -72,6 +72,7 @@ public: output.transformFiltered = transformFiltered; output.transformGroundTruth = transformGroundTruth; output.distanceTravelled = distanceTravelled; + output.type = type; return output; } diff --git a/corelib/include/rtabmap/core/util3d_transforms.h b/corelib/include/rtabmap/core/util3d_transforms.h index be0434d2..811e02b3 100644 --- a/corelib/include/rtabmap/core/util3d_transforms.h +++ b/corelib/include/rtabmap/core/util3d_transforms.h @@ -40,6 +40,10 @@ namespace rtabmap namespace util3d { +cv::Mat RTABMAP_EXP transformLaserScan( + const cv::Mat & laserScan, + const Transform & transform); + pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); diff --git a/corelib/src/OdometryF2F.cpp b/corelib/src/OdometryF2F.cpp index 1102cf3f..7d1f05c6 100644 --- a/corelib/src/OdometryF2F.cpp +++ b/corelib/src/OdometryF2F.cpp @@ -129,7 +129,11 @@ Transform OdometryF2F::computeTransform( { info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t))); } + info->localMapSize = tmpRefFrame.getWords3().size(); info->words = newFrame.getWords(); + + info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().cols; + info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), t); } } else diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index b8899f5b..c269c40e 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -948,12 +948,11 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, { cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6)); bool nullTransform = transform.isNull() || transform.isIdentity(); - Eigen::Affine3f transform3f = transform.toEigen3f(); for(unsigned int i=0; i(i)[0] = pt.x; laserScan.at(i)[1] = pt.y; laserScan.at(i)[2] = pt.z; diff --git a/corelib/src/util3d_transforms.cpp b/corelib/src/util3d_transforms.cpp index 174e7d7b..ed93bde4 100644 --- a/corelib/src/util3d_transforms.cpp +++ b/corelib/src/util3d_transforms.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_transforms.h" #include +#include namespace rtabmap { @@ -35,6 +36,58 @@ namespace rtabmap namespace util3d { +cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform) +{ + UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6)); + + cv::Mat output = laserScan.clone(); + + if(!transform.isNull() && !transform.isIdentity()) + { + for(int i=0; i(i)[0], + laserScan.at(i)[1], 0); + pt = util3d::transformPoint(pt, transform); + output.at(i)[0] = pt.x; + output.at(i)[1] = pt.y; + } + else if(laserScan.type() == CV_32FC3) + { + pcl::PointXYZ pt( + laserScan.at(i)[0], + laserScan.at(i)[1], + laserScan.at(i)[2]); + pt = util3d::transformPoint(pt, transform); + output.at(i)[0] = pt.x; + output.at(i)[1] = pt.y; + output.at(i)[2] = pt.z; + } + else + { + pcl::PointNormal pt; + pt.x=laserScan.at(i)[0]; + pt.y=laserScan.at(i)[1]; + pt.z=laserScan.at(i)[2]; + pt.normal_x=laserScan.at(i)[3]; + pt.normal_y=laserScan.at(i)[4]; + pt.normal_z=laserScan.at(i)[5]; + pt = util3d::transformPoint(pt, transform); + output.at(i)[0] = pt.x; + output.at(i)[1] = pt.y; + output.at(i)[2] = pt.z; + output.at(i)[3] = pt.normal_x; + output.at(i)[4] = pt.normal_y; + output.at(i)[5] = pt.normal_z; + } + } + } + return output; +} + pcl::PointCloud::Ptr transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index b45a8d13..fc926446 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1209,6 +1209,22 @@ void DatabaseViewer::updateIds() ui_->textEdit_info->append(""); ui_->textEdit_info->append(tr("%1 bad signatures in LTM").arg(badcountInLTM)); ui_->textEdit_info->append(tr("%1 bad signatures in the global graph").arg(badCountInGraph)); + ui_->textEdit_info->append(""); + ParametersMap parameters = dbDriver_->getLastParameters(); + QFontMetrics metrics(ui_->textEdit_info->font()); + int tabW = ui_->textEdit_info->tabStopWidth(); + for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + int strW = metrics.width(QString(iter->first.c_str()) + "="); + ui_->textEdit_info->append(tr("%1=%2%3") + .arg(iter->first.c_str()) + .arg(strW < tabW?"\t\t\t\t":strW < tabW*2?"\t\t\t":strW < tabW*3?"\t\t":"\t") + .arg(iter->second.c_str())); + } + + // move back the cursor at the beginning + ui_->textEdit_info->moveCursor(QTextCursor::Start) ; + ui_->textEdit_info->ensureCursorVisible() ; if(ids.size()) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 4b196e37..aca7797b 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -996,7 +996,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI if(_preferencesDialog->isScansShown(1)) { - // scan local map + // F2M: scan local map if(!odom.info().localScanMap.empty()) { pcl::PointCloud::Ptr cloud;