mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 17:40:23 +08:00
Added Optimizer/GravitySigma parameter (with GTSAM support).
This commit is contained in:
matlabbe
@@ -443,7 +443,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
|
||||
|
||||
if(id1 == id2)
|
||||
{
|
||||
if(!priorsIgnored())
|
||||
if(iter->second.type() == Link::kPosePrior && !priorsIgnored())
|
||||
{
|
||||
if(isSlam2d())
|
||||
{
|
||||
|
||||
@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include "optimizer/gtsam/GravityFactor.h"
|
||||
|
||||
#ifdef RTABMAP_VERTIGO
|
||||
#include "vertigo/gtsam/betweenFactorMaxMix.h"
|
||||
@@ -202,7 +203,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
UASSERT(!iter->second.transform().isNull());
|
||||
if(id1 == id2)
|
||||
{
|
||||
if(!priorsIgnored())
|
||||
if(iter->second.type() == Link::kPosePrior && !priorsIgnored())
|
||||
{
|
||||
if(isSlam2d())
|
||||
{
|
||||
@@ -241,6 +242,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
|
||||
}
|
||||
}
|
||||
else if(gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
|
||||
{
|
||||
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
|
||||
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
|
||||
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10));
|
||||
graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
|
||||
}
|
||||
}
|
||||
else if(id1<0 || id2 < 0)
|
||||
{
|
||||
|
||||
84
corelib/src/optimizer/gtsam/GravityFactor.cpp
Normal file
84
corelib/src/optimizer/gtsam/GravityFactor.cpp
Normal file
@@ -0,0 +1,84 @@
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GravityFactor.cpp
|
||||
* @author Frank Dellaert
|
||||
* @brief Implementation file for Attitude factor
|
||||
* @date January 28, 2014
|
||||
**/
|
||||
|
||||
#include "GravityFactor.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
//***************************************************************************
|
||||
Vector GravityFactor::attitudeError(const Rot3& nRb,
|
||||
OptionalJacobian<2, 3> H) const {
|
||||
if (H) {
|
||||
Matrix23 D_nRef_R;
|
||||
Matrix22 D_e_nRef;
|
||||
Vector3 r = nRb.xyz();
|
||||
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
|
||||
Vector e = nZ_.error(nRef, D_e_nRef);
|
||||
(*H) = D_e_nRef * D_nRef_R;
|
||||
//printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n",
|
||||
// nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1),
|
||||
// (*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2));
|
||||
return e;
|
||||
} else {
|
||||
Vector3 r = nRb.xyz();
|
||||
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
|
||||
Vector e = nZ_.error(nRef);
|
||||
//printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1));
|
||||
return e;
|
||||
}
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void Rot3GravityFactor::print(const string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n";
|
||||
nZ_.print(" measured direction in nav frame: ");
|
||||
bRef_.print(" reference direction in body frame: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool Rot3GravityFactor::equals(const NonlinearFactor& expected,
|
||||
double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
|
||||
&& this->bRef_.equals(e->bRef_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void Pose3GravityFactor::print(const string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n";
|
||||
nZ_.print(" measured direction in nav frame: ");
|
||||
bRef_.print(" reference direction in body frame: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool Pose3GravityFactor::equals(const NonlinearFactor& expected,
|
||||
double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
|
||||
&& this->bRef_.equals(e->bRef_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
||||
}/// namespace gtsam
|
||||
238
corelib/src/optimizer/gtsam/GravityFactor.h
Normal file
238
corelib/src/optimizer/gtsam/GravityFactor.h
Normal file
@@ -0,0 +1,238 @@
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* Author: Mathieu Labbe
|
||||
* This file is a copy of AttitudeFactor.h of gtsam library but
|
||||
* with attitudeError() function overridden to ignore yaw errors.
|
||||
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file Pose3GravityFactor.h
|
||||
* @author Frank Dellaert
|
||||
* @brief Header file for Attitude factor
|
||||
* @date January 28, 2014
|
||||
**/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
/**
|
||||
* Base class for prior on gravity
|
||||
* Example:
|
||||
* - measurement is direction of gravity in navigation frame nG
|
||||
* - reference is direction of z axis in body frame bF
|
||||
* This factor will give zero error if nG is opposite direction of bF
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class GravityFactor {
|
||||
|
||||
protected:
|
||||
|
||||
const Unit3 nZ_, bRef_; ///< Position measurement in
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param nZ measured direction in navigation frame
|
||||
* @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1])
|
||||
*/
|
||||
GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
nZ_(nZ), bRef_(bRef) {
|
||||
}
|
||||
|
||||
/** vector of errors */
|
||||
Vector attitudeError(const Rot3& p,
|
||||
OptionalJacobian<2,3> H = boost::none) const;
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
|
||||
ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Version of GravityFactor for Rot3
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class GTSAM_EXPORT Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
|
||||
|
||||
typedef NoiseModelFactor1<Rot3> Base;
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
|
||||
|
||||
/// Typedef to this class
|
||||
typedef Rot3GravityFactor This;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
Rot3GravityFactor() {
|
||||
}
|
||||
|
||||
virtual ~Rot3GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param key of the Rot3 variable that will be constrained
|
||||
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
|
||||
* @param model Gaussian noise model
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
|
||||
const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
Base(model, key), GravityFactor(nZ, bRef) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
/** vector of errors */
|
||||
virtual Vector evaluateError(const Rot3& nRb, //
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
return attitudeError(nRb, H);
|
||||
}
|
||||
Unit3 nZ() const {
|
||||
return nZ_;
|
||||
}
|
||||
Unit3 bRef() const {
|
||||
return bRef_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & boost::serialization::make_nvp("GravityFactor",
|
||||
boost::serialization::base_object<GravityFactor>(*this));
|
||||
}
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Version of GravityFactor for Pose3
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class GTSAM_EXPORT Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
|
||||
public GravityFactor {
|
||||
|
||||
typedef NoiseModelFactor1<Pose3> Base;
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
|
||||
|
||||
/// Typedef to this class
|
||||
typedef Pose3GravityFactor This;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
Pose3GravityFactor() {
|
||||
}
|
||||
|
||||
virtual ~Pose3GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param key of the Pose3 variable that will be constrained
|
||||
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
|
||||
* @param model Gaussian noise model
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
|
||||
const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
Base(model, key), GravityFactor(nZ, bRef) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
/** vector of errors */
|
||||
virtual Vector evaluateError(const Pose3& nTb, //
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
Vector e = attitudeError(nTb.rotation(), H);
|
||||
if (H) {
|
||||
Matrix H23 = *H;
|
||||
*H = Matrix::Zero(2,6);
|
||||
H->block<2,3>(0,0) = H23;
|
||||
}
|
||||
return e;
|
||||
}
|
||||
Unit3 nZ() const {
|
||||
return nZ_;
|
||||
}
|
||||
Unit3 bRef() const {
|
||||
return bRef_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & boost::serialization::make_nvp("GravityFactor",
|
||||
boost::serialization::base_object<GravityFactor>(*this));
|
||||
}
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
||||
Reference in New Issue
Block a user