Add more documentation and clang-format

gtsam_unstable/slam/PoseToPointFactor.h

@@ -1,15 +1,15 @@
 /**
  *  @file   PoseToPointFactor.hpp
- *  @brief  This factor can be used to track a 3D landmark over time by providing
+ *  @brief  This factor can be used to track a 3D landmark over time by
- *          local measurements of its location.
+ *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>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <ostream>
 
 namespace gtsam {
 
@@ -18,16 +18,13 @@ namespace gtsam {
  * @addtogroup SLAM
  */
 class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
-
  private:
-
   typedef PoseToPointFactor This;
   typedef NoiseModelFactor2<Pose3, Point3> Base;
 
-  Point3 measured_; /** the point measurement */
+  Point3 measured_; /** the point measurement in local coordinates */
 
  public:
-
   // shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
 
@@ -36,60 +33,58 @@ public:
 
   /** Constructor */
   PoseToPointFactor(Key key1, Key key2, const Point3& measured,
-                    const SharedNoiseModel& model) :
+                    const SharedNoiseModel& model)
-    Base(model, key1, key2), measured_(measured)
+      : 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 {
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-    std::cout << s << "PoseToPointFactor("
+                                               DefaultKeyFormatter) const {
-        << keyFormatter(this->key1()) << ","
+    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 {
+  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);
+    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_
+   * @brief Error = wTwi.inverse()*wPwp - measured_
-    * (The error is in the measurement coordinate system)
+   * @param wTwi The pose of the sensor in world coordinates
+   * @param wPwp The estimated point location in world coordinates
+   *
+   * Note: measured_ and the error are in local coordiantes.
    */
-  Vector evaluateError(const Pose3& W_T_WI,
+  Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
-                       const Point3& W_r_WC,
                        boost::optional<Matrix&> H1 = boost::none,
-                       boost::optional<Matrix&> H2 = boost::none) const
+                       boost::optional<Matrix&> H2 = boost::none) const {
-  {
+    return wTwi.transformTo(wPwp, H1, H2) - measured_;
-    return W_T_WI.transformTo(W_r_WC, H1, H2) - measured_;
   }
 
   /** return the measured */
-  const Point3& measured() const {
+  const Point3& measured() const { return measured_; }
-    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",
+    ar& boost::serialization::make_nvp(
-        boost::serialization::base_object<Base>(*this));
+        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
     ar& BOOST_SERIALIZATION_NVP(measured_);
   }
 
 };  // \class PoseToPointFactor
 
-} /// namespace gtsam
+}  // namespace gtsam

gtsam_unstable/slam/tests/testPoseToPointFactor.h

@@ -5,9 +5,9 @@
  * @date   June 20, 2020
  */
 
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam_unstable/slam/PoseToPointFactor.h>
-#include <CppUnitLite/TestHarness.h>
 
 using namespace gtsam;
 using namespace gtsam::noiseModel;
@@ -21,7 +21,8 @@ TEST(PoseToPointFactor, errorNoiseless) {
 
   Key pose_key(1);
   Key point_key(2);
-  PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05));
+  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));
@@ -36,7 +37,8 @@ TEST(PoseToPointFactor, errorNoise) {
 
   Key pose_key(1);
   Key point_key(2);
-  PoseToPointFactor factor(pose_key, point_key, measured, Isotropic::Sigma(3, 0.05));
+  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));
@@ -61,8 +63,8 @@ TEST(PoseToPointFactor, jacobian) {
   PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
 
   // Calculate numerical derivatives
-  boost::function<Vector(const Pose3&, const Point3&)> f = boost::bind(
+  auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
-      &LandmarkFactor::evaluateError, factor, _1, _2, boost::none, boost::none);
+                       boost::none, boost::none);
   Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
   Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);