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

Fixed broken 2D icp from 0.11.0

Browse Source
This commit is contained in:
matlabbe
2016-02-19 15:10:28 -05:00
parent d5538ee99d
commit 9b16d93481
5 changed files with 12 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1779,6 +1779,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
(*iter)->sensorData().setCameraModels(models);
(*iter)->sensorData().setStereoCameraModel(stereoModel);
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

2
corelib/src/Parameters.cpp
View File

@@ -218,6 +218,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionByTime", std::make_pair(true, Parameters::kRGBDProximityByTime())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionBySpace", std::make_pair(true, Parameters::kRGBDProximityBySpace())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionTime", std::make_pair(true, Parameters::kRGBDProximityByTime())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionSpace", std::make_pair(true, Parameters::kRGBDProximityBySpace())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged", std::make_pair(true, Parameters::kRGBDProximityPathScansMerged())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxGraphDepth", std::make_pair(true, Parameters::kRGBDProximityMaxGraphDepth())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(true, Parameters::kRGBDProximityPathFilteringRadius())));

2
corelib/src/RegistrationIcp.cpp
View File

@@ -223,7 +223,7 @@ Transform RegistrationIcp::computeTransformationImpl(
hasConverged,
*fromCloudRegistered,
_epsilon,
!this->force3DoF()); // icp2D
this->force3DoF()); // icp2D
}
/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);

11
corelib/src/Rtabmap.cpp
View File

@@ -1002,7 +1002,7 @@ bool Rtabmap::process(
{
// set small variance
UDEBUG("Set small variance. The robot is not moving.");
_memory->updateLink(signature->id(), oldId, guess, 0.0001, 0.0001);
_memory->updateLink(oldId, signature->id(), guess, 0.0001, 0.0001);
}
}
else
@@ -1019,10 +1019,10 @@ bool Rtabmap::process(
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
signature->id(),
oldId,
signature->id(),
info.variance,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
guess.prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(info.variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
@@ -1049,7 +1049,7 @@ bool Rtabmap::process(
if(info.variance > 0)
{
double sqrtVar = sqrt(info.variance);
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
_memory->updateLink(oldId, signature->id(), guess, sqrtVar, sqrtVar);
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
@@ -1064,6 +1064,7 @@ bool Rtabmap::process(
UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId));
newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
_mapCorrection = newPose * signature->getPose().inverse();
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
@@ -1079,7 +1080,7 @@ bool Rtabmap::process(
newPose = _mapCorrection * signature->getPose();
}
UDEBUG("Added pose %s", newPose.prettyPrint().c_str());
UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str());
// Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose