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

Optimizer: don't fix roll/pitch on root node if gravity constraints are fed

Browse Source
This commit is contained in:
matlabbe
2021-12-25 16:57:03 -05:00
parent 93ee8f9b30
commit 3071da42f3
2 changed files with 78 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -310,14 +310,26 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
#endif
// detect if there is a global pose prior set, if so remove rootId
if(!priorsIgnored())
bool hasGravityConstraints = false;
if(!priorsIgnored() || (!isSlam2d() && gravitySigma() > 0))
{
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior)
if(iter->second.from() == iter->second.to())
{
rootId = 0;
break;
if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
{
rootId = 0;
break;
}
else if(iter->second.type() == Link::kGravity)
{
hasGravityConstraints = true;
if(priorsIgnored())
{
break;
}
}
}
}
}
@@ -325,7 +337,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)", rootId);
UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d)", rootId, hasGravityConstraints?1:0);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
@@ -388,7 +400,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
if(id == rootId)
if(id == rootId && !hasGravityConstraints)
{
UDEBUG("Set %d fixed", id);
v3->setFixed(true);
@@ -419,7 +431,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
if(id == rootId)
if(id == rootId && !hasGravityConstraints)
{
UDEBUG("Set %d fixed", id);
v3->setFixed(true);
@@ -434,8 +446,41 @@ std::map<int, Transform> OptimizerG2O::optimize(
continue;
}
}
vertex->setId(id);
UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
if(vertex == 0)
{
UERROR("Could not create vertex for node %d", id);
}
else
{
vertex->setId(id);
UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
if(!isSlam2d() && id == rootId && hasGravityConstraints)
{
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)vertex;
priorEdge->setVertex(0, v1);
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
// pitch and roll not fixed
information(3,3) = information(4,4) = 1;
priorEdge->setInformation(information);
if (priorEdge && !optimizer.addEdge(priorEdge))
{
delete priorEdge;
UERROR("Map: Failed adding fixed constraint of rootid %d, set as fixed instead", id);
v1->setFixed(true);
}
else
{
UDEBUG("Set %d fixed with prior (have gravity constraints)", id);
}
}
}
}
UDEBUG("fill edges to g2o...");

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

@@ -107,22 +107,34 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
// detect if there is a global pose prior set, if so remove rootId
bool hasGPSPrior = false;
if(!priorsIgnored())
bool hasGravityConstraints = false;
if(!priorsIgnored() || (!isSlam2d() && gravitySigma() > 0))
{
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior)
if(iter->second.from() == iter->second.to())
{
hasGPSPrior = true;
if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
(1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0))
if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
{
// orientation is set, don't set root prior (it is no GPS)
rootId = 0;
hasGPSPrior = false;
break;
hasGPSPrior = true;
if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
(1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0))
{
// orientation is set, don't set root prior (it is no GPS)
rootId = 0;
hasGPSPrior = false;
break;
}
}
if(iter->second.type() == Link::kGravity)
{
hasGravityConstraints = true;
if(priorsIgnored())
{
break;
}
}
}
}
@@ -143,7 +155,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
(gtsam::Vector(6) <<
1e-2, 1e-2, hasGPSPrior?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), hasGPSPrior?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
).finished());
graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));