0.19.2: Refactored SensorData interface. DBReader: Fixed GPS not published. #345: both g2o and gtsam working with GPS. g2o: added gravity edges.

corelib/src/optimizer/gtsam/GPSPose2XYFactor.h

@@ -0,0 +1,57 @@
+
+/**
+ * Author: Mathieu Labbe
+ * This file is a copy of GPSPose2Factor.h of gtsam examples
+ */
+
+/**
+ * A simple 2D 'GPS' like factor
+ * The factor contains a X-Y position measurement (mx, my) for a Pose, but no rotation information
+ * The error vector will be [x-mx, y-my]'
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Pose2.h>
+
+
+namespace rtabmap {
+
+class GPSPose2XYFactor: public gtsam::NoiseModelFactor1<gtsam::Pose2> {
+
+private:
+  // measurement information
+  double mx_, my_;
+
+public:
+
+  /**
+   * Constructor
+   * @param poseKey    associated pose varible key
+   * @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()) {}
+
+  // 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 {
+
+    // note that use boost optional like a pointer
+    // only calculate jacobian matrix when non-null pointer exists
+    if (H) *H = (gtsam::Matrix23() << 1.0, 0.0, 0.0,
+                                      0.0, 1.0, 0.0).finished();
+
+    // return error vector
+    return (gtsam::Vector2() << p.x() - mx_, p.y() - my_).finished();
+  }
+
+};
+
+} // namespace gtsamexamples
+

corelib/src/optimizer/gtsam/GPSPose3XYZFactor.h

@@ -0,0 +1,53 @@
+
+/**
+ * Author: Mathieu Labbe
+ * This file is a copy of GPSPose2Factor.h of gtsam examples for Pose3
+ */
+
+/**
+ * A simple 3D 'GPS' like factor
+ * The factor contains a X-Y-Z position measurement (mx, my, mz) for a Pose, but no rotation information
+ * The error vector will be [x-mx, y-my, z-mz]'
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Pose3.h>
+
+
+namespace rtabmap {
+
+class GPSPose3XYZFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
+
+private:
+  // measurement information
+  double mx_, my_, mz_;
+
+public:
+
+  /**
+   * Constructor
+   * @param poseKey    associated pose variable key
+   * @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()) {}
+
+  // error function
+  // @param p    the pose in Pose
+  // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
+  gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
+    if(H)
+    {
+	  p.translation(H);
+    }
+    return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
+  }
+};
+
+} // namespace gtsamexamples
+

corelib/src/optimizer/gtsam/GravityFactor.cpp

@@ -9,6 +9,13 @@
 
  * -------------------------------------------------------------------------- */
 
+/**
+ * Author: Mathieu Labbe
+ * This file is a copy of AttitudeFactor.cpp 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   GravityFactor.cpp
  *  @author Frank Dellaert