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: fix EDGE_XYZ / XY_PRIOR exports

Browse Source
This commit is contained in:
TSC21
2018-12-27 17:08:54 +00:00
parent 308b1484e0
commit c21f466573
1 changed files with 8 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -404,7 +404,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::VertexPointXY* v1 = (g2o::VertexPointXY*)optimizer.vertex(id1); g2o::VertexPointXY* v1 = (g2o::VertexPointXY*)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()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity(); Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored()) if(!isCovarianceIgnored())
{ {
@@ -453,7 +452,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::VertexPointXYZ* v1 = (g2o::VertexPointXYZ*)optimizer.vertex(id1); g2o::VertexPointXYZ* v1 = (g2o::VertexPointXYZ*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1); priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z())); 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(); Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored()) if(!isCovarianceIgnored())
{ {
@@ -1756,7 +1754,7 @@ 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_XY_PRIOR"; prefix = "EDGE_POINTXY_PRIOR";
isSE2 = false; isSE2 = false;
} }
else else
@@ -1777,15 +1775,15 @@ bool OptimizerG2O::saveGraph(
static_cast<double>(iter->second.infMatrix().at<double>(4,4)) == 0.0 && static_cast<double>(iter->second.infMatrix().at<double>(4,4)) == 0.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_XYZ_PRIOR"; to = "";
prefix = "EDGE_POINTXYZ_PRIOR";
isSE3 = false; isSE3 = false;
} }
else else
{ {
to = uFormat(" %d", PARAM_OFFSET);
prefix = "EDGE_SE3_PRIOR"; prefix = "EDGE_SE3_PRIOR";
} }
to = uFormat(" %d", PARAM_OFFSET);
} }
} }
else if(this->isRobust() && else if(this->isRobust() &&
@@ -1803,7 +1801,7 @@ bool OptimizerG2O::saveGraph(
if (isSE2) if (isSE2)
{ {
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 // EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
// EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 // EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n", fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(), prefix.c_str(),
iter->second.from(), iter->second.from(),
@@ -1822,7 +1820,7 @@ bool OptimizerG2O::saveGraph(
else else
{ {
// EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22 // EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
// EDGE_XY_PRIOR observed_vertex_id x y inf_11 inf_12 inf_22 // EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22
fprintf(file, "%s %d%s%s %f %f %f %f %f\n", fprintf(file, "%s %d%s%s %f %f %f %f %f\n",
prefix.c_str(), prefix.c_str(),
iter->second.from(), iter->second.from(),
@@ -1839,7 +1837,7 @@ bool OptimizerG2O::saveGraph(
{ {
if (isSE3) { if (isSE3) {
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 // EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
// EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 // EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf(); Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(), prefix.c_str(),
@@ -1878,7 +1876,7 @@ bool OptimizerG2O::saveGraph(
else else
{ {
// EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33 // EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33
// EDGE_XYZ_PRIOR observed_vertex_id offset_parameter_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33 // EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n", fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(), prefix.c_str(),
iter->second.from(), iter->second.from(),