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

Marker priors (#859)

Browse Source 
* Added MarkerPriors parameter

* Fixed Marker/Priors format to use '|' instead ';'. Fixed landmark priors not used.

* Marker: added priors variance parameters

* g2o: refactored backward compatibility includes

* fixed build with old g2o
This commit is contained in:
matlabbe
2022-04-28 09:18:30 -04:00
committed by GitHub
parent 190071678f
commit b646c5e1db
14 changed files with 731 additions and 283 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -59,9 +59,11 @@ 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"
#include "g2o/edge_se3_xyzprior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
#include "g2o/edge_se3_gravity.h"
#include "g2o/edge_sbacam_gravity.h"
#include "g2o/edge_xy_prior.h" // Include after types_slam2d.h to be ignored on newest g2o versions
#include "g2o/edge_xyz_prior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
#ifdef G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif
@@ -531,11 +533,37 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(id1 == id2)
{
if(iter->second.type() == Link::kPosePrior && !priorsIgnored())
if(iter->second.type() == Link::kPosePrior && !priorsIgnored() &&
(!landmarksIgnored() || id1>0))
{
int idTag= id1;
if(id1<0)
{
// landmark prior, offset ids
id1 = landmarkVertexOffset - id1;
id2 = landmarkVertexOffset - id2;
}
if(isSlam2d())
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
if(idTag < 0 && !isLandmarkWithRotation.at(idTag))
{
g2o::EdgeXYPrior * priorEdge = new g2o::EdgeXYPrior();
g2o::VertexPointXY* v1 = (g2o::VertexPointXY*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(Eigen::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 if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
@@ -578,12 +606,36 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
else
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
if(idTag < 0 && !isLandmarkWithRotation.at(idTag))
{
//XYZ case
g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior();
g2o::VertexPointXYZ* v1 = (g2o::VertexPointXYZ*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(Eigen::Vector3d(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 (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)
{
//GPS XYZ case
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
g2o::EdgeSE3XYZPrior * priorEdge = new g2o::EdgeSE3XYZPrior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(Eigen::Vector3d(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));

30
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -51,8 +51,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include "gtsam/GravityFactor.h"
#include "gtsam/GPSPose2XYFactor.h"
#include "gtsam/GPSPose3XYZFactor.h"
#include <optimizer/gtsam/XYFactor.h>
#include <optimizer/gtsam/XYZFactor.h>
#ifdef RTABMAP_VERTIGO
#include "vertigo/gtsam/betweenFactorSwitchable.h"
@@ -233,16 +233,24 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UASSERT(!iter->second.transform().isNull());
if(id1 == id2)
{
if(iter->second.type() == Link::kPosePrior && !priorsIgnored())
if(iter->second.type() == Link::kPosePrior && !priorsIgnored() &&
(!landmarksIgnored() || id1>0))
{
if(isSlam2d())
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(GPSPose2XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
}
else
{
@@ -266,7 +274,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
graph.add(XYZFactor<gtsam::Point3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
}
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)
{
@@ -274,7 +290,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
graph.add(GPSPose3XYZFactor(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
graph.add(XYZFactor<gtsam::Pose3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
}
else
{

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

@@ -1,107 +0,0 @@
// 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"
namespace rtabmap {
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>()
{
information().setIdentity();
setMeasurement(Eigen::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
Eigen::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;
}
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");
Eigen::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);
}
}

102
corelib/src/optimizer/g2o/edge_se3_xyzprior.h
View File

@@ -24,36 +24,38 @@
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef RTAB_G2O_EDGE_SE3_PRIOR_XYZ_H
#define RTAB_G2O_EDGE_SE3_PRIOR_XYZ_H
#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"
namespace rtabmap {
namespace g2o {
using namespace Eigen;
/**
* \brief Prior for a 3D pose with constraints only in xyz direction
*/
class EdgeSE3XYZPrior : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>
class EdgeSE3XYZPrior : public BaseUnaryEdge<3, Vector3d, VertexSE3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3XYZPrior();
virtual void setMeasurement(const Eigen::Vector3d& m) {
virtual void setMeasurement(const Vector3d& m) {
_measurement = m;
}
virtual bool setMeasurementData(const double * d) {
Eigen::Map<const Eigen::Vector3d> v(d);
Map<const Vector3d> v(d);
_measurement = v;
return true;
}
virtual bool getMeasurementData(double* d) const {
Eigen::Map<Eigen::Vector3d> v(d);
Map<Vector3d> v(d);
v = _measurement;
return true;
}
@@ -65,17 +67,93 @@ public:
virtual void computeError();
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_*/);
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, OptimizableGraph::Vertex* /*to*/) {return 1.;}
virtual void initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/);
const g2o::ParameterSE3Offset* offsetParameter() { return _offsetParam; }
const ParameterSE3Offset* offsetParameter() { return _offsetParam; }
protected:
virtual bool resolveCaches();
g2o::ParameterSE3Offset* _offsetParam;
g2o::CacheSE3Offset* _cache;
ParameterSE3Offset* _offsetParam;
CacheSE3Offset* _cache;
};
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, Vector3d, VertexSE3>()
{
information().setIdentity();
setMeasurement(Vector3d::Zero());
_cache = 0;
_offsetParam = 0;
resizeParameters(1);
installParameter(_offsetParam, 0);
}
bool EdgeSE3XYZPrior::resolveCaches(){
assert(_offsetParam);
ParameterVector pv(1);
pv[0] = _offsetParam;
resolveCache(_cache, (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
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 VertexSE3* v = static_cast<const VertexSE3*>(_vertices[0]);
_error = v->estimate().translation() - _measurement;
}
bool EdgeSE3XYZPrior::setMeasurementFromState() {
const VertexSE3* v = static_cast<const VertexSE3*>(_vertices[0]);
_measurement = v->estimate().translation();
return true;
}
void EdgeSE3XYZPrior::initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *v = static_cast<VertexSE3*>(_vertices[0]);
assert(v && "Vertex for the Prior edge is not set");
Isometry3d newEstimate = _offsetParam->offset().inverse() * 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);
}
}
#endif

127
corelib/src/optimizer/g2o/edge_xy_prior.h Normal file
View File

@@ -0,0 +1,127 @@
// 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.
/**
* rtabmap: To be used with older g2o version not having this file
*/
#ifndef G2O_EDGE_XY_PRIOR_H
#define G2O_EDGE_XY_PRIOR_H
#include "g2o/types/slam2d/vertex_point_xy.h"
#include "g2o/config.h"
#include "g2o/core/base_unary_edge.h"
namespace g2o {
using namespace Eigen;
class EdgeXYPrior : public BaseUnaryEdge<2, Vector2d, VertexPointXY>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeXYPrior();
void computeError()
{
const VertexPointXY* v = static_cast<const VertexPointXY*>(_vertices[0]);
_error = v->estimate()-_measurement;
}
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void setMeasurement(const Vector2d& m){
_measurement = m;
}
virtual bool setMeasurementData(const double* d){
_measurement=Vector2d(d[0], d[1]);
return true;
}
virtual bool getMeasurementData(double* d) const {
Eigen::Map<Vector2d> m(d);
m=_measurement;
return true;
}
virtual int measurementDimension() const {return 2;}
virtual bool setMeasurementFromState() {
const VertexPointXY* v = static_cast<const VertexPointXY*>(_vertices[0]);
_measurement = v->estimate();
return true;
}
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 0.;}
#ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
virtual void linearizeOplus();
#endif
};
EdgeXYPrior::EdgeXYPrior() :
BaseUnaryEdge<2, Vector2d, VertexPointXY>()
{
_information.setIdentity();
_error.setZero();
}
bool EdgeXYPrior::read(std::istream& is)
{
Vector2d p;
is >> p[0] >> p[1];
setMeasurement(p);
for (int i = 0; i < 2; ++i)
for (int j = i; j < 2; ++j) {
is >> information()(i, j);
if (i != j)
information()(j, i) = information()(i, j);
}
return true;
}
bool EdgeXYPrior::write(std::ostream& os) const
{
Vector2d p = measurement();
os << p.x() << " " << p.y();
for (int i = 0; i < 2; ++i)
for (int j = i; j < 2; ++j)
os << " " << information()(i, j);
return os.good();
}
#ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
void EdgeXYPrior::linearizeOplus()
{
_jacobianOplusXi=Matrix2d::Identity();
}
#endif
} // end namespace
#endif

131
corelib/src/optimizer/g2o/edge_xyz_prior.h Normal file
View File

@@ -0,0 +1,131 @@
// 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.
/**
* rtabmap: To be used with older g2o version not having this file
*/
#ifndef G2O_EDGE_XYZ_PRIOR_H_
#define G2O_EDGE_XYZ_PRIOR_H_
#include "g2o/core/base_unary_edge.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
namespace g2o {
using namespace Eigen;
/**
* \brief prior for an XYZ vertex (VertexPointXYZ)
*
* Provides a prior for a 3d point vertex. The measurement is represented by a
* Vector3d with a corresponding 3x3 upper triangle covariance matrix (upper triangle only).
*/
class EdgeXYZPrior : public BaseUnaryEdge<3, Vector3d, VertexPointXYZ> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeXYZPrior();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError();
// jacobian
virtual void linearizeOplus();
virtual void setMeasurement(const Vector3d& m){
_measurement = m;
}
virtual bool setMeasurementData(const double* d){
Eigen::Map<const Vector3d> v(d);
_measurement = v;
return true;
}
virtual bool getMeasurementData(double* d) const{
Eigen::Map<Vector3d> v(d);
v = _measurement;
return true;
}
virtual int measurementDimension() const { return 3; }
virtual bool setMeasurementFromState() ;
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/,
OptimizableGraph::Vertex* /*to*/) {
return 0;
}
};
EdgeXYZPrior::EdgeXYZPrior() : BaseUnaryEdge<3, Vector3d, VertexPointXYZ>() {
information().setIdentity();
}
bool EdgeXYZPrior::read(std::istream& is) {
// read measurement
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<information().rows(); i++)
for (int j=i; j<information().cols(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
}
return !is.fail();
}
bool EdgeXYZPrior::write(std::ostream& os) const {
for (int i = 0; i<3; i++) os << measurement()[i] << " ";
for (int i=0; i<information().rows(); i++)
for (int j=i; j<information().cols(); j++) {
os << information()(i,j) << " ";
}
return os.good();
}
void EdgeXYZPrior::computeError() {
const VertexPointXYZ* v = static_cast<const VertexPointXYZ*>(_vertices[0]);
_error = v->estimate() - _measurement;
}
void EdgeXYZPrior::linearizeOplus(){
_jacobianOplusXi = Matrix3d::Identity();
}
bool EdgeXYZPrior::setMeasurementFromState(){
const VertexPointXYZ* v = static_cast<const VertexPointXYZ*>(_vertices[0]);
_measurement = v->estimate();
return true;
}
}
#endif

9
corelib/src/optimizer/gtsam/GPSPose2XYFactor.h → corelib/src/optimizer/gtsam/XYFactor.h
View File

@@ -20,7 +20,8 @@
namespace rtabmap {
class GPSPose2XYFactor: public gtsam::NoiseModelFactor1<gtsam::Pose2> {
template<class VALUE>
class XYFactor: public gtsam::NoiseModelFactor1<VALUE> {
private:
// measurement information
@@ -34,13 +35,13 @@ public:
* @param model noise model for GPS snesor, in X-Y
* @param m Point2 measurement
*/
GPSPose2XYFactor(gtsam::Key poseKey, const gtsam::Point2 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<gtsam::Pose2>(model, poseKey), mx_(m.x()), my_(m.y()) {}
XYFactor(gtsam::Key poseKey, const gtsam::Point2 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<VALUE>(model, poseKey), mx_(m.x()), my_(m.y()) {}
// error function
// @param p the pose in Pose2
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose2& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
gtsam::Vector evaluateError(const VALUE& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists

10
corelib/src/optimizer/gtsam/GPSPose3XYZFactor.h → corelib/src/optimizer/gtsam/XYZFactor.h
View File

@@ -20,7 +20,8 @@
namespace rtabmap {
class GPSPose3XYZFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
template<class VALUE>
class XYZFactor: public gtsam::NoiseModelFactor1<VALUE> {
private:
// measurement information
@@ -34,8 +35,8 @@ public:
* @param model noise model for GPS sensor, in X-Y
* @param m Point2 measurement
*/
GPSPose3XYZFactor(gtsam::Key poseKey, const gtsam::Point3 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<gtsam::Pose3>(model, poseKey), mx_(m.x()), my_(m.y()), mz_(m.z()) {}
XYZFactor(gtsam::Key poseKey, const gtsam::Point3 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<VALUE>(model, poseKey), mx_(m.x()), my_(m.y()), mz_(m.z()) {}
// error function
// @param p the pose in Pose
@@ -47,6 +48,9 @@ public:
}
return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
}
gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
}
};
} // namespace gtsamexamples