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

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

2
corelib/src/DBDriverSqlite3.cpp
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;

9
corelib/src/Memory.cpp
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);

16
corelib/src/Rtabmap.cpp
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)