i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed FLOAM not working when local transform is not Identity. DBViewer: warn user when scan from dpeth is checked and there are no depth images in db.

Browse Source
This commit is contained in:
matlabbe
2021-09-30 09:53:50 -04:00
parent 533d78d570
commit 77d947d4fb
2 changed files with 22 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/src/odometry/OdometryFLOAM.cpp
View File

@@ -130,7 +130,7 @@ Transform OdometryFLOAM::computeTransform(
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
if(!lost_)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudInPtr = util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudInPtr = util3d::laserScanToPointCloudI(data.laserScanRaw());
UDEBUG("Scan conversion: %fs", timer.ticks());
@@ -152,8 +152,8 @@ Transform OdometryFLOAM::computeTransform(
if(!pose.isNull())
{
covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance(cv::Range(0,3), cv::Range(0,3)) *= 0.01;
covariance(cv::Range(3,6), cv::Range(3,6)) *= 0.01;
covariance(cv::Range(0,3), cv::Range(0,3)) *= linVar_;
covariance(cv::Range(3,6), cv::Range(3,6)) *= angVar_;
t = lastPose_.inverse() * pose; // incremental
lastPose_ = pose;