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

OptimizerG2O: add XYZ/SE3 and XY/SE2 priors depending on the angular covariances

Browse Source
This commit is contained in:
TSC21
2018-12-26 16:30:40 +00:00
parent 97f956f138
commit b7f2eb8df9
1 changed files with 85 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -396,6 +396,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(!priorsIgnored()) if(!priorsIgnored())
{ {
if(isSlam2d()) if(isSlam2d())
{
if (iter->second.infMatrix().at<double>(3,3) <= 9999 && iter->second.infMatrix().at<double>(4,4) <= 9999 && iter->second.infMatrix().at<double>(5,5) <= 9999)
{ {
g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior(); g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
@@ -419,6 +421,27 @@ std::map<int, Transform> OptimizerG2O::optimize(
edge = priorEdge; edge = priorEdge;
} }
else else
{
g2o::EdgeXYPrior * priorEdge = new g2o::EdgeXYPrior();
g2o::VertexPointXY* v1 = (g2o::VertexPointXY*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector2(iter->second.transform().x(), iter->second.transform().y()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
}
priorEdge->setInformation(information);
edge = priorEdge;
}
}
else
{
if (iter->second.infMatrix().at<double>(3,3) <= 9999 && iter->second.infMatrix().at<double>(4,4) <= 9999 && iter->second.infMatrix().at<double>(5,5) <= 9999)
{ {
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior(); g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
@@ -437,6 +460,30 @@ std::map<int, Transform> OptimizerG2O::optimize(
priorEdge->setInformation(information); priorEdge->setInformation(information);
edge = priorEdge; edge = priorEdge;
} }
else
{
g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior();
g2o::VertexPointXYZ* v1 = (g2o::VertexPointXYZ*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,2); // x-z
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,2); // y-z
information(2,0) = iter->second.infMatrix().at<double>(2,0); // z-x
information(2,1) = iter->second.infMatrix().at<double>(2,1); // z-y
information(2,2) = iter->second.infMatrix().at<double>(2,2); // z-z
}
priorEdge->setInformation(information);
edge = priorEdge;
}
}
} }
} }
else if(id1<0 || id2 < 0) else if(id1<0 || id2 < 0)