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

Fixed RGBD/LocalizationPriorError not used when adding external links

Browse Source
This commit is contained in:
matlabbe
2025-10-28 20:57:32 -07:00
parent 210aadb92b
commit a3076392b5
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+2 -1
View File
@@ -6315,6 +6315,7 @@ bool Rtabmap::addLink(const Link & link)
std::map<int, Transform> poses = _odomCachePoses; std::map<int, Transform> poses = _odomCachePoses;
std::multimap<int, Link> constraints = _odomCacheConstraints; std::multimap<int, Link> constraints = _odomCacheConstraints;
constraints.insert(std::make_pair(link.from(), link)); constraints.insert(std::make_pair(link.from(), link));
cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{ {
std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to()); std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
@@ -6322,7 +6323,7 @@ bool Rtabmap::addLink(const Link & link)
{ {
poses.insert(*iterPose); poses.insert(*iterPose);
// make the poses in the map fixed // make the poses in the map fixed
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*999999))); constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
} }
} }