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

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