Fix PartialPriorFactor (again)

gtsam_unstable/dynamics/DynamicsPriors.h

@@ -22,12 +22,54 @@ static const size_t kHeightIndex = 5;
 static const size_t kVelocityZIndex = 8;
 static const std::vector<size_t> kVelocityIndices = { 6, 7, 8 };
 
+
+class PartialPoseRTVPriorFactor : public PartialPriorFactor<PoseRTV> {
+ private:
+  typedef PartialPriorFactor<PoseRTV> Base;
+  typedef PartialPoseRTVPriorFactor This;
+
+ public:
+  using Base::Base;
+  using Base::evaluateError;
+
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+
+ private:
+  /**
+   * Maps a PoseRTV input x to a parameter vector h(x).
+   *
+   * The parameter vector is a 9-vector [ r, t, v ], where r is the angle-axis
+   * rotation, t is the translation, and v is the velocity.
+  */
+  virtual Vector Parameterize(const PoseRTV& x, Matrix* H = nullptr) const override {
+    Vector9 p; // The output parameter vector.
+
+    p.middleRows(3, 3) = x.translation();
+
+    Matrix3 H_rot;
+    p.middleRows(0, 3) = (H) ? Rot3::Logmap(x.rotation(), H_rot)
+                             : Rot3::Logmap(x.rotation());
+
+    if (H) {
+      *H = Matrix9::Zero();
+      (*H).block<3, 3>(0, 0) = H_rot;
+      (*H).block<3, 3>(3, 3) = x.rotation().matrix();
+      (*H).block<3, 3>(6, 6) = x.rotation().matrix();
+    }
+
+    return p;
+  }
+};
+
+
 /**
  * Forces the value of the height (z) in a PoseRTV to a specific value.
  * Dim: 1
  */
-struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
-  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
+struct DHeightPrior : public PartialPoseRTVPriorFactor {
+  typedef PartialPoseRTVPriorFactor Base;
 
   DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel& model)
   : Base(key, kHeightIndex, height, model)  {}
@@ -38,8 +80,8 @@ struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  * Implied value is zero
  * Dim: 1
  */
-struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
-  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
+struct DRollPrior : public PartialPoseRTVPriorFactor {
+  typedef PartialPoseRTVPriorFactor Base;
 
   /** allows for explicit roll parameterization - uses canonical coordinate */
   DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel& model)
@@ -55,8 +97,8 @@ struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  * Useful for enforcing a stationary state
  * Dim: 3
  */
-struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
-  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
+struct VelocityPrior : public PartialPoseRTVPriorFactor {
+  typedef PartialPoseRTVPriorFactor Base;
 
   VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model)
       : Base(key, kVelocityIndices, vel, model) {}
@@ -67,8 +109,8 @@ struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  * velocity in z direction
  * Dim: 4
  */
-struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
-  typedef gtsam::PartialPriorFactor<PoseRTV> Base;
+struct DGroundConstraint : public PartialPoseRTVPriorFactor {
+  typedef PartialPoseRTVPriorFactor Base;
 
   /**
    * Primary constructor allows for variable height of the "floor"

gtsam_unstable/slam/PartialPosePriorFactor.h

@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file PartialPosePriorFactor.h
+ * @brief Factor for setting a partial rotation or translation prior on a pose.
+ * @author Milo Knowles
+ */
+#pragma once
+
+#include <gtsam_unstable/slam/PartialPriorFactor.h>
+
+namespace gtsam {
+
+template <typename POSE>
+class PartialPosePriorFactor : public PartialPriorFactor<POSE> {
+ private:
+  typedef PartialPriorFactor<POSE> Base;
+  typedef PartialPosePriorFactor<POSE> This;
+  typedef typename POSE::Translation Translation;
+  typedef typename POSE::Rotation Rotation;
+
+ public:
+  GTSAM_CONCEPT_LIE_TYPE(POSE)
+
+  using Base::Base;
+  using Base::evaluateError;
+
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+
+ private:
+  /**
+   * Maps a POSE input x to a parameter vector h(x). In this case, the "parameter"
+   * vector is the same as the tangent vector representation (Logmap(x)) for
+   * rotation but is different for translation.
+   *
+   * For Pose2, the parameter vector is [ t, theta ].
+   * For Pose3, the parameter vector is [ r t ], where r is angle-axis
+   * representation of rotation, and t is the translation part of x.
+  */
+  virtual Vector Parameterize(const POSE& x, Matrix* H = nullptr) const override {
+    const int rDim = traits<Rotation>::GetDimension(x.rotation());
+    const int tDim = traits<Translation>::GetDimension(x.translation());
+    const int xDim = traits<POSE>::GetDimension(x);
+
+    Vector p(xDim); // The output parameter vector.
+
+    const std::pair<size_t, size_t> transInterval = POSE::translationInterval();
+    const std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
+
+    p.middleRows(transInterval.first, tDim) = x.translation();
+
+    Matrix H_rot;
+    p.middleRows(rotInterval.first, rDim) = (H) ? Rotation::Logmap(x.rotation(), H_rot)
+                                                : Rotation::Logmap(x.rotation());
+
+    if (H) {
+      *H = Matrix::Zero(xDim, xDim);
+      (*H).block(rotInterval.first, rotInterval.first, rDim, rDim) = H_rot;
+      (*H).block(transInterval.first, transInterval.first, tDim, tDim) = x.rotation().matrix();
+    }
+
+    return p;
+  }
+};
+
+}