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

g2o/gps support: added edge_se3_xyzprior.h to rtabmap repo. Fixed build for kinetic g2o version.

Browse Source
This commit is contained in:
matlabbe
2019-01-03 15:49:25 -05:00
parent 248d6f1167
commit 0f8c70bcdf
8 changed files with 363 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -180,7 +180,7 @@ option(WITH_VISO2 "Include VISO2 support" ON)
option(WITH_DVO "Include DVO support" ON) option(WITH_DVO "Include DVO support" ON)
option(WITH_ORB_SLAM2 "Include ORB_SLAM2 support" ON) option(WITH_ORB_SLAM2 "Include ORB_SLAM2 support" ON)
option(WITH_OKVIS "Include OKVIS support" ON) option(WITH_OKVIS "Include OKVIS support" ON)
option(WITH_MSCKF_VIO "Include MSCKF_VIO support" OFF) option(WITH_MSCKF_VIO "Include MSCKF_VIO support" OFF)
option(PCL_OMP "With PCL OMP implementations" ON) option(PCL_OMP "With PCL OMP implementations" ON)
FIND_PACKAGE(OpenCV REQUIRED QUIET) FIND_PACKAGE(OpenCV REQUIRED QUIET)

1
corelib/src/CMakeLists.txt
View File

@@ -68,6 +68,7 @@ SET(SRC_FILES
optimizer/OptimizerG2O.cpp optimizer/OptimizerG2O.cpp
optimizer/OptimizerGTSAM.cpp optimizer/OptimizerGTSAM.cpp
optimizer/OptimizerCVSBA.cpp optimizer/OptimizerCVSBA.cpp
optimizer/g2o/edge_se3_xyzprior.cpp
Registration.cpp Registration.cpp
RegistrationIcp.cpp RegistrationIcp.cpp

7
corelib/src/Memory.cpp
View File

@@ -4989,12 +4989,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
} }
cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords()); cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI); Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)*0.00000001; cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999
if(data.gps().error() > 0.0) if(data.gps().error() > 0.0)
{ {
// only set x, y as we don't know variance for other degrees of freedom. // only set x, y and z as we don't know variance for other degrees of freedom.
gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 0.1; gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = gpsInfMatrix.at<double>(2,2) = 1.0/data.gps().error();
gpsInfMatrix.at<double>(2,2) = 10000;
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix)); s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
} }
else else

25
corelib/src/Rtabmap.cpp
View File

@@ -464,6 +464,13 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
} }
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd); Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError); Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
if(_optimizationMaxError > 0.0 && _optimizationMaxError < 1.0)
{
UWARN("RGBD/OptimizeMaxError (value=%f) is smaller than 1.0, setting to default %f "
"instead (for backward compatibility issues when this parameter was previously "
"an absolute error value).", _optimizationMaxError, Parameters::defaultRGBDOptimizeMaxError());
_optimizationMaxError = Parameters::defaultRGBDOptimizeMaxError();
}
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius); Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
@@ -2448,7 +2455,7 @@ bool Rtabmap::process(
(_loopClosureHypothesis.first>0 || (_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() || statistics_.reducedIds().size() ||
signature->hasLink(signature->id()) || // prior edge (signature->hasLink(signature->id()) && !_graphOptimizer->priorsIgnored()) || // prior edge
proximityDetectionsInTimeFound>0 || proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 || landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kPosePrior).size()) && // In localization mode, the new node should be linked ((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kPosePrior).size()) && // In localization mode, the new node should be linked
@@ -2648,7 +2655,21 @@ bool Rtabmap::process(
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update _lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
{ {
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); bool hasPrior = signature->hasLink(signature->id());
if(!_graphOptimizer->priorsIgnored())
{
for(std::multimap<int, Link>::reverse_iterator iter=_constraints.rbegin(); !hasPrior && iter!=_constraints.rend(); ++iter)
{
if(iter->second.type() == Link::kPosePrior)
{
hasPrior = true;
}
}
}
if(!hasPrior || _graphOptimizer->priorsIgnored())
{
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
}
} }
} }
_lastLocalizationNodeId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:_lastLocalizationNodeId; _lastLocalizationNodeId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:_lastLocalizationNodeId;

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

@@ -59,6 +59,7 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
#include "g2o/config.h" #include "g2o/config.h"
#include "g2o/types/slam2d/types_slam2d.h" #include "g2o/types/slam2d/types_slam2d.h"
#include "g2o/types/slam3d/types_slam3d.h" #include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/edge_se3_xyzprior.h"
#ifdef G2O_HAVE_CSPARSE #ifdef G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h" #include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif #endif
@@ -397,96 +398,96 @@ std::map<int, Transform> OptimizerG2O::optimize(
{ {
if(isSlam2d()) if(isSlam2d())
{ {
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::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior(); g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)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::Vector2D(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();
if(!isCovarianceIgnored()) if(!isCovarianceIgnored())
{ {
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x 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,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x 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,1) = iter->second.infMatrix().at<double>(1,1); // y-y
} }
priorEdge->setInformation(information); priorEdge->setInformation(information);
edge = priorEdge; edge = priorEdge;
} }
else else
{ {
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);
priorEdge->setVertex(0, v1); priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
priorEdge->setParameterId(0, PARAM_OFFSET); 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())
{ {
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x 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,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta 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,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y 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(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,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
} }
priorEdge->setInformation(information); priorEdge->setInformation(information);
edge = priorEdge; edge = priorEdge;
} }
} }
else else
{ {
if ((1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) > 9999.0 && 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>(4,4)) >= 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 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>(3,3)) == 0.0 &&
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))
{ {
g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior(); EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); g2o::VertexSE3* v1 = (g2o::VertexSE3*)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::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(); Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored()) if(!isCovarianceIgnored())
{ {
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x 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,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,2); // x-z 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,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y 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(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,0) = iter->second.infMatrix().at<double>(2,0); // z-x
information(2,1) = iter->second.infMatrix().at<double>(2,1); // z-y information(2,1) = iter->second.infMatrix().at<double>(2,1); // z-y
information(2,2) = iter->second.infMatrix().at<double>(2,2); // z-z information(2,2) = iter->second.infMatrix().at<double>(2,2); // z-z
} }
priorEdge->setInformation(information); priorEdge->setInformation(information);
edge = priorEdge; edge = priorEdge;
} }
else else
{ {
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);
priorEdge->setVertex(0, v1); priorEdge->setVertex(0, v1);
Eigen::Affine3d a = iter->second.transform().toEigen3d(); Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d pose; Eigen::Isometry3d pose;
pose = a.linear(); pose = a.linear();
pose.translation() = a.translation(); pose.translation() = a.translation();
priorEdge->setMeasurement(pose); priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET); priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity(); Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored()) if(!isCovarianceIgnored())
{ {
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
} }
priorEdge->setInformation(information); priorEdge->setInformation(information);
edge = priorEdge; edge = priorEdge;
} }
} }
} }
} }
@@ -1740,8 +1741,8 @@ bool OptimizerG2O::saveGraph(
std::string suffix = ""; std::string suffix = "";
std::string to = uFormat(" %d", iter->second.to()); std::string to = uFormat(" %d", iter->second.to());
bool isSE2 = true; bool isSE2 = true;
bool isSE3 = true; bool isSE3 = true;
if (iter->second.type() == Link::kPosePrior) if (iter->second.type() == Link::kPosePrior)
{ {
@@ -1749,42 +1750,42 @@ bool OptimizerG2O::saveGraph(
{ {
continue; continue;
} }
if (isSlam2d()) if (isSlam2d())
{ {
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_PRIOR_SE2_XY"; prefix = "EDGE_PRIOR_SE2_XY";
isSE2 = false; isSE2 = false;
} }
else else
{ {
prefix = "EDGE_PRIOR_SE2"; 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 // there is no pid for the 2d prior case
to = ""; to = "";
} }
else else
{ {
if ((1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) > 9999.0 && 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>(4,4)) > 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 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>(3,3)) == 0.0 &&
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))
{ {
to = ""; to = "";
prefix = "EDGE_POINTXYZ_PRIOR"; prefix = "EDGE_POINTXYZ_PRIOR";
isSE3 = false; isSE3 = false;
} }
else else
{ {
to = uFormat(" %d", PARAM_OFFSET); to = uFormat(" %d", PARAM_OFFSET);
prefix = "EDGE_SE3_PRIOR"; prefix = "EDGE_SE3_PRIOR";
} }
} }
} }
else if(this->isRobust() && else if(this->isRobust() &&
iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kNeighbor &&
@@ -1798,11 +1799,11 @@ bool OptimizerG2O::saveGraph(
if(isSlam2d()) if(isSlam2d())
{ {
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(),
to.c_str(), to.c_str(),
@@ -1816,12 +1817,12 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(1, 1), iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 5), iter->second.infMatrix().at<double>(1, 5),
iter->second.infMatrix().at<double>(5, 5)); iter->second.infMatrix().at<double>(5, 5));
} }
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_POINTXY_PRIOR 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(),
to.c_str(), to.c_str(),
@@ -1831,15 +1832,16 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 0), iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1), iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1)); iter->second.infMatrix().at<double>(1, 1));
} }
} }
else else
{ {
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_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 observed_vertex_id observing_vertex_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(); // 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
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", 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(), prefix.c_str(),
iter->second.from(), iter->second.from(),
to.c_str(), to.c_str(),
@@ -1872,12 +1874,12 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(4, 4), iter->second.infMatrix().at<double>(4, 4),
iter->second.infMatrix().at<double>(4, 5), iter->second.infMatrix().at<double>(4, 5),
iter->second.infMatrix().at<double>(5, 5)); iter->second.infMatrix().at<double>(5, 5));
} }
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_POINTXYZ_PRIOR observed_vertex_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(),
to.c_str(), to.c_str(),
@@ -1891,7 +1893,7 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(1, 1), iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2), iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2)); iter->second.infMatrix().at<double>(2, 2));
} }
} }
} }
UINFO("Graph saved to %s", fileName.c_str()); UINFO("Graph saved to %s", fileName.c_str());

107
corelib/src/optimizer/g2o/edge_se3_xyzprior.cpp Normal file
View File

@@ -0,0 +1,107 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "edge_se3_xyzprior.h"
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3>()
{
information().setIdentity();
setMeasurement(g2o::Vector3D::Zero());
_cache = 0;
_offsetParam = 0;
resizeParameters(1);
installParameter(_offsetParam, 0);
}
bool EdgeSE3XYZPrior::resolveCaches(){
assert(_offsetParam);
g2o::ParameterVector pv(1);
pv[0] = _offsetParam;
resolveCache(_cache, (g2o::OptimizableGraph::Vertex*)_vertices[0], "CACHE_SE3_OFFSET", pv);
return _cache != 0;
}
bool EdgeSE3XYZPrior::read(std::istream& is)
{
int pid;
is >> pid;
if (!setParameterId(0, pid))
return false;
// measured keypoint
g2o::Vector3D meas;
for (int i = 0; i < 3; i++) is >> meas[i];
setMeasurement(meas);
// read covariance matrix (upper triangle)
if (is.good()) {
for (int i = 0; i < 3; i++) {
for (int j = i; j < 3; j++) {
is >> information()(i,j);
if (i != j)
information()(j,i) = information()(i,j);
}
}
}
return !is.fail();
}
bool EdgeSE3XYZPrior::write(std::ostream& os) const {
os << _offsetParam->id() << " ";
for (int i = 0; i < 3; i++) os << measurement()[i] << " ";
for (int i = 0; i < 3; i++) {
for (int j = i; j < 3; j++) {
os << information()(i,j) << " ";
}
}
return os.good();
}
void EdgeSE3XYZPrior::computeError() {
const g2o::VertexSE3* v = static_cast<const g2o::VertexSE3*>(_vertices[0]);
_error = v->estimate().translation() - _measurement;
}
void EdgeSE3XYZPrior::linearizeOplus() {
_jacobianOplusXi << g2o::Matrix3D::Identity();
}
bool EdgeSE3XYZPrior::setMeasurementFromState() {
const g2o::VertexSE3* v = static_cast<const g2o::VertexSE3*>(_vertices[0]);
_measurement = v->estimate().translation();
return true;
}
void EdgeSE3XYZPrior::initialEstimate(const g2o::OptimizableGraph::VertexSet& /*from_*/, g2o::OptimizableGraph::Vertex* /*to_*/) {
g2o::VertexSE3 *v = static_cast<g2o::VertexSE3*>(_vertices[0]);
assert(v && "Vertex for the Prior edge is not set");
g2o::Isometry3D newEstimate = _offsetParam->offset().inverse() * Eigen::Translation3d(measurement());
if (_information.block<3,3>(0,0).array().abs().sum() == 0){ // do not set translation, as that part of the information is all zero
newEstimate.translation() = v->estimate().translation();
}
v->setEstimate(newEstimate);
}

78
corelib/src/optimizer/g2o/edge_se3_xyzprior.h Normal file
View File

@@ -0,0 +1,78 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef G2O_EDGE_SE3_PRIOR_XYZ_H
#define G2O_EDGE_SE3_PRIOR_XYZ_H
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/types/slam3d/parameter_se3_offset.h"
/**
* \brief Prior for a 3D pose with constraints only in xyz direction
*/
class EdgeSE3XYZPrior : public g2o::BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3XYZPrior();
virtual void setMeasurement(const g2o::Vector3D& m) {
_measurement = m;
}
virtual bool setMeasurementData(const double * d) {
Eigen::Map<const g2o::Vector3D> v(d);
_measurement = v;
return true;
}
virtual bool getMeasurementData(double* d) const {
Eigen::Map<g2o::Vector3D> v(d);
v = _measurement;
return true;
}
virtual int measurementDimension() const {return 3;}
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();
virtual bool setMeasurementFromState();
virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& /*from*/, g2o::OptimizableGraph::Vertex* /*to*/) {return 1.;}
virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& /*from_*/, g2o::OptimizableGraph::Vertex* /*to_*/);
const g2o::ParameterSE3Offset* offsetParameter() { return _offsetParam; }
protected:
virtual bool resolveCaches();
g2o::ParameterSE3Offset* _offsetParam;
g2o::CacheSE3Offset* _cache;
};
#endif

10
guilib/src/MainWindow.cpp
View File

@@ -1904,14 +1904,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{ {
uInsert(labels, std::pair<int, std::string>(*iter)); // overwrite labels because they could have been modified uInsert(labels, std::pair<int, std::string>(*iter)); // overwrite labels because they could have been modified
} }
if(_ui->graphicsView_graphView->getWorldMapRotation()==0.0f &&
stat.getLastSignatureData().sensorData().gps().stamp()!=0.0 &&
stat.poses().find(stat.getLastSignatureData().id())!=stat.poses().end())
{
float bearing = (float)((-(stat.getLastSignatureData().sensorData().gps().bearing()-90))*M_PI/180.0);
float gpsRotationOffset = stat.poses().at(stat.getLastSignatureData().id()).theta()-bearing;
_ui->graphicsView_graphView->setWorldMapRotation(gpsRotationOffset);
}
std::map<int, Transform> poses = stat.poses(); std::map<int, Transform> poses = stat.poses();
@@ -2402,7 +2394,7 @@ void MainWindow::updateMapCloud(
} }
} }
if(id != 0 && poses.find(id) == poses.end()) if(poses.find(id) == poses.end())
{ {
if(_cloudViewer->getCloudVisibility(iter.key())) if(_cloudViewer->getCloudVisibility(iter.key()))
{ {