Templated PoseToPointFactor

gtsam_unstable/slam/PoseToPointFactor.h

@@ -1,11 +1,14 @@
 /**
  *  @file   PoseToPointFactor.hpp
- *  @brief  This factor can be used to track a 3D landmark over time by
- *providing local measurements of its location.
+ *  @brief  This factor can be used to model relative position measurements
+ *  from a (2D or 3D) pose to a landmark
  *  @author David Wisth
+ *  @author Luca Carlone
  **/
 #pragma once
 
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -17,12 +20,13 @@ namespace gtsam {
  * A class for a measurement between a pose and a point.
  * @addtogroup SLAM
  */
-class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
+template<typename POSE = Pose3, typename POINT = Point3>
+class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
  private:
   typedef PoseToPointFactor This;
-  typedef NoiseModelFactor2<Pose3, Point3> Base;
+  typedef NoiseModelFactor2<POSE, POINT> Base;
 
-  Point3 measured_; /** the point measurement in local coordinates */
+  POINT measured_; /** the point measurement in local coordinates */
 
  public:
   // shorthand for a smart pointer to a factor
@@ -32,7 +36,7 @@ class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
   PoseToPointFactor() {}
 
   /** Constructor */
-  PoseToPointFactor(Key key1, Key key2, const Point3& measured,
+  PoseToPointFactor(Key key1, Key key2, const POINT& measured,
                     const SharedNoiseModel& model)
       : Base(model, key1, key2), measured_(measured) {}
 
@@ -54,26 +58,26 @@ class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
                       double tol = 1e-9) const {
     const This* e = dynamic_cast<const This*>(&expected);
     return e != nullptr && Base::equals(*e, tol) &&
-           traits<Point3>::Equals(this->measured_, e->measured_, tol);
+           traits<POINT>::Equals(this->measured_, e->measured_, tol);
   }
 
   /** implement functions needed to derive from Factor */
 
   /** vector of errors
-   * @brief Error = wTwi.inverse()*wPwp - measured_
-   * @param wTwi The pose of the sensor in world coordinates
-   * @param wPwp The estimated point location in world coordinates
+   * @brief Error = w_T_b.inverse()*w_P - measured_
+   * @param w_T_b The pose of the body in world coordinates
+   * @param w_P The estimated point location in world coordinates
    *
    * Note: measured_ and the error are in local coordiantes.
    */
-  Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
+  Vector evaluateError(const POSE& w_T_b, const POINT& w_P,
                        boost::optional<Matrix&> H1 = boost::none,
                        boost::optional<Matrix&> H2 = boost::none) const {
-    return wTwi.transformTo(wPwp, H1, H2) - measured_;
+    return w_T_b.transformTo(w_P, H1, H2) - measured_;
   }
 
   /** return the measured */
-  const Point3& measured() const { return measured_; }
+  const POINT& measured() const { return measured_; }
 
  private:
   /** Serialization function */

gtsam_unstable/slam/tests/testPoseToPointFactor.cpp

@@ -0,0 +1,161 @@
+/**
+ * @file   testPoseToPointFactor.cpp
+ * @brief
+ * @author David Wisth
+ * @author Luca Carlone
+ * @date   June 20, 2020
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam_unstable/slam/PoseToPointFactor.h>
+
+using namespace gtsam;
+using namespace gtsam::noiseModel;
+
+/* ************************************************************************* */
+// Verify zero error when there is no noise
+TEST(PoseToPointFactor, errorNoiseless_2D) {
+  Pose2 pose = Pose2::identity();
+  Point2 point(1.0, 2.0);
+  Point2 noise(0.0, 0.0);
+  Point2 measured = point + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(2, 0.05));
+  Vector expectedError = Vector2(0.0, 0.0);
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/* ************************************************************************* */
+// Verify expected error in test scenario
+TEST(PoseToPointFactor, errorNoise_2D) {
+  Pose2 pose = Pose2::identity();
+  Point2 point(1.0, 2.0);
+  Point2 noise(-1.0, 0.5);
+  Point2 measured = point + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(2, 0.05));
+  Vector expectedError = -noise;
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/* ************************************************************************* */
+// Check Jacobians are correct
+TEST(PoseToPointFactor, jacobian_2D) {
+  // Measurement
+  gtsam::Point2 l_meas(1, 2);
+
+  // Linearisation point
+  gtsam::Point2 p_t(-5, 12);
+  gtsam::Rot2 p_R(1.5 * M_PI);
+  Pose2 p(p_R, p_t);
+
+  gtsam::Point2 l(3, 0);
+
+  // Factor
+  Key pose_key(1);
+  Key point_key(2);
+  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
+  PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, l_meas, noise);
+
+  // Calculate numerical derivatives
+  auto f = std::bind(&PoseToPointFactor<Pose2,Point2>::evaluateError, factor,
+                     std::placeholders::_1, std::placeholders::_2, boost::none,
+                     boost::none);
+  Matrix numerical_H1 = numericalDerivative21<Vector, Pose2, Point2>(f, p, l);
+  Matrix numerical_H2 = numericalDerivative22<Vector, Pose2, Point2>(f, p, l);
+
+  // Use the factor to calculate the derivative
+  Matrix actual_H1;
+  Matrix actual_H2;
+  factor.evaluateError(p, l, actual_H1, actual_H2);
+
+  // Verify we get the expected error
+  EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8));
+  EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8));
+}
+
+/* ************************************************************************* */
+// Verify zero error when there is no noise
+TEST(PoseToPointFactor, errorNoiseless_3D) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(0.0, 0.0, 0.0);
+  Point3 measured = point + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(3, 0.05));
+  Vector expectedError = Vector3(0.0, 0.0, 0.0);
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/* ************************************************************************* */
+// Verify expected error in test scenario
+TEST(PoseToPointFactor, errorNoise_3D) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(-1.0, 0.5, 0.3);
+  Point3 measured = point + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(3, 0.05));
+  Vector expectedError = -noise;
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/* ************************************************************************* */
+// Check Jacobians are correct
+TEST(PoseToPointFactor, jacobian_3D) {
+  // Measurement
+  gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
+
+  // Linearisation point
+  gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
+  gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI);
+  Pose3 p(p_R, p_t);
+
+  gtsam::Point3 l = gtsam::Point3(3, 0, 5);
+
+  // Factor
+  Key pose_key(1);
+  Key point_key(2);
+  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
+  PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, l_meas, noise);
+
+  // Calculate numerical derivatives
+  auto f = std::bind(&PoseToPointFactor<Pose3,Point3>::evaluateError, factor,
+                     std::placeholders::_1, std::placeholders::_2, boost::none,
+                     boost::none);
+  Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
+  Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);
+
+  // Use the factor to calculate the derivative
+  Matrix actual_H1;
+  Matrix actual_H2;
+  factor.evaluateError(p, l, actual_H1, actual_H2);
+
+  // Verify we get the expected error
+  EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8));
+  EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */