Unit tests for Jacobian of PartialPriorFactor<Pose3>

gtsam_unstable/dynamics/DynamicsPriors.h

@@ -50,16 +50,7 @@ struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
 struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
   typedef gtsam::PartialPriorFactor<PoseRTV> Base;
   VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model)
-  : Base(key, model) {
-    this->prior_ = vel;
-    assert(vel.size() == 3);
-    this->mask_.resize(3);
-    this->mask_[0] = 6;
-    this->mask_[1] = 7;
-    this->mask_[2] = 8;
-    this->H_ = Matrix::Zero(3, 9);
-    this->fillH();
-  }
+      : Base(key, {6, 7, 8}, vel, model) {}
 };
 
 /**
@@ -74,31 +65,14 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
    * Primary constructor allows for variable height of the "floor"
    */
   DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model)
-  : Base(key, model) {
-    this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch]
-    this->mask_.resize(4);
-    this->mask_[0] = 5; // z = height
-    this->mask_[1] = 8; // vz
-    this->mask_[2] = 0; // roll
-    this->mask_[3] = 1; // pitch
-    this->H_ = Matrix::Zero(3, 9);
-    this->fillH();
-  }
+      : Base(key, {5, 8, 0, 1}, Vector::Unit(4,0)*height, model) {}
 
   /**
    * Fully specify vector - use only for debugging
    */
   DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model)
-  : Base(key, model) {
+  : Base(key, {5, 8, 0, 1}, constraint, model) {
     assert(constraint.size() == 4);
-    this->prior_ = constraint; // [z, vz, roll, pitch]
-    this->mask_.resize(4);
-    this->mask_[0] = 5; // z = height
-    this->mask_[1] = 8; // vz
-    this->mask_[2] = 0; // roll
-    this->mask_[3] = 1; // pitch
-    this->H_ = Matrix::Zero(3, 9);
-    this->fillH();
   }
 };
 

gtsam_unstable/slam/PartialPriorFactor.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include <Eigen/Core>
+
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Lie.h>
 
@@ -29,11 +31,9 @@ namespace gtsam {
    *
    * The prior vector used in this factor is stored in compressed form, such that
    * it only contains values for measurements that are to be compared, and they are in
-   * the same order as VALUE::Logmap().  The mask will determine which components to extract
-   * in the error function.
+   * the same order as VALUE::Logmap(). The provided indices will determine which components to
+   * extract in the error function.
    *
-   * For practical use, it would be good to subclass this factor and have the class type
-   * construct the mask.
    * @tparam VALUE is the type of variable the prior effects
    */
   template<class VALUE>
@@ -43,16 +43,14 @@ namespace gtsam {
     typedef VALUE T;
 
   protected:
-
     // Concept checks on the variable type - currently requires Lie
     GTSAM_CONCEPT_LIE_TYPE(VALUE)
 
     typedef NoiseModelFactor1<VALUE> Base;
     typedef PartialPriorFactor<VALUE> This;
 
-    Vector prior_;             ///< measurement on tangent space parameters, in compressed form
-    std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector
-    Matrix H_;                  ///< Constant Jacobian - computed at creation
+    Vector prior_;                 ///< Measurement on tangent space parameters, in compressed form.
+    std::vector<size_t> indices_;  ///< Indices of the measured tangent space parameters.
 
     /** default constructor - only use for serialization */
     PartialPriorFactor() {}
@@ -68,20 +66,22 @@ namespace gtsam {
 
     ~PartialPriorFactor() override {}
 
-    /** Single Element Constructor: acts on a single parameter specified by idx */
+    /** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/
     PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
-      Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(Matrix::Zero(1, T::dimension)) {
+      Base(model, key),
+      prior_((Vector(1) << prior).finished()),
+      indices_(1, idx) {
       assert(model->dim() == 1);
-      this->fillH();
     }
 
-    /** Indices Constructor: specify the mask with a set of indices */
-    PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
+    /** Indices Constructor: Specify the relevant measured indices in the tangent vector.*/
+    PartialPriorFactor(Key key, const std::vector<size_t>& indices, const Vector& prior,
         const SharedNoiseModel& model) :
-      Base(model, key), prior_(prior), mask_(mask), H_(Matrix::Zero(mask.size(), T::dimension)) {
-      assert((size_t)prior_.size() == mask.size());
-      assert(model->dim() == (size_t) prior.size());
-      this->fillH();
+        Base(model, key),
+        prior_(prior),
+        indices_(indices) {
+      assert((size_t)prior_.size() == indices_.size());
+      assert(model->dim() == (size_t)prior.size());
     }
 
     /// @return a deep copy of this factor
@@ -102,35 +102,34 @@ namespace gtsam {
       const This *e = dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) &&
           gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
-          this->mask_ == e->mask_;
+          this->indices_ == e->indices_;
     }
 
     /** implement functions needed to derive from Factor */
 
-    /** vector of errors */
+    /** Returns a vector of errors for the measured tangent parameters.  */
     Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const override {
-      if (H) (*H) = H_;
-      // FIXME: this was originally the generic retraction - may not produce same results
-      Vector full_logmap = T::Logmap(p);
-//      Vector full_logmap = T::identity().localCoordinates(p); // Alternate implementation
-      Vector masked_logmap = Vector::Zero(this->dim());
-      for (size_t i=0; i<mask_.size(); ++i)
-        masked_logmap(i) = full_logmap(mask_[i]);
-      return masked_logmap - prior_;
+      if (H) {
+        Matrix H_logmap;
+        p.localCoordinates(T::identity(), H_logmap);
+        (*H) = Matrix::Zero(indices_.size(), T::dimension);
+        for (size_t i = 0; i < indices_.size(); ++i) {
+          (*H).row(i) = H_logmap.row(indices_.at(i));
+        }
+      }
+      // Compute the tangent vector representation of T and select relevant parameters.
+      const Vector& full_logmap = p.localCoordinates(T::identity());
+      Vector partial_logmap = Vector::Zero(T::dimension);
+      for (size_t i = 0; i < indices_.size(); ++i) {
+        partial_logmap(i) = full_logmap(indices_.at(i));
+      }
+
+      return partial_logmap - prior_;
     }
 
     // access
     const Vector& prior() const { return prior_; }
-    const std::vector<size_t>& mask() const { return  mask_; }
-    const Matrix& H() const { return H_; }
-
-  protected:
-
-    /** Constructs the jacobian matrix in place */
-    void fillH() {
-      for (size_t i=0; i<mask_.size(); ++i)
-        H_(i, mask_[i]) = 1.0;
-    }
+    const std::vector<size_t>& indices() const { return indices_; }
 
   private:
     /** Serialization function */
@@ -140,8 +139,8 @@ namespace gtsam {
       ar & boost::serialization::make_nvp("NoiseModelFactor1",
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(prior_);
-      ar & BOOST_SERIALIZATION_NVP(mask_);
-      ar & BOOST_SERIALIZATION_NVP(H_);
+      ar & BOOST_SERIALIZATION_NVP(indices_);
+      // ar & BOOST_SERIALIZATION_NVP(H_);
     }
   }; // \class PartialPriorFactor
 

gtsam_unstable/slam/tests/testPartialPriorFactor.cpp

@@ -0,0 +1,140 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+#include <gtsam_unstable/slam/PartialPriorFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+
+// Pose representation is [ Rx Ry Rz Tx Ty Tz ].
+static const int kIndexRx = 0;
+static const int kIndexRy = 1;
+static const int kIndexRz = 2;
+static const int kIndexTx = 3;
+static const int kIndexTy = 4;
+static const int kIndexTz = 5;
+
+
+typedef PartialPriorFactor<Pose3> TestPartialPriorFactor;
+
+/// traits
+namespace gtsam {
+template<>
+struct traits<TestPartialPriorFactor> : public Testable<TestPartialPriorFactor> {
+};
+}
+
+
+/* ************************************************************************* */
+TEST(PartialPriorFactor, JacobianAtIdentity)
+{
+  Key poseKey(1);
+  Pose3 measurement = Pose3::identity();
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
+
+  TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), model);
+
+  // Create a linearization point at the zero-error point
+  Pose3 pose = Pose3::identity();
+
+  // Calculate numerical derivatives.
+  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
+      boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
+
+  // Use the factor to calculate the derivative.
+  Matrix actualH1;
+  factor.evaluateError(pose, actualH1);
+
+  // Verify we get the expected error.
+  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
+}
+
+
+/* ************************************************************************* */
+TEST(PartialPriorFactor, JacobianPartialTranslation) {
+  Key poseKey(1);
+  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
+
+  TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), model);
+
+  // Create a linearization point at the zero-error point
+  Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
+
+  // Calculate numerical derivatives.
+  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
+      boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
+
+  // Use the factor to calculate the derivative.
+  Matrix actualH1;
+  factor.evaluateError(pose, actualH1);
+
+  // Verify we get the expected error.
+  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(PartialPriorFactor, JacobianFullTranslation) {
+  Key poseKey(1);
+  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
+
+  std::vector<size_t> translationIndices = { kIndexTx, kIndexTy, kIndexTz };
+  TestPartialPriorFactor factor(poseKey, translationIndices, measurement.translation(), model);
+
+  // Create a linearization point at the zero-error point
+  Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
+
+  // Calculate numerical derivatives.
+  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
+      boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
+
+  // Use the factor to calculate the derivative.
+  Matrix actualH1;
+  factor.evaluateError(pose, actualH1);
+
+  // Verify we get the expected error.
+  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(PartialPriorFactor, JacobianFullRotation) {
+  Key poseKey(1);
+  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
+
+  std::vector<size_t> rotationIndices = { kIndexRx, kIndexRy, kIndexRz };
+  TestPartialPriorFactor factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model);
+
+  // Create a linearization point at the zero-error point
+  Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
+
+  // Calculate numerical derivatives.
+  Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
+      boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
+
+  // Use the factor to calculate the derivative.
+  Matrix actualH1;
+  factor.evaluateError(pose, actualH1);
+
+  // Verify we get the expected error.
+  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */