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

Added RGBD/ProximityMergedScanCovFactor parameter. Localization: fixed output height when Reg/Force3DoF=true but input poses are 6DoF. Transform: added is3DoF() and is4DoF() functions. GTSAM: when Reg/Force3DoF=true, copy input roll,pitch,z values for output poses. RegIcp: fixed working memory dir '~' conversion. RGBD/ProximityGlobalScanMap: fixed map::at error when some nodes don't have scans.

Browse Source
This commit is contained in:
matlabbe
2022-01-12 16:14:42 -05:00
parent a4ec95963e
commit fcec98105d
14 changed files with 122 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -359,7 +359,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
int landmarkVertexOffset = poses.rbegin()->first+1;
std::map<int, bool> isLandmarkWithRotation;
UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d)", rootId, hasGravityConstraints?1:0);
UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());

3
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -625,8 +625,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(key > 0)
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
}
else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
{