Added Optimizer/GravitySigma parameter (with GTSAM support).

corelib/include/rtabmap/core/Link.h

@@ -48,6 +48,7 @@ public:
 		kNeighborMerged,
 		kPosePrior, // Absolute pose in /world frame, From == To
 		kLandmark,  // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To"
+		kPoseOdom,  // Pose in /odom frame, From == To (mainly used for gravity constraints)
 		kEnd,
 		kAllWithLandmarks = 98,
 		kAllWithoutLandmarks = 99,

corelib/include/rtabmap/core/Optimizer.h

@@ -90,6 +90,7 @@ public:
 	bool isRobust() const {return robust_;}
 	bool priorsIgnored() const {return priorsIgnored_;}
 	bool landmarksIgnored() const {return landmarksIgnored_;}
+	float gravitySigma() const {return gravitySigma_;}
 
 	// setters
 	void setIterations(int iterations) {iterations_ = iterations;}
@@ -99,6 +100,7 @@ public:
 	void setRobust(bool enabled) {robust_ = enabled;}
 	void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
 	void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
+	void setGravitySigma(float value) {gravitySigma_ = value;}
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
@@ -175,7 +177,8 @@ protected:
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
 			bool robust            = Parameters::defaultOptimizerRobust(),
 			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored(),
-			bool landmarksIgnored  = Parameters::defaultOptimizerLandmarksIgnored());
+			bool landmarksIgnored  = Parameters::defaultOptimizerLandmarksIgnored(),
+			float gravitySigma     = Parameters::defaultOptimizerGravitySigma());
 	Optimizer(const ParametersMap & parameters);
 
 private:
@@ -186,6 +189,7 @@ private:
 	bool robust_;
 	bool priorsIgnored_;
 	bool landmarksIgnored_;
+	float gravitySigma_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -388,6 +388,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Optimizer, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
     RTABMAP_PARAM(Optimizer, LandmarksIgnored,   bool, false,  "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
+    RTABMAP_PARAM(Optimizer, GravitySigma,    float, 0.0,      uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with GTSAM optimization strategy (see %s).", kOptimizerStrategy().c_str()));
 
 #ifdef RTABMAP_ORB_SLAM2
     RTABMAP_PARAM(g2o, Solver,            int, 3,          "0=csparse 1=pcg 2=cholmod 3=Eigen");

corelib/src/CMakeLists.txt

@@ -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") 

corelib/src/Optimizer.cpp

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

corelib/src/Rtabmap.cpp

@@ -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());

corelib/src/optimizer/OptimizerG2O.cpp

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

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -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)
 			{

corelib/src/optimizer/gtsam/GravityFactor.cpp

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

corelib/src/optimizer/gtsam/GravityFactor.h

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