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

OptimizerG2O: fix slam2d prior types naming

Browse Source
This commit is contained in:
TSC21
2018-12-28 13:38:12 +00:00
parent c21f466573
commit be75c7591c
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -400,8 +400,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0 || if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0 ||
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0) static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)
{ {
g2o::EdgeXYPrior * priorEdge = new g2o::EdgeXYPrior(); g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
g2o::VertexPointXY* v1 = (g2o::VertexPointXY*)optimizer.vertex(id1); g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1); priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector2(iter->second.transform().x(), iter->second.transform().y())); priorEdge->setMeasurement(g2o::Vector2(iter->second.transform().x(), iter->second.transform().y()));
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity(); Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
@@ -1754,12 +1754,12 @@ bool OptimizerG2O::saveGraph(
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0 || if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0 ||
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0) static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)
{ {
prefix = "EDGE_POINTXY_PRIOR"; prefix = "EDGE_PRIOR_SE2_XY";
isSE2 = false; isSE2 = false;
} }
else else
{ {
prefix = "EDGE_SE2_PRIOR"; prefix = "EDGE_PRIOR_SE2";
} }
// based on https://github.com/RainerKuemmerle/g2o/blob/38347944c6ad7a3b31976b97406ff0de20be1530/g2o/types/slam2d/edge_se2_prior.cpp#L42 // based on https://github.com/RainerKuemmerle/g2o/blob/38347944c6ad7a3b31976b97406ff0de20be1530/g2o/types/slam2d/edge_se2_prior.cpp#L42