i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added util3d::transformLaserScan(). OdomF2F: filling local scan map field with key frame scane in odom info. DbViewer: Print database parameters in Info view.

Browse Source
This commit is contained in:
matlabbe
2016-08-08 14:35:39 -04:00
parent 6ef9034a67
commit a824b24058
7 changed files with 80 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/OdometryInfo.h
+1
View File
@@ -72,6 +72,7 @@ public:
output.transformFiltered = transformFiltered;
output.transformGroundTruth = transformGroundTruth;
output.distanceTravelled = distanceTravelled;
output.type = type;
return output;
}
corelib/include/rtabmap/core/util3d_transforms.h
+4
View File
@@ -40,6 +40,10 @@ namespace rtabmap
namespace util3d
{
cv::Mat RTABMAP_EXP transformLaserScan(
const cv::Mat & laserScan,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & transform);
corelib/src/OdometryF2F.cpp
+4
View File
@@ -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
corelib/src/util3d.cpp
+1 -2
View File
@@ -948,12 +948,11 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & 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<cloud.size(); ++i)
{
if(!nullTransform)
{
pcl::PointNormal pt = pcl::transformPoint(cloud.at(i), transform3f);
pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
laserScan.at<cv::Vec6f>(i)[0] = pt.x;
laserScan.at<cv::Vec6f>(i)[1] = pt.y;
laserScan.at<cv::Vec6f>(i)[2] = pt.z;
corelib/src/util3d_transforms.cpp
+53
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_transforms.h"
#include <pcl/common/transforms.h>
#include <rtabmap/utilite/ULogger.h>
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<laserScan.cols; ++i)
{
if(laserScan.type() == CV_32FC2)
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec2f>(i)[0],
laserScan.at<cv::Vec2f>(i)[1], 0);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec2f>(i)[0] = pt.x;
output.at<cv::Vec2f>(i)[1] = pt.y;
}
else if(laserScan.type() == CV_32FC3)
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec3f>(i)[0],
laserScan.at<cv::Vec3f>(i)[1],
laserScan.at<cv::Vec3f>(i)[2]);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec3f>(i)[0] = pt.x;
output.at<cv::Vec3f>(i)[1] = pt.y;
output.at<cv::Vec3f>(i)[2] = pt.z;
}
else
{
pcl::PointNormal pt;
pt.x=laserScan.at<cv::Vec6f>(i)[0];
pt.y=laserScan.at<cv::Vec6f>(i)[1];
pt.z=laserScan.at<cv::Vec6f>(i)[2];
pt.normal_x=laserScan.at<cv::Vec6f>(i)[3];
pt.normal_y=laserScan.at<cv::Vec6f>(i)[4];
pt.normal_z=laserScan.at<cv::Vec6f>(i)[5];
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec6f>(i)[0] = pt.x;
output.at<cv::Vec6f>(i)[1] = pt.y;
output.at<cv::Vec6f>(i)[2] = pt.z;
output.at<cv::Vec6f>(i)[3] = pt.normal_x;
output.at<cv::Vec6f>(i)[4] = pt.normal_y;
output.at<cv::Vec6f>(i)[5] = pt.normal_z;
}
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & transform)
guilib/src/DatabaseViewer.cpp
+16
View File
@@ -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())
{
guilib/src/MainWindow.cpp
+1 -1
View File
@@ -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<pcl::PointNormal>::Ptr cloud;