mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-10 03:49:50 +08:00
g2o/gps support: added edge_se3_xyzprior.h to rtabmap repo. Fixed build for kinetic g2o version.
This commit is contained in:
matlabbe
@@ -59,6 +59,7 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
|
||||
#include "g2o/config.h"
|
||||
#include "g2o/types/slam2d/types_slam2d.h"
|
||||
#include "g2o/types/slam3d/types_slam3d.h"
|
||||
#include "g2o/edge_se3_xyzprior.h"
|
||||
#ifdef G2O_HAVE_CSPARSE
|
||||
#include "g2o/solvers/csparse/linear_solver_csparse.h"
|
||||
#endif
|
||||
@@ -397,96 +398,96 @@ std::map<int, Transform> OptimizerG2O::optimize(
|
||||
{
|
||||
if(isSlam2d())
|
||||
{
|
||||
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)
|
||||
{
|
||||
g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
|
||||
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
|
||||
priorEdge->setVertex(0, v1);
|
||||
priorEdge->setMeasurement(g2o::Vector2(iter->second.transform().x(), iter->second.transform().y()));
|
||||
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
|
||||
{
|
||||
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()));
|
||||
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,5); // x-theta
|
||||
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,5); // y-theta
|
||||
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
|
||||
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
|
||||
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
|
||||
}
|
||||
priorEdge->setInformation(information);
|
||||
edge = priorEdge;
|
||||
}
|
||||
}
|
||||
else
|
||||
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)
|
||||
{
|
||||
g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
|
||||
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
|
||||
priorEdge->setVertex(0, v1);
|
||||
priorEdge->setMeasurement(g2o::Vector2D(iter->second.transform().x(), iter->second.transform().y()));
|
||||
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
|
||||
{
|
||||
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()));
|
||||
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,5); // x-theta
|
||||
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,5); // y-theta
|
||||
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
|
||||
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
|
||||
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
|
||||
}
|
||||
priorEdge->setInformation(information);
|
||||
edge = priorEdge;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((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) ||
|
||||
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 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))
|
||||
{
|
||||
g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior();
|
||||
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
|
||||
priorEdge->setVertex(0, v1);
|
||||
priorEdge->setMeasurement(g2o::Vector3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
|
||||
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
|
||||
{
|
||||
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
|
||||
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
|
||||
priorEdge->setVertex(0, v1);
|
||||
Eigen::Affine3d a = iter->second.transform().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();
|
||||
if(!isCovarianceIgnored())
|
||||
{
|
||||
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
|
||||
}
|
||||
priorEdge->setInformation(information);
|
||||
edge = priorEdge;
|
||||
}
|
||||
if ((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) ||
|
||||
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 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))
|
||||
{
|
||||
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
|
||||
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
|
||||
priorEdge->setVertex(0, v1);
|
||||
priorEdge->setMeasurement(g2o::Vector3D(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
|
||||
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
|
||||
{
|
||||
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
|
||||
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
|
||||
priorEdge->setVertex(0, v1);
|
||||
Eigen::Affine3d a = iter->second.transform().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();
|
||||
if(!isCovarianceIgnored())
|
||||
{
|
||||
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
|
||||
}
|
||||
priorEdge->setInformation(information);
|
||||
edge = priorEdge;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1740,8 +1741,8 @@ bool OptimizerG2O::saveGraph(
|
||||
std::string suffix = "";
|
||||
std::string to = uFormat(" %d", iter->second.to());
|
||||
|
||||
bool isSE2 = true;
|
||||
bool isSE3 = true;
|
||||
bool isSE2 = true;
|
||||
bool isSE3 = true;
|
||||
|
||||
if (iter->second.type() == Link::kPosePrior)
|
||||
{
|
||||
@@ -1749,42 +1750,42 @@ bool OptimizerG2O::saveGraph(
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if (isSlam2d())
|
||||
{
|
||||
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)
|
||||
{
|
||||
prefix = "EDGE_PRIOR_SE2_XY";
|
||||
isSE2 = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
prefix = "EDGE_PRIOR_SE2";
|
||||
}
|
||||
if (isSlam2d())
|
||||
{
|
||||
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)
|
||||
{
|
||||
prefix = "EDGE_PRIOR_SE2_XY";
|
||||
isSE2 = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
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
|
||||
// there is no pid for the 2d prior case
|
||||
to = "";
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((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) ||
|
||||
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 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))
|
||||
{
|
||||
to = "";
|
||||
prefix = "EDGE_POINTXYZ_PRIOR";
|
||||
isSE3 = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
to = uFormat(" %d", PARAM_OFFSET);
|
||||
prefix = "EDGE_SE3_PRIOR";
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((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) ||
|
||||
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 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))
|
||||
{
|
||||
to = "";
|
||||
prefix = "EDGE_POINTXYZ_PRIOR";
|
||||
isSE3 = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
to = uFormat(" %d", PARAM_OFFSET);
|
||||
prefix = "EDGE_SE3_PRIOR";
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(this->isRobust() &&
|
||||
iter->second.type() != Link::kNeighbor &&
|
||||
@@ -1798,11 +1799,11 @@ bool OptimizerG2O::saveGraph(
|
||||
|
||||
if(isSlam2d())
|
||||
{
|
||||
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_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",
|
||||
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_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",
|
||||
prefix.c_str(),
|
||||
iter->second.from(),
|
||||
to.c_str(),
|
||||
@@ -1816,12 +1817,12 @@ bool OptimizerG2O::saveGraph(
|
||||
iter->second.infMatrix().at<double>(1, 1),
|
||||
iter->second.infMatrix().at<double>(1, 5),
|
||||
iter->second.infMatrix().at<double>(5, 5));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// EDGE_XY observed_vertex_id observing_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",
|
||||
{
|
||||
// EDGE_XY observed_vertex_id observing_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",
|
||||
prefix.c_str(),
|
||||
iter->second.from(),
|
||||
to.c_str(),
|
||||
@@ -1831,15 +1832,16 @@ bool OptimizerG2O::saveGraph(
|
||||
iter->second.infMatrix().at<double>(0, 0),
|
||||
iter->second.infMatrix().at<double>(0, 1),
|
||||
iter->second.infMatrix().at<double>(1, 1));
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
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_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();
|
||||
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",
|
||||
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_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();
|
||||
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(),
|
||||
iter->second.from(),
|
||||
to.c_str(),
|
||||
@@ -1872,12 +1874,12 @@ bool OptimizerG2O::saveGraph(
|
||||
iter->second.infMatrix().at<double>(4, 4),
|
||||
iter->second.infMatrix().at<double>(4, 5),
|
||||
iter->second.infMatrix().at<double>(5, 5));
|
||||
}
|
||||
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_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",
|
||||
}
|
||||
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_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",
|
||||
prefix.c_str(),
|
||||
iter->second.from(),
|
||||
to.c_str(),
|
||||
@@ -1891,7 +1893,7 @@ bool OptimizerG2O::saveGraph(
|
||||
iter->second.infMatrix().at<double>(1, 1),
|
||||
iter->second.infMatrix().at<double>(1, 2),
|
||||
iter->second.infMatrix().at<double>(2, 2));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
UINFO("Graph saved to %s", fileName.c_str());
|
||||
|
||||
Reference in New Issue
Block a user