i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 21:09:50 +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
corelib/src/odometry/OdometryFLOAM.cpp
+3 -3
View File
@@ -130,7 +130,7 @@ Transform OdometryFLOAM::computeTransform(
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999; cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
if(!lost_) 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()); UDEBUG("Scan conversion: %fs", timer.ticks());
@@ -152,8 +152,8 @@ Transform OdometryFLOAM::computeTransform(
if(!pose.isNull()) if(!pose.isNull())
{ {
covariance = cv::Mat::eye(6,6,CV_64FC1); covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance(cv::Range(0,3), cv::Range(0,3)) *= 0.01; covariance(cv::Range(0,3), cv::Range(0,3)) *= linVar_;
covariance(cv::Range(3,6), cv::Range(3,6)) *= 0.01; covariance(cv::Range(3,6), cv::Range(3,6)) *= angVar_;
t = lastPose_.inverse() * pose; // incremental t = lastPose_.inverse() * pose; // incremental
lastPose_ = pose; lastPose_ = pose;
guilib/src/DatabaseViewer.cpp
+19 -2
View File
@@ -7599,10 +7599,27 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0)); fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0)); toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty()) if(!fromS->sensorData().laserScanCompressed().isEmpty() && !toS->sensorData().laserScanCompressed().isEmpty())
{ {
UWARN("There are laser scans in data, but generate laser scan from " UWARN("There are laser scans in data, but generate laser scan from "
"depth image option is activated. Ignoring saved laser scans..."); "depth image option is activated (GUI Parameters->Refine). "
"Ignoring saved laser scans...");
}
else
{
QString msg = tr("Generating laser scan from depth image is checked "
"(GUI Parameters->Refine), but selected nodes don't contain "
"depth data. Empty laser scans are generated, so transform "
"estimation will likely fail. Uncheck to use laser scans instead "
"(if there are some).");
if(!silent)
{
QMessageBox::warning(this,
tr("Refine a link"),
msg);
}
UWARN(msg.toStdString().c_str());
} }
} }
else else