i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Optimizer/GravitySigma parameter (with GTSAM support).

Browse Source
This commit is contained in:
matlabbe
2019-03-31 15:51:50 -04:00
parent 35933cafba
commit fc9c762516
17 changed files with 565 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/CMakeLists.txt
View File

@@ -467,6 +467,10 @@ IF(GTSAM_FOUND)
${GTSAM_INCLUDE_DIRS}
)
ENDIF()
SET(SRC_FILES
${SRC_FILES}
optimizer/gtsam/GravityFactor.cpp
)
IF(WIN32)
# GTSAM should be built in STATIC on Windows to avoid "error C2338: THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS" when building GTSAM
add_definitions("-DGTSAM_IMPORT_STATIC")

11
corelib/src/Optimizer.cpp
View File

@@ -210,7 +210,7 @@ void Optimizer::getConnectedGraph(
{
if(!uContains(posesOut, toId))
{
posesOut.insert(std::make_pair(toId, posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse())));
posesOut.insert(*posesIn.find(toId));//std::make_pair(toId, posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse())));
if(curentPoses.find(toId) == curentPoses.end())
{
nextPoses.insert(toId);
@@ -229,14 +229,15 @@ void Optimizer::getConnectedGraph(
}
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored) :
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored, float gravitySigma) :
iterations_(iterations),
slam2d_(slam2d),
covarianceIgnored_(covarianceIgnored),
epsilon_(epsilon),
robust_(robust),
priorsIgnored_(priorsIgnored),
landmarksIgnored_(landmarksIgnored)
landmarksIgnored_(landmarksIgnored),
gravitySigma_(gravitySigma)
{
}
@@ -247,7 +248,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) :
epsilon_(Parameters::defaultOptimizerEpsilon()),
robust_(Parameters::defaultOptimizerRobust()),
priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored()),
landmarksIgnored_(Parameters::defaultOptimizerLandmarksIgnored())
landmarksIgnored_(Parameters::defaultOptimizerLandmarksIgnored()),
gravitySigma_(Parameters::defaultOptimizerGravitySigma())
{
parseParameters(parameters);
}
@@ -261,6 +263,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
Parameters::parse(parameters, Parameters::kOptimizerLandmarksIgnored(), landmarksIgnored_);
Parameters::parse(parameters, Parameters::kOptimizerGravitySigma(), gravitySigma_);
}
std::map<int, Transform> Optimizer::optimizeIncremental(

28
corelib/src/Rtabmap.cpp
View File

@@ -3737,15 +3737,21 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, true);
UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
// Apply guess poses (if some)
if(_graphOptimizer->iterations() > 0)
{
for(std::map<int, Transform>::const_iterator iter=guessPoses.begin(); iter!=guessPoses.end(); ++iter)
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, Transform>::iterator foundPose = poses.find(iter->first);
if(foundPose!=poses.end())
if(iter->first > 0 && _graphOptimizer->gravitySigma() > 0.0f)
{
foundPose->second = iter->second;
// add odometry constraints
edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
// Apply guess poses (if some)
std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(iter->first);
if(foundGuess!=guessPoses.end())
{
iter->second = foundGuess->second;
}
}
}
@@ -4275,6 +4281,18 @@ int Rtabmap::detectMoreLoopClosures(
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
if(_graphOptimizer->gravitySigma() > 0.0f)
{
for(std::map<int, Transform>::iterator jter=poses.lower_bound(1); jter!=poses.end(); ++jter)
{
std::map<int, Signature>::iterator ster = signatures.find(iter->first);
if(ster != signatures.end() && !ster->second.getPose().isNull())
{
// add odometry constraints
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, ster->second.getPose())));
}
}
}
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());

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

@@ -443,7 +443,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(id1 == id2)
{
if(!priorsIgnored())
if(iter->second.type() == Link::kPosePrior && !priorsIgnored())
{
if(isSlam2d())
{

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

@@ -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
View 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
View 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