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

RGBD/SavedLocalizationIgnored: if true, it is now starting at the origin (0,0,0) instead of null (not linked to graph).

Browse Source
This commit is contained in:
matlabbe
2020-04-23 11:57:01 -04:00
parent 046e0b7888
commit 00d8375a2e
5 changed files with 41 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/DBDriverSqlite3.cpp
+1 -1
View File
@@ -4702,7 +4702,7 @@ cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
void DBDriverSqlite3::saveOptimizedPosesQuery(const std::map<int, Transform> & poses, const Transform & lastlocalizationPose) const
{
UDEBUG("");
UDEBUG("poses=%d lastlocalizationPose=%s", (int)poses.size(), lastlocalizationPose.prettyPrint().c_str());
if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
{
UTimer timer;
corelib/src/Memory.cpp
+6 -3
View File
@@ -3066,6 +3066,7 @@ Transform Memory::computeIcpTransformMulti(
pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
UDEBUG("maxPoints from(%d) = %d", fromId, maxPoints);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first != fromId)
@@ -3075,7 +3076,7 @@ Transform Memory::computeIcpTransformMulti(
{
LaserScan scan;
s->sensorData().uncompressData(0, 0, &scan);
if(!scan.isEmpty() && scan.format() == fromScan.format())
if(!scan.isEmpty() && scan.format() == toScan.format())
{
if(scan.hasIntensity())
{
@@ -3106,12 +3107,13 @@ Transform Memory::computeIcpTransformMulti(
if(scan.size() > maxPoints)
{
UDEBUG("maxPoints scan(%d) = %d", iter->first, (int)scan.size());
maxPoints = scan.size();
}
}
else if(!scan.isEmpty())
{
UWARN("Incompatible scan format %d vs %d", (int)fromScan.format(), (int)scan.format());
UWARN("Incompatible scan format %s vs %s", toScan.formatName().c_str(), scan.formatName().c_str());
}
}
else
@@ -3138,13 +3140,14 @@ Transform Memory::computeIcpTransformMulti(
{
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
}
UDEBUG("assembledScan=%d points", assembledScan.cols);
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScan(
LaserScan(assembledScan,
fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
fromScan.rangeMax(),
fromScan.format(),
toScan.format(),
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);
corelib/src/Rtabmap.cpp
+12 -4
View File
@@ -318,17 +318,25 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
Transform lastPose;
_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), lastPose.prettyPrint().c_str());
if(!_optimizedPoses.empty())
{
if(!_savedLocalizationIgnored)
if(_savedLocalizationIgnored)
{
_lastLocalizationPose = lastPose;
UDEBUG("lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDSavedLocalizationIgnored().c_str());
lastPose.setIdentity();
}
_lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
std::map<int, Transform> tmp;
// Get just the links
_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
}
else
{
UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
}
if(_databasePath.empty())
{
@@ -5015,7 +5023,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
{
if(_lastLocalizationPose.isNull() || _optimizedPoses.empty())
{
UWARN("Last localization pose is null... cannot compute a path");
UWARN("Last localization pose is null or optimized graph is empty... cannot compute a path");
return false;
}
if(_optimizedPoses.begin()->first < 0)
guilib/src/DatabaseViewer.cpp
+6 -1
View File
@@ -3019,6 +3019,11 @@ void DatabaseViewer::regenerateSavedMap()
dbDriver_->save2DMap(map, xMin, yMin, grid.getCellSize());
Transform lastlocalizationPose;
dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
if(lastlocalizationPose.isNull() && !graphes_.back().empty())
{
// use last pose by default
lastlocalizationPose = graphes_.back().rbegin()->second;
}
dbDriver_->saveOptimizedPoses(graphes_.back(), lastlocalizationPose);
// reset optimized mesh as poses have changed
dbDriver_->saveOptimizedMesh(cv::Mat());
@@ -4345,7 +4350,7 @@ void DatabaseViewer::update(int value,
if(data.laserScanRaw().size())
{
labelScan->setText(tr("Format=%1 Points=%2 [max=%3] Range=[%4->%5 m] Angle=[%6->%7 rad inc=%8] Has [Color=%9 2D=%10 Normals=%11 Intensity=%12]")
.arg(data.laserScanRaw().format())
.arg(data.laserScanRaw().formatName().c_str())
.arg(data.laserScanRaw().size())
.arg(data.laserScanRaw().maxPoints())
.arg(data.laserScanRaw().rangeMin())
guilib/src/MainWindow.cpp
+16 -17
View File
@@ -1344,7 +1344,6 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
{
_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
}
_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
if(_preferencesDialog->isFramesShown())
{
@@ -1355,7 +1354,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_cloudViewer->removeLine("odom_to_base_link");
}
#endif
_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
}
_cloudViewer->update();
@@ -2041,6 +2040,21 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart());
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
if(_preferencesDialog->isFramesShown())
{
_cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false);
_cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false);
_cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), false, false);
}
else
{
_cloudViewer->removeLine("map_to_odom");
_cloudViewer->removeCoordinate("odom_frame");
_cloudViewer->removeCoordinate("map_frame");
}
#endif
if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
{
if(poses.rbegin()->first == stat.getLastSignatureData().id())
@@ -2063,21 +2077,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
}
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
if(_preferencesDialog->isFramesShown())
{
_cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false);
_cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false);
_cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), false, false);
}
else
{
_cloudViewer->removeLine("map_to_odom");
_cloudViewer->removeCoordinate("odom_frame");
_cloudViewer->removeCoordinate("map_frame");
}
#endif
if(_cachedSignatures.contains(0) && stat.refImageId()>0)
{
if(poses.find(stat.refImageId())!=poses.end())