Add Pose3-Point3 factor

gtsam_unstable/slam/PoseToPointFactor.h

@@ -0,0 +1,95 @@
+/**
+ *  @file   PoseToPointFactor.hpp
+ *  @brief  This factor can be used to track a 3D landmark over time by providing
+ *          local measurements of its location.
+ *  @author David Wisth
+ **/
+#pragma once
+
+#include <ostream>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+
+namespace gtsam {
+
+/**
+ * A class for a measurement between a pose and a point.
+ * @addtogroup SLAM
+ */
+class PoseToPointFactor: public NoiseModelFactor2<Pose3, Point3> {
+
+private:
+
+  typedef PoseToPointFactor This;
+  typedef NoiseModelFactor2<Pose3, Point3> Base;
+
+  Point3 measured_; /** the point measurement */
+
+public:
+
+  // shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
+
+  /** default constructor - only use for serialization */
+  PoseToPointFactor() {}
+
+  /** Constructor */
+  PoseToPointFactor(Key key1, Key key2, const Point3& measured,
+                    const SharedNoiseModel& model) :
+    Base(model, key1, key2), measured_(measured)
+  {
+  }
+
+  virtual ~PoseToPointFactor() {}
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    std::cout << s << "PoseToPointFactor("
+        << keyFormatter(this->key1()) << ","
+        << keyFormatter(this->key2()) << ")\n"
+        << "  measured: " << measured_.transpose() << std::endl;
+    this->noiseModel_->print("  noise model: ");
+  }
+
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected, 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);
+  }
+
+  /** implement functions needed to derive from Factor */
+
+  /** vector of errors
+    * Error = pose_est.inverse()*point_est - measured_
+    * (The error is in the measurement coordinate system)
+    */
+  Vector evaluateError(const Pose3& W_T_WI,
+                       const Point3& W_r_WC,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const
+  {
+    return W_T_WI.transformTo(W_r_WC, H1, H2) - measured_;
+  }
+
+  /** return the measured */
+  const Point3& measured() const {
+    return measured_;
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & boost::serialization::make_nvp("NoiseModelFactor2",
+        boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(measured_);
+  }
+
+}; // \class PoseToPointFactor
+
+} /// namespace gtsam

gtsam_unstable/slam/tests/testPoseToPointFactor.h

@@ -0,0 +1,84 @@
+/**
+ * @file   testPoseToPointFactor.cpp
+ * @brief
+ * @author David Wisth
+ * @date   June 20, 2020
+ */
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam_unstable/slam/PoseToPointFactor.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace gtsam;
+using namespace gtsam::noiseModel;
+
+/// Verify zero error when there is no noise
+TEST(LandmarkFactor, errorNoiseless) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(0.0, 0.0, 0.0);
+  Point3 measured = t + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor 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(LandmarkFactor, errorNoise) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0 , 2.0, 3.0);
+  Point3 noise(-1.0, 0.5, 0.3);
+  Point3 measured = t + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor 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(LandmarkFactor, jacobian) {
+  // 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 factor(pose_key, point_key, l_meas, noise);
+
+  // Calculate numerical derivatives
+  boost::function<Vector(const Pose3&, const Point3&)> f = boost::bind(
+      &LandmarkFactor::evaluateError, factor, _1, _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_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8));
+  EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */