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

g2o: update initial pose estimate when prior is provided (https://github.com/introlab/rtabmap_ros/issues/1371)

Browse Source
This commit is contained in:
matlabbe
2025-11-23 14:07:01 -08:00
parent 3093df8e71
commit 3268707c00
2 changed files with 4 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/Rtabmap.cpp
View File

@@ -3260,7 +3260,6 @@ bool Rtabmap::process(
}
cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
std::map<int, Transform> addedPriors;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
@@ -3271,7 +3270,6 @@ bool Rtabmap::process(
// make the poses in the map fixed
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
addedPriors.insert(*iterPose);
}
UDEBUG("Constraint %d->%d: %s (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance());
}
@@ -3286,8 +3284,6 @@ bool Rtabmap::process(
// If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
// Set back map's poses corresponding to priors so that g2o can converge correctly (https://github.com/introlab/rtabmap_ros/issues/1371)
uInsert(posesOut, addedPriors);
if(ULogger::level() == ULogger::kDebug)
{
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)

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

@@ -593,7 +593,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
auto pose = g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
v1->setEstimate(pose); // This will help g2o to converge faster (https://github.com/introlab/rtabmap_ros/issues/1371)
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
@@ -674,6 +676,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
v1->setEstimate(pose); // This will help g2o to converge faster (https://github.com/introlab/rtabmap_ros/issues/1371)
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();