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

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

3
corelib/src/Rtabmap.cpp
View File

@@ -6315,6 +6315,7 @@ bool Rtabmap::addLink(const Link & link)
std::map<int, Transform> poses = _odomCachePoses;
std::multimap<int, Link> constraints = _odomCacheConstraints;
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)
{
std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
@@ -6322,7 +6323,7 @@ bool Rtabmap::addLink(const Link & link)
{
poses.insert(*iterPose);
// 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)));
}
}