Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors

# Conflicts:
#	gtsam/geometry/CameraSet.h

doc/CMakeLists.txt

@@ -22,18 +22,19 @@ if (GTSAM_BUILD_DOCS)
 
     # GTSAM core subfolders
     set(gtsam_doc_subdirs 
-        gtsam/base 
-        gtsam/discrete 
-        gtsam/geometry 
-        gtsam/inference 
-        gtsam/linear 
-        gtsam/navigation 
-        gtsam/nonlinear 
-        gtsam/sam 
-        gtsam/sfm 
-        gtsam/slam 
-        gtsam/smart 
-        gtsam/symbolic 
+        gtsam/base
+        gtsam/basis 
+        gtsam/discrete
+        gtsam/geometry
+        gtsam/inference
+        gtsam/linear
+        gtsam/navigation
+        gtsam/nonlinear
+        gtsam/sam
+        gtsam/sfm
+        gtsam/slam
+        gtsam/smart
+        gtsam/symbolic
         gtsam
     )
     

gtsam/CMakeLists.txt

@@ -5,6 +5,7 @@ project(gtsam LANGUAGES CXX)
 # The following variable is the master list of subdirs to add
 set (gtsam_subdirs
     base
+    basis
     geometry
     inference
     symbolic

gtsam/base/Lie.h

@@ -320,28 +320,28 @@ T expm(const Vector& x, int K=7) {
 }
 
 /**
- * Linear interpolation between X and Y by coefficient t (typically t \in [0,1],
- * but can also be used to extrapolate before pose X or after pose Y), with optional jacobians.
+ * Linear interpolation between X and Y by coefficient t. Typically t \in [0,1],
+ * but can also be used to extrapolate before pose X or after pose Y.
  */
 template <typename T>
 T interpolate(const T& X, const T& Y, double t,
               typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
               typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
-
   if (Hx || Hy) {
-    typename traits<T>::TangentVector log_Xinv_Y;
     typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
+    const T between =
+        traits<T>::Between(X, Y, between_H_x);  // between_H_y = identity
+    typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
+    const T Delta = traits<T>::Expmap(t * delta, exp_H);
+    const T result = traits<T>::Compose(
+        X, Delta, compose_H_x);  // compose_H_xinv_y = identity
 
-    T Xinv_Y = traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
-    log_Xinv_Y = traits<T>::Logmap(Xinv_Y, log_H);
-    Xinv_Y = traits<T>::Expmap(t * log_Xinv_Y, exp_H);
-    Xinv_Y = traits<T>::Compose(X, Xinv_Y, compose_H_x); // compose_H_xinv_y = identity
-
-    if(Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
-    if(Hy) *Hy = t * exp_H * log_H;
-    return Xinv_Y;
+    if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
+    if (Hy) *Hy = t * exp_H * log_H;
+    return result;
   }
-  return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
+  return traits<T>::Compose(
+      X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
 }
 
 /**

gtsam/basis/Basis.h

@@ -0,0 +1,507 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Basis.h
+ * @brief Compute an interpolating basis
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/basis/ParameterMatrix.h>
+
+#include <iostream>
+
+/**
+ * This file supports creating continuous functions `f(x;p)` as a linear
+ * combination of `basis functions` such as the Fourier basis on SO(2) or a set
+ * of Chebyshev polynomials on [-1,1].
+ *
+ * In the expression `f(x;p)` the variable `x` is
+ * the continuous argument at which the function is evaluated, and `p` are
+ * the parameters which are coefficients of the different basis functions,
+ * e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial.
+ * However, different parameterizations are also possible.
+
+ * The `Basis` class below defines a number of functors that can be used to
+ * evaluate `f(x;p)` at a given `x`, and these functors also calculate
+ * the Jacobian of `f(x;p)` with respect to the parameters `p`.
+ * This is actually the most important calculation, as it will allow GTSAM
+ * to optimize over the parameters `p`.
+
+ * This functionality is implemented using the `CRTP` or "Curiously recurring
+ * template pattern" C++ idiom, which is a meta-programming technique in which
+ * the derived class is passed as a template argument to `Basis<DERIVED>`.
+ * The DERIVED class is assumed to satisfy a C++ concept,
+ * i.e., we expect it to define the following types and methods:
+
+  - type `Parameters`: the parameters `p` in f(x;p)
+  - `CalculateWeights(size_t N, double x, double a=default, double b=default)`
+  - `DerivativeWeights(size_t N, double x, double a=default, double b=default)`
+
+  where `Weights` is an N*1 row vector which defines the basis values for the
+ polynomial at the specified point `x`.
+
+ E.g. A Fourier series would give the following:
+  - `CalculateWeights` -> For N=5, the values for the bases:
+        [1, cos(x), sin(x), cos(2x), sin(2x)]
+  - `DerivativeWeights` -> For N=5, these are:
+        [0, -sin(x), cos(x), -2sin(2x), 2cos(x)]
+
+ Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are
+ instead the values for the Barycentric interpolation formula, since the values
+ at the polynomial points (e.g. Chebyshev points) define the bases.
+ */
+
+namespace gtsam {
+
+using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
+
+/**
+ * @brief Function for computing the kronecker product of the 1*N Weight vector
+ * `w` with the MxM identity matrix `I` efficiently.
+ * The main reason for this is so we don't need to use Eigen's Unsupported
+ * library.
+ *
+ * @tparam M Size of the identity matrix.
+ * @param w The weights of the polynomial.
+ * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I]
+ */
+template <size_t M>
+Matrix kroneckerProductIdentity(const Weights& w) {
+  Matrix result(M, w.cols() * M);
+  result.setZero();
+
+  for (int i = 0; i < w.cols(); i++) {
+    result.block(0, i * M, M, M).diagonal().array() = w(i);
+  }
+  return result;
+}
+
+/// CRTP Base class for function bases
+template <typename DERIVED>
+class GTSAM_EXPORT Basis {
+ public:
+  /**
+   * Calculate weights for all x in vector X.
+   * Returns M*N matrix where M is the size of the vector X,
+   * and N is the number of basis functions.
+   */
+  static Matrix WeightMatrix(size_t N, const Vector& X) {
+    Matrix W(X.size(), N);
+    for (int i = 0; i < X.size(); i++)
+      W.row(i) = DERIVED::CalculateWeights(N, X(i));
+    return W;
+  }
+
+  /**
+   * @brief Calculate weights for all x in vector X, with interval [a,b].
+   *
+   * @param N The number of basis functions.
+   * @param X The vector for which to compute the weights.
+   * @param a The lower bound for the interval range.
+   * @param b The upper bound for the interval range.
+   * @return Returns M*N matrix where M is the size of the vector X.
+   */
+  static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) {
+    Matrix W(X.size(), N);
+    for (int i = 0; i < X.size(); i++)
+      W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b);
+    return W;
+  }
+
+  /**
+   * An instance of an EvaluationFunctor calculates f(x;p) at a given `x`,
+   * applied to Parameters `p`.
+   * This functor is used to evaluate a parameterized function at a given scalar
+   * value x. When given a specific N*1 vector of Parameters, returns the scalar
+   * value of the function at x, possibly with Jacobians wrpt the parameters.
+   */
+  class EvaluationFunctor {
+   protected:
+    Weights weights_;
+
+   public:
+    /// For serialization
+    EvaluationFunctor() {}
+
+    /// Constructor with interval [a,b]
+    EvaluationFunctor(size_t N, double x)
+        : weights_(DERIVED::CalculateWeights(N, x)) {}
+
+    /// Constructor with interval [a,b]
+    EvaluationFunctor(size_t N, double x, double a, double b)
+        : weights_(DERIVED::CalculateWeights(N, x, a, b)) {}
+
+    /// Regular 1D evaluation
+    double apply(const typename DERIVED::Parameters& p,
+                 OptionalJacobian<-1, -1> H = boost::none) const {
+      if (H) *H = weights_;
+      return (weights_ * p)(0);
+    }
+
+    /// c++ sugar
+    double operator()(const typename DERIVED::Parameters& p,
+                      OptionalJacobian<-1, -1> H = boost::none) const {
+      return apply(p, H);  // might call apply in derived
+    }
+
+    void print(const std::string& s = "") const {
+      std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
+    }
+  };
+
+  /**
+   * VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
+   * This functor is used to evaluate a parameterized function at a given scalar
+   * value x. When given a specific M*N parameters, returns an M-vector the M
+   * corresponding functions at x, possibly with Jacobians wrpt the parameters.
+   */
+  template <int M>
+  class VectorEvaluationFunctor : protected EvaluationFunctor {
+   protected:
+    using VectorM = Eigen::Matrix<double, M, 1>;
+    using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
+    Jacobian H_;
+
+    /**
+     * Calculate the `M*(M*N)` Jacobian of this functor with respect to
+     * the M*N parameter matrix `P`.
+     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
+     *      H =[ w(0) 0 w(1)  0 w(2)  0
+     *           0  w(0)  0 w(1)  0 w(2) ]
+     * i.e., the Kronecker product of weights_ with the MxM identity matrix.
+     */
+    void calculateJacobian() {
+      H_ = kroneckerProductIdentity<M>(this->weights_);
+    }
+
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    /// For serialization
+    VectorEvaluationFunctor() {}
+
+    /// Default Constructor
+    VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
+      calculateJacobian();
+    }
+
+    /// Constructor, with interval [a,b]
+    VectorEvaluationFunctor(size_t N, double x, double a, double b)
+        : EvaluationFunctor(N, x, a, b) {
+      calculateJacobian();
+    }
+
+    /// M-dimensional evaluation
+    VectorM apply(const ParameterMatrix<M>& P,
+                  OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
+      if (H) *H = H_;
+      return P.matrix() * this->weights_.transpose();
+    }
+
+    /// c++ sugar
+    VectorM operator()(const ParameterMatrix<M>& P,
+                       OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);
+    }
+  };
+
+  /**
+   * Given a M*N Matrix of M-vectors at N polynomial points, an instance of
+   * VectorComponentFunctor computes the N-vector value for a specific row
+   * component of the M-vectors at all the polynomial points.
+   *
+   * This component is specified by the row index i, with 0<i<M.
+   */
+  template <int M>
+  class VectorComponentFunctor : public EvaluationFunctor {
+   protected:
+    using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
+    size_t rowIndex_;
+    Jacobian H_;
+
+    /*
+     * Calculate the `1*(M*N)` Jacobian of this functor with respect to
+     * the M*N parameter matrix `P`.
+     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
+     *      H=[w(0) 0    w(1) 0    w(2) 0]    for rowIndex==0
+     *      H=[0    w(0) 0    w(1) 0    w(2)] for rowIndex==1
+     * i.e., one row of the Kronecker product of weights_ with the
+     * MxM identity matrix. See also VectorEvaluationFunctor.
+     */
+    void calculateJacobian(size_t N) {
+      H_.setZero(1, M * N);
+      for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
+        H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j);
+    }
+
+   public:
+    /// For serialization
+    VectorComponentFunctor() {}
+
+    /// Construct with row index
+    VectorComponentFunctor(size_t N, size_t i, double x)
+        : EvaluationFunctor(N, x), rowIndex_(i) {
+      calculateJacobian(N);
+    }
+
+    /// Construct with row index and interval
+    VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
+        : EvaluationFunctor(N, x, a, b), rowIndex_(i) {
+      calculateJacobian(N);
+    }
+
+    /// Calculate component of component rowIndex_ of P
+    double apply(const ParameterMatrix<M>& P,
+                 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
+      if (H) *H = H_;
+      return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
+    }
+
+    /// c++ sugar
+    double operator()(const ParameterMatrix<M>& P,
+                      OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);
+    }
+  };
+
+  /**
+   * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
+   * This functor is used to evaluate a parameterized function at a given scalar
+   * value x. When given a specific M*N parameters, returns an M-vector the M
+   * corresponding functions at x, possibly with Jacobians wrpt the parameters.
+   *
+   * The difference with the VectorEvaluationFunctor is that after computing the
+   * M*1 vector xi=F(x;P), with x a scalar and P the M*N parameter vector, we
+   * also retract xi back to the T manifold.
+   * For example, if T==Rot3, then we first compute a 3-vector xi using x and P,
+   * and then map that 3-vector xi back to the Rot3 manifold, yielding a valid
+   * 3D rotation.
+   */
+  template <class T>
+  class ManifoldEvaluationFunctor
+      : public VectorEvaluationFunctor<traits<T>::dimension> {
+    enum { M = traits<T>::dimension };
+    using Base = VectorEvaluationFunctor<M>;
+
+   public:
+    /// For serialization
+    ManifoldEvaluationFunctor() {}
+
+    /// Default Constructor
+    ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {}
+
+    /// Constructor, with interval [a,b]
+    ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
+        : Base(N, x, a, b) {}
+
+    /// Manifold evaluation
+    T apply(const ParameterMatrix<M>& P,
+            OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
+      // Interpolate the M-dimensional vector to yield a vector in tangent space
+      Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
+
+      // Now call retract with this M-vector, possibly with derivatives
+      Eigen::Matrix<double, M, M> D_result_xi;
+      T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0);
+
+      // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N)
+      // derivative of interpolation and D_result_xi is MxM derivative of
+      // retract.
+      if (H) *H = D_result_xi * (*H);
+
+      // and return a T
+      return result;
+    }
+
+    /// c++ sugar
+    T operator()(const ParameterMatrix<M>& P,
+                 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);  // might call apply in derived
+    }
+  };
+
+  /// Base class for functors below that calculate derivative weights
+  class DerivativeFunctorBase {
+   protected:
+    Weights weights_;
+
+   public:
+    /// For serialization
+    DerivativeFunctorBase() {}
+
+    DerivativeFunctorBase(size_t N, double x)
+        : weights_(DERIVED::DerivativeWeights(N, x)) {}
+
+    DerivativeFunctorBase(size_t N, double x, double a, double b)
+        : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {}
+
+    void print(const std::string& s = "") const {
+      std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
+    }
+  };
+
+  /**
+   * An instance of a DerivativeFunctor calculates f'(x;p) at a given `x`,
+   * applied to Parameters `p`.
+   * When given a scalar value x and a specific N*1 vector of Parameters,
+   * this functor returns the scalar derivative value of the function at x,
+   * possibly with Jacobians wrpt the parameters.
+   */
+  class DerivativeFunctor : protected DerivativeFunctorBase {
+   public:
+    /// For serialization
+    DerivativeFunctor() {}
+
+    DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {}
+
+    DerivativeFunctor(size_t N, double x, double a, double b)
+        : DerivativeFunctorBase(N, x, a, b) {}
+
+    double apply(const typename DERIVED::Parameters& p,
+                 OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
+      if (H) *H = this->weights_;
+      return (this->weights_ * p)(0);
+    }
+    /// c++ sugar
+    double operator()(const typename DERIVED::Parameters& p,
+                      OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
+      return apply(p, H);  // might call apply in derived
+    }
+  };
+
+  /**
+   * VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
+   *
+   * This functor is used to evaluate the derivatives of a parameterized
+   * function at a given scalar value x. When given a specific M*N parameters,
+   * returns an M-vector the M corresponding function derivatives at x, possibly
+   * with Jacobians wrpt the parameters.
+   */
+  template <int M>
+  class VectorDerivativeFunctor : protected DerivativeFunctorBase {
+   protected:
+    using VectorM = Eigen::Matrix<double, M, 1>;
+    using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
+    Jacobian H_;
+
+    /**
+     * Calculate the `M*(M*N)` Jacobian of this functor with respect to
+     * the M*N parameter matrix `P`.
+     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
+     *      H =[ w(0) 0 w(1)  0 w(2)  0
+     *           0  w(0)  0 w(1)  0 w(2) ]
+     * i.e., the Kronecker product of weights_ with the MxM identity matrix.
+     */
+    void calculateJacobian() {
+      H_ = kroneckerProductIdentity<M>(this->weights_);
+    }
+
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    /// For serialization
+    VectorDerivativeFunctor() {}
+
+    /// Default Constructor
+    VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
+      calculateJacobian();
+    }
+
+    /// Constructor, with optional interval [a,b]
+    VectorDerivativeFunctor(size_t N, double x, double a, double b)
+        : DerivativeFunctorBase(N, x, a, b) {
+      calculateJacobian();
+    }
+
+    VectorM apply(const ParameterMatrix<M>& P,
+                  OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
+      if (H) *H = H_;
+      return P.matrix() * this->weights_.transpose();
+    }
+    /// c++ sugar
+    VectorM operator()(
+        const ParameterMatrix<M>& P,
+        OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);
+    }
+  };
+
+  /**
+   * Given a M*N Matrix of M-vectors at N polynomial points, an instance of
+   * ComponentDerivativeFunctor computes the N-vector derivative for a specific
+   * row component of the M-vectors at all the polynomial points.
+   *
+   * This component is specified by the row index i, with 0<i<M.
+   */
+  template <int M>
+  class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
+   protected:
+    using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
+    size_t rowIndex_;
+    Jacobian H_;
+
+    /*
+     * Calculate the `1*(M*N)` Jacobian of this functor with respect to
+     * the M*N parameter matrix `P`.
+     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
+     *      H=[w(0) 0    w(1) 0    w(2) 0]    for rowIndex==0
+     *      H=[0    w(0) 0    w(1) 0    w(2)] for rowIndex==1
+     * i.e., one row of the Kronecker product of weights_ with the
+     * MxM identity matrix. See also VectorDerivativeFunctor.
+     */
+    void calculateJacobian(size_t N) {
+      H_.setZero(1, M * N);
+      for (int j = 0; j < this->weights_.size(); j++)
+        H_(0, rowIndex_ + j * M) = this->weights_(j);
+    }
+
+   public:
+    /// For serialization
+    ComponentDerivativeFunctor() {}
+
+    /// Construct with row index
+    ComponentDerivativeFunctor(size_t N, size_t i, double x)
+        : DerivativeFunctorBase(N, x), rowIndex_(i) {
+      calculateJacobian(N);
+    }
+
+    /// Construct with row index and interval
+    ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
+        : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
+      calculateJacobian(N);
+    }
+    /// Calculate derivative of component rowIndex_ of F
+    double apply(const ParameterMatrix<M>& P,
+                 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
+      if (H) *H = H_;
+      return P.row(rowIndex_) * this->weights_.transpose();
+    }
+    /// c++ sugar
+    double operator()(const ParameterMatrix<M>& P,
+                      OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
+      return apply(P, H);
+    }
+  };
+
+  // Vector version for MATLAB :-(
+  static double Derivative(double x, const Vector& p,  //
+                           OptionalJacobian</*1xN*/ -1, -1> H = boost::none) {
+    return DerivativeFunctor(x)(p.transpose(), H);
+  }
+};
+
+}  // namespace gtsam

gtsam/basis/BasisFactors.h

@@ -0,0 +1,424 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  BasisFactors.h
+ *  @brief Factor definitions for various Basis functors.
+ *  @author Varun Agrawal
+ *  @date July 4, 2020
+ **/
+
+#pragma once
+
+#include <gtsam/basis/Basis.h>
+#include <gtsam/nonlinear/FunctorizedFactor.h>
+
+namespace gtsam {
+
+/**
+ * @brief Factor for enforcing the scalar value of the polynomial BASIS
+ * representation at `x` is the same as the measurement `z` when using a
+ * pseudo-spectral parameterization.
+ *
+ * @tparam BASIS The basis class to use e.g. Chebyshev2
+ */
+template <class BASIS>
+class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor<double, Vector> {
+ private:
+  using Base = FunctorizedFactor<double, Vector>;
+
+ public:
+  EvaluationFactor() {}
+
+  /**
+   * @brief Construct a new EvaluationFactor object
+   *
+   * @param key Symbol for value to optimize.
+   * @param z The measurement value.
+   * @param model Noise model
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the polynomial.
+   */
+  EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model,
+                   const size_t N, double x)
+      : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {}
+
+  /**
+   * @brief Construct a new EvaluationFactor object
+   *
+   * @param key Symbol for value to optimize.
+   * @param z The measurement value.
+   * @param model Noise model
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model,
+                   const size_t N, double x, double a, double b)
+      : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {}
+
+  virtual ~EvaluationFactor() {}
+};
+
+/**
+ * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix
+ * of size (M, N) is equal to a vector-valued measurement at the same point,
+ when
+ * using a pseudo-spectral parameterization.
+ *
+ * This factors tries to enforce the basis function evaluation `f(x;p)` to take
+ * on the value `z` at location `x`, providing a gradient on the parameters p.
+ * In a probabilistic estimation context, `z` is known as a measurement, and the
+ * parameterized basis function can be seen as a
+ * measurement prediction function.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param M: Size of the evaluated state vector.
+ */
+template <class BASIS, int M>
+class GTSAM_EXPORT VectorEvaluationFactor
+    : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
+ private:
+  using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
+
+ public:
+  VectorEvaluationFactor() {}
+
+  /**
+   * @brief Construct a new VectorEvaluationFactor object.
+   *
+   * @param key The key to the ParameterMatrix object used to represent the
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  VectorEvaluationFactor(Key key, const Vector &z,
+                         const SharedNoiseModel &model, const size_t N,
+                         double x)
+      : Base(key, z, model,
+             typename BASIS::template VectorEvaluationFunctor<M>(N, x)) {}
+
+  /**
+   * @brief Construct a new VectorEvaluationFactor object.
+   *
+   * @param key The key to the ParameterMatrix object used to represent the
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  VectorEvaluationFactor(Key key, const Vector &z,
+                         const SharedNoiseModel &model, const size_t N,
+                         double x, double a, double b)
+      : Base(key, z, model,
+             typename BASIS::template VectorEvaluationFunctor<M>(N, x, a, b)) {}
+
+  virtual ~VectorEvaluationFactor() {}
+};
+
+/**
+ * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix
+ * of size (P, N) is equal to specified measurement at the same point, when
+ * using a pseudo-spectral parameterization.
+ *
+ * This factor is similar to `VectorEvaluationFactor` with the key difference
+ * being that it only enforces the constraint for a single scalar in the vector,
+ * indexed by `i`.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param P: Size of the fixed-size vector.
+ *
+ * Example:
+ *  VectorComponentFactor<BASIS, P> controlPrior(key, measured, model,
+ *                                               N, i, t, a, b);
+ *  where N is the degree and i is the component index.
+ */
+template <class BASIS, size_t P>
+class GTSAM_EXPORT VectorComponentFactor
+    : public FunctorizedFactor<double, ParameterMatrix<P>> {
+ private:
+  using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
+
+ public:
+  VectorComponentFactor() {}
+
+  /**
+   * @brief Construct a new VectorComponentFactor object.
+   *
+   * @param key The key to the ParameterMatrix object used to represent the
+   * polynomial.
+   * @param z The scalar value at a specified index `i` of the full measurement
+   * vector.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param i The index for the evaluated vector to give us the desired scalar
+   * value.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
+                        const size_t N, size_t i, double x)
+      : Base(key, z, model,
+             typename BASIS::template VectorComponentFunctor<P>(N, i, x)) {}
+
+  /**
+   * @brief Construct a new VectorComponentFactor object.
+   *
+   * @param key The key to the ParameterMatrix object used to represent the
+   * polynomial.
+   * @param z The scalar value at a specified index `i` of the full measurement
+   * vector.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param i The index for the evaluated vector to give us the desired scalar
+   * value.
+   * @param x The point at which to evaluate 0the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
+                        const size_t N, size_t i, double x, double a, double b)
+      : Base(
+            key, z, model,
+            typename BASIS::template VectorComponentFunctor<P>(N, i, x, a, b)) {
+  }
+
+  virtual ~VectorComponentFactor() {}
+};
+
+/**
+ * For a measurement value of type T i.e. `T z = g(x)`, this unary
+ * factor enforces that the polynomial basis, when evaluated at `x`, gives us
+ * the same `z`, i.e. `T z = g(x) = f(x;p)`.
+ *
+ * This is done via computations on the tangent space of the
+ * manifold of T.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param T: Object type which is synthesized by the provided functor.
+ *
+ * Example:
+ *  ManifoldEvaluationFactor<Chebyshev2, Rot3> rotationFactor(key, measurement,
+ * model, N, x, a, b);
+ *
+ * where `x` is the value (e.g. timestep) at which the rotation was evaluated.
+ */
+template <class BASIS, typename T>
+class GTSAM_EXPORT ManifoldEvaluationFactor
+    : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
+ private:
+  using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
+
+ public:
+  ManifoldEvaluationFactor() {}
+
+  /**
+   * @brief Construct a new ManifoldEvaluationFactor object.
+   *
+   * @param key Key for the state matrix parameterizing the function to estimate
+   * via the BASIS.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which the estimated function is evaluated.
+   */
+  ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
+                           const size_t N, double x)
+      : Base(key, z, model,
+             typename BASIS::template ManifoldEvaluationFunctor<T>(N, x)) {}
+
+  /**
+   * @brief Construct a new ManifoldEvaluationFactor object.
+   *
+   * @param key Key for the state matrix parameterizing the function to estimate
+   * via the BASIS.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which the estimated function is evaluated.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
+                           const size_t N, double x, double a, double b)
+      : Base(
+            key, z, model,
+            typename BASIS::template ManifoldEvaluationFunctor<T>(N, x, a, b)) {
+  }
+
+  virtual ~ManifoldEvaluationFactor() {}
+};
+
+/**
+ * A unary factor which enforces the evaluation of the derivative of a BASIS
+ * polynomial at a specified point`x` is equal to the scalar measurement `z`.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ */
+template <class BASIS>
+class GTSAM_EXPORT DerivativeFactor
+    : public FunctorizedFactor<double, typename BASIS::Parameters> {
+ private:
+  using Base = FunctorizedFactor<double, typename BASIS::Parameters>;
+
+ public:
+  DerivativeFactor() {}
+
+  /**
+   * @brief Construct a new DerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
+                   const size_t N, double x)
+      : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {}
+
+  /**
+   * @brief Construct a new DerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
+                   const size_t N, double x, double a, double b)
+      : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {}
+
+  virtual ~DerivativeFactor() {}
+};
+
+/**
+ * A unary factor which enforces the evaluation of the derivative of a BASIS
+ * polynomial at a specified point `x` is equal to the vector value `z`.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param M: Size of the evaluated state vector derivative.
+ */
+template <class BASIS, int M>
+class GTSAM_EXPORT VectorDerivativeFactor
+    : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
+ private:
+  using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
+  using Func = typename BASIS::template VectorDerivativeFunctor<M>;
+
+ public:
+  VectorDerivativeFactor() {}
+
+  /**
+   * @brief Construct a new VectorDerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  VectorDerivativeFactor(Key key, const Vector &z,
+                         const SharedNoiseModel &model, const size_t N,
+                         double x)
+      : Base(key, z, model, Func(N, x)) {}
+
+  /**
+   * @brief Construct a new VectorDerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The measurement value.
+   * @param model The noise model.
+   * @param N The degree of the polynomial.
+   * @param x The point at which to evaluate the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  VectorDerivativeFactor(Key key, const Vector &z,
+                         const SharedNoiseModel &model, const size_t N,
+                         double x, double a, double b)
+      : Base(key, z, model, Func(N, x, a, b)) {}
+
+  virtual ~VectorDerivativeFactor() {}
+};
+
+/**
+ * A unary factor which enforces the evaluation of the derivative of a BASIS
+ * polynomial is equal to the scalar value at a specific index `i` of a
+ * vector-valued measurement `z`.
+ *
+ * @param BASIS: The basis class to use e.g. Chebyshev2
+ * @param P: Size of the control component derivative.
+ */
+template <class BASIS, int P>
+class GTSAM_EXPORT ComponentDerivativeFactor
+    : public FunctorizedFactor<double, ParameterMatrix<P>> {
+ private:
+  using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
+  using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
+
+ public:
+  ComponentDerivativeFactor() {}
+
+  /**
+   * @brief Construct a new ComponentDerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The scalar measurement value at a specific index `i` of the full
+   * measurement vector.
+   * @param model The degree of the polynomial.
+   * @param N The degree of the polynomial.
+   * @param i The index for the evaluated vector to give us the desired scalar
+   * value.
+   * @param x The point at which to evaluate the basis polynomial.
+   */
+  ComponentDerivativeFactor(Key key, const double &z,
+                            const SharedNoiseModel &model, const size_t N,
+                            size_t i, double x)
+      : Base(key, z, model, Func(N, i, x)) {}
+
+  /**
+   * @brief Construct a new ComponentDerivativeFactor object.
+   *
+   * @param key The key to the ParameterMatrix which represents the basis
+   * polynomial.
+   * @param z The scalar measurement value at a specific index `i` of the full
+   * measurement vector.
+   * @param model The degree of the polynomial.
+   * @param N The degree of the polynomial.
+   * @param i The index for the evaluated vector to give us the desired scalar
+   * value.
+   * @param x The point at which to evaluate the basis polynomial.
+   * @param a Lower bound for the polynomial.
+   * @param b Upper bound for the polynomial.
+   */
+  ComponentDerivativeFactor(Key key, const double &z,
+                            const SharedNoiseModel &model, const size_t N,
+                            size_t i, double x, double a, double b)
+      : Base(key, z, model, Func(N, i, x, a, b)) {}
+
+  virtual ~ComponentDerivativeFactor() {}
+};
+
+}  // namespace gtsam

gtsam/basis/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Install headers
+file(GLOB basis_headers "*.h")
+install(FILES ${basis_headers} DESTINATION include/gtsam/basis)
+
+# Build tests
+add_subdirectory(tests)

gtsam/basis/Chebyshev.cpp

@@ -0,0 +1,98 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Chebyshev.cpp
+ * @brief Chebyshev basis decompositions
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#include <gtsam/basis/Chebyshev.h>
+
+namespace gtsam {
+
+/**
+ * @brief Scale x from [a, b] to [t1, t2]
+ *
+ * ((b'-a') * (x - a) / (b - a)) + a'
+ *
+ * @param x Value to scale to new range.
+ * @param a Original lower limit.
+ * @param b Original upper limit.
+ * @param t1 New lower limit.
+ * @param t2 New upper limit.
+ * @return double
+ */
+static double scale(double x, double a, double b, double t1, double t2) {
+  return ((t2 - t1) * (x - a) / (b - a)) + t1;
+}
+
+Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a,
+                                          double b) {
+  Weights Tx(1, N);
+
+  x = scale(x, a, b, -1, 1);
+
+  Tx(0) = 1;
+  Tx(1) = x;
+  for (size_t i = 2; i < N; i++) {
+    // instead of cos(i*acos(x)), this recurrence is much faster
+    Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2);
+  }
+  return Tx;
+}
+
+Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a,
+                                           double b) {
+  Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b);
+  Weights weights = Weights::Zero(N);
+  for (size_t n = 1; n < N; n++) {
+    weights(n) = n * Ux(n - 1);
+  }
+  return weights;
+}
+
+Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a,
+                                          double b) {
+  Weights Ux(N);
+
+  x = scale(x, a, b, -1, 1);
+
+  Ux(0) = 1;
+  Ux(1) = 2 * x;
+  for (size_t i = 2; i < N; i++) {
+    // instead of cos(i*acos(x)), this recurrence is much faster
+    Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2);
+  }
+  return Ux;
+}
+
+Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a,
+                                           double b) {
+  Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b);
+  Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b);
+
+  Weights weights(N);
+
+  x = scale(x, a, b, -1, 1);
+  if (x == -1 || x == 1) {
+    throw std::runtime_error(
+        "Derivative of Chebyshev2 Basis does not exist at range limits.");
+  }
+
+  for (size_t n = 0; n < N; n++) {
+    weights(n) = ((n + 1) * Tx(n + 1) - x * Ux(n)) / (x * x - 1);
+  }
+  return weights;
+}
+
+}  // namespace gtsam

gtsam/basis/Chebyshev.h

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Chebyshev.h
+ * @brief Chebyshev basis decompositions
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#pragma once
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/basis/Basis.h>
+
+#include <unsupported/Eigen/KroneckerProduct>
+
+namespace gtsam {
+
+/**
+ * Basis of Chebyshev polynomials of the first kind
+ * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind
+ * These are typically denoted with the symbol T_n, where n is the degree.
+ * The parameter N is the number of coefficients, i.e., N = n+1.
+ */
+struct Chebyshev1Basis : Basis<Chebyshev1Basis> {
+  using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
+
+  Parameters parameters_;
+
+  /**
+   * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values)
+   *
+   * @param N Degree of the polynomial.
+   * @param x Point to evaluate polynomial at.
+   * @param a Lower limit of polynomial (default=-1).
+   * @param b Upper limit of polynomial (default=1).
+   */
+  static Weights CalculateWeights(size_t N, double x, double a = -1,
+                                  double b = 1);
+
+  /**
+   * @brief Evaluate Chebyshev derivative at x.
+   * The derivative weights are pre-multiplied to the polynomial Parameters.
+   *
+   * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x)
+   * I.e. the derivative fo a first kind cheb is just a second kind cheb
+   * So, we define a second kind basis here of order N-1
+   * Note that it has one less weight.
+   *
+   * The Parameters pertain to 1st kind chebs up to order N-1
+   * But of course the first one (order 0) is constant, so omit that weight.
+   *
+   * @param N Degree of the polynomial.
+   * @param x Point to evaluate polynomial at.
+   * @param a Lower limit of polynomial (default=-1).
+   * @param b Upper limit of polynomial (default=1).
+   * @return Weights
+   */
+  static Weights DerivativeWeights(size_t N, double x, double a = -1,
+                                   double b = 1);
+};  // Chebyshev1Basis
+
+/**
+ * Basis of Chebyshev polynomials of the second kind.
+ * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind
+ * These are typically denoted with the symbol U_n, where n is the degree.
+ * The parameter N is the number of coefficients, i.e., N = n+1.
+ * In contrast to the templates in Chebyshev2, the classes below specify
+ * basis functions, weighted combinations of which are used to approximate
+ * functions. In this sense, they are like the sines and cosines of the Fourier
+ * basis.
+ */
+struct Chebyshev2Basis : Basis<Chebyshev2Basis> {
+  using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
+
+  /**
+   *  Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values).
+   *
+   * @param N Degree of the polynomial.
+   * @param x Point to evaluate polynomial at.
+   * @param a Lower limit of polynomial (default=-1).
+   * @param b Upper limit of polynomial (default=1).
+   */
+  static Weights CalculateWeights(size_t N, double x, double a = -1,
+                                  double b = 1);
+
+  /**
+   * @brief Evaluate Chebyshev derivative at x.
+   *
+   * @param N Degree of the polynomial.
+   * @param x Point to evaluate polynomial at.
+   * @param a Lower limit of polynomial (default=-1).
+   * @param b Upper limit of polynomial (default=1).
+   * @return Weights
+   */
+  static Weights DerivativeWeights(size_t N, double x, double a = -1,
+                                   double b = 1);
+};  // Chebyshev2Basis
+
+}  // namespace gtsam

gtsam/basis/Chebyshev2.cpp

@@ -0,0 +1,205 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Chebyshev2.cpp
+ * @brief Chebyshev parameterizations on Chebyshev points of second kind
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#include <gtsam/basis/Chebyshev2.h>
+
+namespace gtsam {
+
+Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
+  // Allocate space for weights
+  Weights weights(N);
+
+  // We start by getting distances from x to all Chebyshev points
+  // as well as getting smallest distance
+  Weights distances(N);
+
+  for (size_t j = 0; j < N; j++) {
+    const double dj =
+        x - Point(N, j, a, b);  // only thing that depends on [a,b]
+
+    if (std::abs(dj) < 1e-10) {
+      // exceptional case: x coincides with a Chebyshev point
+      weights.setZero();
+      weights(j) = 1;
+      return weights;
+    }
+    distances(j) = dj;
+  }
+
+  // Beginning of interval, j = 0, x(0) = a
+  weights(0) = 0.5 / distances(0);
+
+  // All intermediate points j=1:N-2
+  double d = weights(0), s = -1;  // changes sign s at every iteration
+  for (size_t j = 1; j < N - 1; j++, s = -s) {
+    weights(j) = s / distances(j);
+    d += weights(j);
+  }
+
+  // End of interval, j = N-1, x(N-1) = b
+  weights(N - 1) = 0.5 * s / distances(N - 1);
+  d += weights(N - 1);
+
+  // normalize
+  return weights / d;
+}
+
+Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
+  // Allocate space for weights
+  Weights weightDerivatives(N);
+
+  // toggle variable so we don't need to use `pow` for -1
+  double t = -1;
+
+  // We start by getting distances from x to all Chebyshev points
+  // as well as getting smallest distance
+  Weights distances(N);
+
+  for (size_t j = 0; j < N; j++) {
+    const double dj =
+        x - Point(N, j, a, b);  // only thing that depends on [a,b]
+    if (std::abs(dj) < 1e-10) {
+      // exceptional case: x coincides with a Chebyshev point
+      weightDerivatives.setZero();
+      // compute the jth row of the differentiation matrix for this point
+      double cj = (j == 0 || j == N - 1) ? 2. : 1.;
+      for (size_t k = 0; k < N; k++) {
+        if (j == 0 && k == 0) {
+          // we reverse the sign since we order the cheb points from -1 to 1
+          weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0;
+        } else if (j == N - 1 && k == N - 1) {
+          // we reverse the sign since we order the cheb points from -1 to 1
+          weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0;
+        } else if (k == j) {
+          double xj = Point(N, j);
+          double xj2 = xj * xj;
+          weightDerivatives(k) = -0.5 * xj / (1 - xj2);
+        } else {
+          double xj = Point(N, j);
+          double xk = Point(N, k);
+          double ck = (k == 0 || k == N - 1) ? 2. : 1.;
+          t = ((j + k) % 2) == 0 ? 1 : -1;
+          weightDerivatives(k) = (cj / ck) * t / (xj - xk);
+        }
+      }
+      return 2 * weightDerivatives / (b - a);
+    }
+    distances(j) = dj;
+  }
+
+  // This section of code computes the derivative of
+  // the Barycentric Interpolation weights formula by applying
+  // the chain rule on the original formula.
+
+  // g and k are multiplier terms which represent the derivatives of
+  // the numerator and denominator
+  double g = 0, k = 0;
+  double w = 1;
+
+  for (size_t j = 0; j < N; j++) {
+    if (j == 0 || j == N - 1) {
+      w = 0.5;
+    } else {
+      w = 1.0;
+    }
+
+    t = (j % 2 == 0) ? 1 : -1;
+
+    double c = t / distances(j);
+    g += w * c;
+    k += (w * c / distances(j));
+  }
+
+  double s = 1;  // changes sign s at every iteration
+  double g2 = g * g;
+
+  for (size_t j = 0; j < N; j++, s = -s) {
+    // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1,
+    // x0 = 1.0
+    if (j == 0 || j == N - 1) {
+      w = 0.5;
+    } else {
+      // All intermediate points j=1:N-2
+      w = 1.0;
+    }
+    weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) -
+                           (w * -s * k / (g2 * distances(j)));
+  }
+
+  return weightDerivatives;
+}
+
+Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a,
+                                                         double b) {
+  DiffMatrix D(N, N);
+  if (N == 1) {
+    D(0, 0) = 1;
+    return D;
+  }
+
+  // toggle variable so we don't need to use `pow` for -1
+  double t = -1;
+
+  for (size_t i = 0; i < N; i++) {
+    double xi = Point(N, i);
+    double ci = (i == 0 || i == N - 1) ? 2. : 1.;
+    for (size_t j = 0; j < N; j++) {
+      if (i == 0 && j == 0) {
+        // we reverse the sign since we order the cheb points from -1 to 1
+        D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0;
+      } else if (i == N - 1 && j == N - 1) {
+        // we reverse the sign since we order the cheb points from -1 to 1
+        D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0;
+      } else if (i == j) {
+        double xi2 = xi * xi;
+        D(i, j) = -xi / (2 * (1 - xi2));
+      } else {
+        double xj = Point(N, j);
+        double cj = (j == 0 || j == N - 1) ? 2. : 1.;
+        t = ((i + j) % 2) == 0 ? 1 : -1;
+        D(i, j) = (ci / cj) * t / (xi - xj);
+      }
+    }
+  }
+  // scale the matrix to the range
+  return D / ((b - a) / 2.0);
+}
+
+Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
+  // Allocate space for weights
+  Weights weights(N);
+  size_t K = N - 1,  // number of intervals between N points
+      K2 = K * K;
+  weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1);
+  weights(N - 1) = weights(0);
+
+  size_t last_k = K / 2 + K % 2 - 1;
+
+  for (size_t i = 1; i <= N - 2; ++i) {
+    double theta = i * M_PI / K;
+    weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1;
+
+    for (size_t k = 1; k <= last_k; ++k)
+      weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1);
+    weights(i) *= (b - a) / K;
+  }
+
+  return weights;
+}
+
+}  // namespace gtsam

gtsam/basis/Chebyshev2.h

@@ -0,0 +1,148 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Chebyshev2.h
+ * @brief Pseudo-spectral parameterization for Chebyshev polynomials of the
+ * second kind.
+ *
+ * In a pseudo-spectral case, rather than the parameters acting as
+ * weights for the bases polynomials (as in Chebyshev2Basis), here the
+ * parameters are the *values* at a specific set of points in the interval, the
+ * "Chebyshev points". These values uniquely determine the polynomial that
+ * interpolates them at the Chebyshev points.
+ *
+ * This is different from Chebyshev.h since it leverage ideas from
+ * pseudo-spectral optimization, i.e. we don't decompose into basis functions,
+ * rather estimate function parameters that enforce function nodes at Chebyshev
+ * points.
+ *
+ * Please refer to Agrawal21icra for more details.
+ *
+ * @author Varun Agrawal, Jing Dong, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#pragma once
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/basis/Basis.h>
+
+#include <boost/function.hpp>
+
+namespace gtsam {
+
+/**
+ * Chebyshev Interpolation on Chebyshev points of the second kind
+ * Note that N here, the number of points, is one less than N from
+ * 'Approximation Theory and Approximation Practice by L. N. Trefethen (pg.42)'.
+ */
+class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  using Base = Basis<Chebyshev2>;
+  using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
+  using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
+
+  /// Specific Chebyshev point
+  static double Point(size_t N, int j) {
+    assert(j >= 0 && size_t(j) < N);
+    const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
+    // We add -PI so that we get values ordered from -1 to +1
+    // sin(- M_PI_2 + dtheta*j); also works
+    return cos(-M_PI + dtheta * j);
+  }
+
+  /// Specific Chebyshev point, within [a,b] interval
+  static double Point(size_t N, int j, double a, double b) {
+    assert(j >= 0 && size_t(j) < N);
+    const double dtheta = M_PI / (N - 1);
+    // We add -PI so that we get values ordered from -1 to +1
+    return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
+  }
+
+  /// All Chebyshev points
+  static Vector Points(size_t N) {
+    Vector points(N);
+    for (size_t j = 0; j < N; j++) points(j) = Point(N, j);
+    return points;
+  }
+
+  /// All Chebyshev points, within [a,b] interval
+  static Vector Points(size_t N, double a, double b) {
+    Vector points = Points(N);
+    const double T1 = (a + b) / 2, T2 = (b - a) / 2;
+    points = T1 + (T2 * points).array();
+    return points;
+  }
+
+  /**
+   * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values)
+   * These weights implement barycentric interpolation at a specific x.
+   * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the
+   * values of the function f at the Chebyshev points. As such, for a given x we
+   * obtain a linear map from parameter vectors f to interpolated values f(x).
+   * Optional [a,b] interval can be specified as well.
+   */
+  static Weights CalculateWeights(size_t N, double x, double a = -1,
+                                  double b = 1);
+
+  /**
+   *  Evaluate derivative of barycentric weights.
+   *  This is easy and efficient via the DifferentiationMatrix.
+   */
+  static Weights DerivativeWeights(size_t N, double x, double a = -1,
+                                   double b = 1);
+
+  /// compute D = differentiation matrix, Trefethen00book p.53
+  /// when given a parameter vector f of function values at the Chebyshev
+  /// points, D*f are the values of f'.
+  /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4
+  static DiffMatrix DifferentiationMatrix(size_t N, double a = -1,
+                                          double b = 1);
+
+  /**
+   *  Evaluate Clenshaw-Curtis integration weights.
+   *  Trefethen00book, pg 128, clencurt.m
+   *  Note that N in clencurt.m is 1 less than our N
+   *  K = N-1;
+      theta = pi*(0:K)'/K;
+      w = zeros(1,N); ii = 2:K; v = ones(K-1, 1);
+      if mod(K,2) == 0
+          w(1) = 1/(K^2-1); w(N) = w(1);
+          for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
+          v = v - cos(K*theta(ii))/(K^2-1);
+      else
+          w(1) = 1/K^2; w(N) = w(1);
+          for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end
+      end
+      w(ii) = 2*v/K;
+
+   */
+  static Weights IntegrationWeights(size_t N, double a = -1, double b = 1);
+
+  /**
+   * Create matrix of values at Chebyshev points given vector-valued function.
+   */
+  template <size_t M>
+  static Matrix matrix(boost::function<Eigen::Matrix<double, M, 1>(double)> f,
+                       size_t N, double a = -1, double b = 1) {
+    Matrix Xmat(M, N);
+    for (size_t j = 0; j < N; j++) {
+      Xmat.col(j) = f(Point(N, j, a, b));
+    }
+    return Xmat;
+  }
+};  // \ Chebyshev2
+
+}  // namespace gtsam

gtsam/basis/FitBasis.h

@@ -0,0 +1,99 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 FitBasis.h
+ * @date July 4, 2020
+ * @author Varun Agrawal, Frank Dellaert
+ * @brief Fit a Basis using least-squares
+ */
+
+/*
+ *  Concept needed for LS. Parameters = Coefficients | Values
+ *    - Parameters, Jacobian
+ *    - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H)
+ */
+
+#pragma once
+
+#include <gtsam/basis/Basis.h>
+#include <gtsam/basis/BasisFactors.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+namespace gtsam {
+
+/// Our sequence representation is a map of {x: y} values where y = f(x)
+using Sequence = std::map<double, double>;
+/// A sample is a key-value pair from a sequence.
+using Sample = std::pair<double, double>;
+
+/**
+ * Class that does regression via least squares
+ * Example usage:
+ *  size_t N = 3;
+ *  auto fit = FitBasis<Chebyshev2>(data_points, noise_model, N);
+ *  Vector coefficients = fit.parameters();
+ *
+ * where `data_points` are a map from `x` to `y` values indicating a function
+ * mapping at specific points, `noise_model` is the gaussian noise model, and
+ * `N` is the degree of the polynomial basis used to fit the function.
+ */
+template <class Basis>
+class FitBasis {
+ public:
+  using Parameters = typename Basis::Parameters;
+
+ private:
+  Parameters parameters_;
+
+ public:
+  /// Create nonlinear FG from Sequence
+  static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence,
+                                             const SharedNoiseModel& model,
+                                             size_t N) {
+    NonlinearFactorGraph graph;
+    for (const Sample sample : sequence) {
+      graph.emplace_shared<EvaluationFactor<Basis>>(0, sample.second, model, N,
+                                                    sample.first);
+    }
+    return graph;
+  }
+
+  /// Create linear FG from Sequence
+  static GaussianFactorGraph::shared_ptr LinearGraph(
+      const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
+    NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N);
+    Values values;
+    values.insert<Parameters>(0, Parameters::Zero(N));
+    GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
+    return gfg;
+  }
+
+  /**
+   * @brief Construct a new FitBasis object.
+   *
+   * @param sequence map of x->y values for a function, a.k.a. y = f(x).
+   * @param model The noise model to use.
+   * @param N The degree of the polynomial to fit.
+   */
+  FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
+    GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N);
+    VectorValues solution = gfg->optimize();
+    parameters_ = solution.at(0);
+  }
+
+  /// Return Fourier coefficients
+  Parameters parameters() const { return parameters_; }
+};
+
+}  // namespace gtsam

gtsam/basis/Fourier.h

@@ -0,0 +1,108 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Fourier.h
+ * @brief Fourier decomposition, see e.g.
+ *        http://mathworld.wolfram.com/FourierSeries.html
+ * @author Varun Agrawal, Frank Dellaert
+ * @date July 4, 2020
+ */
+
+#pragma once
+
+#include <gtsam/basis/Basis.h>
+
+namespace gtsam {
+
+/// Fourier basis
+class GTSAM_EXPORT FourierBasis : public Basis<FourierBasis> {
+ public:
+  using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
+  using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
+
+  /**
+   * @brief Evaluate Real Fourier Weights of size N in interval [a, b],
+   *        e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x)
+   *
+   * @param N The degree of the polynomial to use.
+   * @param x The point at which to compute the derivaive weights.
+   * @return Weights
+   */
+  static Weights CalculateWeights(size_t N, double x) {
+    Weights b(N);
+    b[0] = 1;
+    for (size_t i = 1, n = 1; i < N; i += 2, n++) {
+      b[i] = cos(n * x);
+      b[i + 1] = sin(n * x);
+    }
+    return b;
+  }
+
+  /**
+   * @brief Evaluate Real Fourier Weights of size N in interval [a, b],
+   *        e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x)
+   *
+   * @param N The degree of the polynomial to use.
+   * @param x The point at which to compute the weights.
+   * @param a Lower bound of interval.
+   * @param b Upper bound of interval.
+   * @return Weights
+   */
+  static Weights CalculateWeights(size_t N, double x, double a, double b) {
+    // TODO(Varun) How do we enforce an interval for Fourier series?
+    return CalculateWeights(N, x);
+  }
+
+  /**
+   * Compute D = differentiation matrix.
+   * Given coefficients c of a Fourier series c, D*c are the values of c'.
+   */
+  static DiffMatrix DifferentiationMatrix(size_t N) {
+    DiffMatrix D = DiffMatrix::Zero(N, N);
+    double k = 1;
+    for (size_t i = 1; i < N; i += 2) {
+      D(i, i + 1) = k;   // sin'(k*x) = k*cos(k*x)
+      D(i + 1, i) = -k;  // cos'(k*x) = -k*sin(k*x)
+      k += 1;
+    }
+
+    return D;
+  }
+
+  /**
+   * @brief Get weights at a given x that calculate the derivative.
+   *
+   * @param N The degree of the polynomial to use.
+   * @param x The point at which to compute the derivaive weights.
+   * @return Weights
+   */
+  static Weights DerivativeWeights(size_t N, double x) {
+    return CalculateWeights(N, x) * DifferentiationMatrix(N);
+  }
+
+  /**
+   * @brief Get derivative weights at a given x that calculate the derivative,
+  in the interval [a, b].
+   *
+   * @param N The degree of the polynomial to use.
+   * @param x The point at which to compute the derivaive weights.
+   * @param a Lower bound of interval.
+   * @param b Upper bound of interval.
+   * @return Weights
+   */
+  static Weights DerivativeWeights(size_t N, double x, double a, double b) {
+    return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N);
+  }
+
+};  // FourierBasis
+
+}  // namespace gtsam

gtsam/basis/ParameterMatrix.h

@@ -0,0 +1,215 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 ParamaterMatrix.h
+ * @brief Define ParameterMatrix class which is used to store values at
+ * interpolation points.
+ * @author Varun Agrawal, Frank Dellaert
+ * @date September 21, 2020
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/VectorSpace.h>
+
+#include <iostream>
+
+namespace gtsam {
+
+/**
+ * A matrix abstraction of MxN values at the Basis points.
+ * This class serves as a wrapper over an Eigen matrix.
+ * @tparam M: The dimension of the type you wish to evaluate.
+ * @param N: the number of Basis points (e.g. Chebyshev points of the second
+ * kind).
+ */
+template <int M>
+class ParameterMatrix {
+  using MatrixType = Eigen::Matrix<double, M, -1>;
+
+ private:
+  MatrixType matrix_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum { dimension = Eigen::Dynamic };
+
+  /**
+   * Create ParameterMatrix using the number of basis points.
+   * @param N: The number of basis points (the columns).
+   */
+  ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); }
+
+  /**
+   * Create ParameterMatrix from an MxN Eigen Matrix.
+   * @param matrix: An Eigen matrix used to initialze the ParameterMatrix.
+   */
+  ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {}
+
+  /// Get the number of rows.
+  size_t rows() const { return matrix_.rows(); }
+
+  /// Get the number of columns.
+  size_t cols() const { return matrix_.cols(); }
+
+  /// Get the underlying matrix.
+  MatrixType matrix() const { return matrix_; }
+
+  /// Return the tranpose of the underlying matrix.
+  Eigen::Matrix<double, -1, M> transpose() const { return matrix_.transpose(); }
+
+  /**
+   * Get the matrix row specified by `index`.
+   * @param index: The row index to retrieve.
+   */
+  Eigen::Matrix<double, 1, -1> row(size_t index) const {
+    return matrix_.row(index);
+  }
+
+  /**
+   * Set the matrix row specified by `index`.
+   * @param index: The row index to set.
+   */
+  auto row(size_t index) -> Eigen::Block<MatrixType, 1, -1, false> {
+    return matrix_.row(index);
+  }
+
+  /**
+   * Get the matrix column specified by `index`.
+   * @param index: The column index to retrieve.
+   */
+  Eigen::Matrix<double, M, 1> col(size_t index) const {
+    return matrix_.col(index);
+  }
+
+  /**
+   * Set the matrix column specified by `index`.
+   * @param index: The column index to set.
+   */
+  auto col(size_t index) -> Eigen::Block<MatrixType, M, 1, true> {
+    return matrix_.col(index);
+  }
+
+  /**
+   * Set all matrix coefficients to zero.
+   */
+  void setZero() { matrix_.setZero(); }
+
+  /**
+   * Add a ParameterMatrix to another.
+   * @param other: ParameterMatrix to add.
+   */
+  ParameterMatrix<M> operator+(const ParameterMatrix<M>& other) const {
+    return ParameterMatrix<M>(matrix_ + other.matrix());
+  }
+
+  /**
+   * Add a MxN-sized vector to the ParameterMatrix.
+   * @param other: Vector which is reshaped and added.
+   */
+  ParameterMatrix<M> operator+(
+      const Eigen::Matrix<double, -1, 1>& other) const {
+    // This form avoids a deep copy and instead typecasts `other`.
+    Eigen::Map<const MatrixType> other_(other.data(), M, cols());
+    return ParameterMatrix<M>(matrix_ + other_);
+  }
+
+  /**
+   * Subtract a ParameterMatrix from another.
+   * @param other: ParameterMatrix to subtract.
+   */
+  ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
+    return ParameterMatrix<M>(matrix_ - other.matrix());
+  }
+
+  /**
+   * Subtract a MxN-sized vector from the ParameterMatrix.
+   * @param other: Vector which is reshaped and subracted.
+   */
+  ParameterMatrix<M> operator-(
+      const Eigen::Matrix<double, -1, 1>& other) const {
+    Eigen::Map<const MatrixType> other_(other.data(), M, cols());
+    return ParameterMatrix<M>(matrix_ - other_);
+  }
+
+  /**
+   * Multiply ParameterMatrix with an Eigen matrix.
+   * @param other: Eigen matrix which should be multiplication compatible with
+   * the ParameterMatrix.
+   */
+  MatrixType operator*(const Eigen::Matrix<double, -1, -1>& other) const {
+    return matrix_ * other;
+  }
+
+  /// @name Vector Space requirements, following LieMatrix
+  /// @{
+
+  /**
+   * Print the ParameterMatrix.
+   * @param s: The prepend string to add more contextual info.
+   */
+  void print(const std::string& s = "") const {
+    std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl;
+  }
+
+  /**
+   * Check for equality up to absolute tolerance.
+   * @param other: The ParameterMatrix to check equality with.
+   * @param tol: The absolute tolerance threshold.
+   */
+  bool equals(const ParameterMatrix<M>& other, double tol = 1e-8) const {
+    return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol);
+  }
+
+  /// Returns dimensionality of the tangent space
+  inline size_t dim() const { return matrix_.size(); }
+
+  /// Convert to vector form, is done row-wise
+  inline Vector vector() const {
+    using RowMajor = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;
+    Vector result(matrix_.size());
+    Eigen::Map<RowMajor>(&result(0), rows(), cols()) = matrix_;
+    return result;
+  }
+
+  /** Identity function to satisfy VectorSpace traits.
+   *
+   * NOTE: The size at compile time is unknown so this identity is zero
+   * length and thus not valid.
+   */
+  inline static ParameterMatrix identity() {
+    // throw std::runtime_error(
+    //     "ParameterMatrix::identity(): Don't use this function");
+    return ParameterMatrix(0);
+  }
+
+  /// @}
+};
+
+// traits for ParameterMatrix
+template <int M>
+struct traits<ParameterMatrix<M>>
+    : public internal::VectorSpace<ParameterMatrix<M>> {};
+
+/* ************************************************************************* */
+// Stream operator that takes a ParameterMatrix. Used for printing.
+template <int M>
+inline std::ostream& operator<<(std::ostream& os,
+                                const ParameterMatrix<M>& parameterMatrix) {
+  os << parameterMatrix.matrix();
+  return os;
+}
+
+}  // namespace gtsam

gtsam/basis/basis.i

@@ -0,0 +1,146 @@
+//*************************************************************************
+// basis
+//*************************************************************************
+
+namespace gtsam {
+
+// TODO(gerry): add all the Functors to the Basis interfaces, e.g.
+// `EvaluationFunctor`
+
+#include <gtsam/basis/Fourier.h>
+
+class FourierBasis {
+  static Vector CalculateWeights(size_t N, double x);
+  static Matrix WeightMatrix(size_t N, Vector x);
+
+  static Matrix DifferentiationMatrix(size_t N);
+  static Vector DerivativeWeights(size_t N, double x);
+};
+
+#include <gtsam/basis/Chebyshev.h>
+
+class Chebyshev1Basis {
+  static Matrix CalculateWeights(size_t N, double x);
+  static Matrix WeightMatrix(size_t N, Vector X);
+};
+
+class Chebyshev2Basis {
+  static Matrix CalculateWeights(size_t N, double x);
+  static Matrix WeightMatrix(size_t N, Vector x);
+};
+
+#include <gtsam/basis/Chebyshev2.h>
+class Chebyshev2 {
+  static double Point(size_t N, int j);
+  static double Point(size_t N, int j, double a, double b);
+
+  static Vector Points(size_t N);
+  static Vector Points(size_t N, double a, double b);
+
+  static Matrix WeightMatrix(size_t N, Vector X);
+  static Matrix WeightMatrix(size_t N, Vector X, double a, double b);
+
+  static Matrix CalculateWeights(size_t N, double x, double a, double b);
+  static Matrix DerivativeWeights(size_t N, double x, double a, double b);
+  static Matrix IntegrationWeights(size_t N, double a, double b);
+  static Matrix DifferentiationMatrix(size_t N, double a, double b);
+
+  // TODO Needs OptionalJacobian
+  // static double Derivative(double x, Vector f);
+};
+
+#include <gtsam/basis/ParameterMatrix.h>
+
+template <M = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}>
+class ParameterMatrix {
+  ParameterMatrix(const size_t N);
+  ParameterMatrix(const Matrix& matrix);
+
+  Matrix matrix() const;
+
+  void print(const string& s = "") const;
+};
+
+#include <gtsam/basis/BasisFactors.h>
+
+template <BASIS = {gtsam::Chebyshev2, gtsam::Chebyshev1Basis,
+                   gtsam::Chebyshev2Basis, gtsam::FourierBasis}>
+virtual class EvaluationFactor : gtsam::NoiseModelFactor {
+  EvaluationFactor();
+  EvaluationFactor(gtsam::Key key, const double z,
+                   const gtsam::noiseModel::Base* model, const size_t N,
+                   double x);
+  EvaluationFactor(gtsam::Key key, const double z,
+                   const gtsam::noiseModel::Base* model, const size_t N,
+                   double x, double a, double b);
+};
+
+template <BASIS, M>
+virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor {
+  VectorEvaluationFactor();
+  VectorEvaluationFactor(gtsam::Key key, const Vector& z,
+                         const gtsam::noiseModel::Base* model, const size_t N,
+                         double x);
+  VectorEvaluationFactor(gtsam::Key key, const Vector& z,
+                         const gtsam::noiseModel::Base* model, const size_t N,
+                         double x, double a, double b);
+};
+
+// TODO(Varun) Better way to support arbitrary dimensions?
+// Especially if users mainly do `pip install gtsam` for the Python wrapper.
+typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 3>
+    VectorEvaluationFactorChebyshev2D3;
+typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 4>
+    VectorEvaluationFactorChebyshev2D4;
+typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 12>
+    VectorEvaluationFactorChebyshev2D12;
+
+template <BASIS, P>
+virtual class VectorComponentFactor : gtsam::NoiseModelFactor {
+  VectorComponentFactor();
+  VectorComponentFactor(gtsam::Key key, const double z,
+                        const gtsam::noiseModel::Base* model, const size_t N,
+                        size_t i, double x);
+  VectorComponentFactor(gtsam::Key key, const double z,
+                        const gtsam::noiseModel::Base* model, const size_t N,
+                        size_t i, double x, double a, double b);
+};
+
+typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 3>
+    VectorComponentFactorChebyshev2D3;
+typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 4>
+    VectorComponentFactorChebyshev2D4;
+typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 12>
+    VectorComponentFactorChebyshev2D12;
+
+template <BASIS, T>
+virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor {
+  ManifoldEvaluationFactor();
+  ManifoldEvaluationFactor(gtsam::Key key, const T& z,
+                           const gtsam::noiseModel::Base* model, const size_t N,
+                           double x);
+  ManifoldEvaluationFactor(gtsam::Key key, const T& z,
+                           const gtsam::noiseModel::Base* model, const size_t N,
+                           double x, double a, double b);
+};
+
+// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and
+// `ComponentDerivativeFactor`
+
+#include <gtsam/basis/FitBasis.h>
+template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
+                   gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
+class FitBasis {
+  FitBasis(const std::map<double, double>& sequence,
+           const gtsam::noiseModel::Base* model, size_t N);
+
+  static gtsam::NonlinearFactorGraph NonlinearGraph(
+      const std::map<double, double>& sequence,
+      const gtsam::noiseModel::Base* model, size_t N);
+  static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
+      const std::map<double, double>& sequence,
+      const gtsam::noiseModel::Base* model, size_t N);
+  Parameters parameters() const;
+};
+
+}  // namespace gtsam

gtsam/basis/tests/CMakeLists.txt

@@ -0,0 +1 @@
+gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam")

gtsam/basis/tests/testChebyshev.cpp

@@ -0,0 +1,236 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testChebyshev.cpp
+ * @date July 4, 2020
+ * @author Varun Agrawal
+ * @brief Unit tests for Chebyshev Basis Decompositions
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/basis/Chebyshev.h>
+#include <gtsam/basis/FitBasis.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+using namespace std;
+using namespace gtsam;
+
+auto model = noiseModel::Unit::Create(1);
+
+const size_t N = 3;
+
+//******************************************************************************
+TEST(Chebyshev, Chebyshev1) {
+  using Synth = Chebyshev1Basis::EvaluationFunctor;
+  Vector c(N);
+  double x;
+  c << 12, 3, 1;
+  x = -1.0;
+  EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9);
+  x = -0.5;
+  EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9);
+  x = 0.3;
+  EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9);
+}
+
+//******************************************************************************
+TEST(Chebyshev, Chebyshev2) {
+  using Synth = Chebyshev2Basis::EvaluationFunctor;
+  Vector c(N);
+  double x;
+  c << 12, 3, 1;
+  x = -1.0;
+  EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9);
+  x = -0.5;
+  EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9);
+  x = 0.3;
+  EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9);
+}
+
+//******************************************************************************
+TEST(Chebyshev, Evaluation) {
+  Chebyshev1Basis::EvaluationFunctor fx(N, 0.5);
+  Vector c(N);
+  c << 3, 5, -12;
+  EXPECT_DOUBLES_EQUAL(11.5, fx(c), 1e-9);
+}
+
+//******************************************************************************
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+TEST(Chebyshev, Expression) {
+  // Create linear factor graph
+  NonlinearFactorGraph graph;
+  Key key(1);
+
+  // Let's pretend we have 6 GPS measurements (we just do x coordinate)
+  // at times
+  const size_t m = 6;
+  Vector t(m);
+  t << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9;
+  Vector x(m);
+  x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9;
+
+  for (size_t i = 0; i < m; i++) {
+    graph.emplace_shared<EvaluationFactor<Chebyshev1Basis>>(key, x(i), model, N,
+                                                            t(i));
+  }
+
+  // Solve
+  Values initial;
+  initial.insert<Vector>(key, Vector::Zero(N));  // initial does not matter
+
+  // ... and optimize
+  GaussNewtonParams parameters;
+  GaussNewtonOptimizer optimizer(graph, initial, parameters);
+  Values result = optimizer.optimize();
+
+  // Check
+  Vector expected(N);
+  expected << 0, 1, 0;
+  Vector actual_c = result.at<Vector>(key);
+  EXPECT(assert_equal(expected, actual_c));
+
+  // Calculate and print covariances
+  Marginals marginals(graph, result);
+  Matrix3 cov = marginals.marginalCovariance(key);
+  EXPECT_DOUBLES_EQUAL(0.626, cov(1, 1), 1e-3);
+
+  // Predict x at time 1.0
+  Chebyshev1Basis::EvaluationFunctor f(N, 1.0);
+  Matrix H;
+  double actual = f(actual_c, H);
+  EXPECT_DOUBLES_EQUAL(1.0, actual, 1e-9);
+
+  // Calculate predictive variance on prediction
+  double actual_variance_on_prediction = (H * cov * H.transpose())(0);
+  EXPECT_DOUBLES_EQUAL(1.1494, actual_variance_on_prediction, 1e-4);
+}
+
+//******************************************************************************
+TEST(Chebyshev, Decomposition) {
+  const size_t M = 16;
+
+  // Create example sequence
+  Sequence sequence;
+  for (size_t i = 0; i < M; i++) {
+    double x = ((double)i / M);  // - 0.99;
+    double y = x;
+    sequence[x] = y;
+  }
+
+  // Do Chebyshev Decomposition
+  FitBasis<Chebyshev1Basis> actual(sequence, model, N);
+
+  // Check
+  Vector expected = Vector::Zero(N);
+  expected(1) = 1;
+  EXPECT(assert_equal(expected, (Vector)actual.parameters(), 1e-4));
+}
+
+//******************************************************************************
+TEST(Chebyshev1, Derivative) {
+  Vector c(N);
+  c << 12, 3, 2;
+
+  Weights D;
+
+  double x = -1.0;
+  D = Chebyshev1Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(-5, (D * c)(0), 1e-9);
+
+  x = -0.5;
+  D = Chebyshev1Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(-1, (D * c)(0), 1e-9);
+
+  x = 0.3;
+  D = Chebyshev1Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(5.4, (D * c)(0), 1e-9);
+}
+
+//******************************************************************************
+Vector3 f(-6, 1, 0.5);
+
+double proxy1(double x, size_t N) {
+  return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f));
+}
+
+TEST(Chebyshev1, Derivative2) {
+  const double x = 0.5;
+  auto D = Chebyshev1Basis::DerivativeWeights(N, x);
+
+  Matrix numeric_dTdx =
+      numericalDerivative21<double, double, double>(proxy1, x, N);
+  // regression
+  EXPECT_DOUBLES_EQUAL(2, numeric_dTdx(0, 0), 1e-9);
+  EXPECT_DOUBLES_EQUAL(2, (D * f)(0), 1e-9);
+}
+
+//******************************************************************************
+TEST(Chebyshev2, Derivative) {
+  Vector c(N);
+  c << 12, 6, 2;
+
+  Weights D;
+
+  double x = -1.0;
+  CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error);
+  x = 1.0;
+  CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error);
+
+  x = -0.5;
+  D = Chebyshev2Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(4, (D * c)(0), 1e-9);
+
+  x = 0.3;
+  D = Chebyshev2Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(16.8, (D * c)(0), 1e-9);
+
+  x = 0.75;
+  D = Chebyshev2Basis::DerivativeWeights(N, x);
+  // regression
+  EXPECT_DOUBLES_EQUAL(24, (D * c)(0), 1e-9);
+
+  x = 10;
+  D = Chebyshev2Basis::DerivativeWeights(N, x, 0, 20);
+  // regression
+  EXPECT_DOUBLES_EQUAL(12, (D * c)(0), 1e-9);
+}
+
+//******************************************************************************
+double proxy2(double x, size_t N) {
+  return Chebyshev2Basis::EvaluationFunctor(N, x)(Vector(f));
+}
+
+TEST(Chebyshev2, Derivative2) {
+  const double x = 0.5;
+  auto D = Chebyshev2Basis::DerivativeWeights(N, x);
+
+  Matrix numeric_dTdx =
+      numericalDerivative21<double, double, double>(proxy2, x, N);
+  // regression
+  EXPECT_DOUBLES_EQUAL(4, numeric_dTdx(0, 0), 1e-9);
+  EXPECT_DOUBLES_EQUAL(4, (D * f)(0), 1e-9);
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/basis/tests/testChebyshev2.cpp

@@ -0,0 +1,435 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testChebyshev.cpp
+ * @date July 4, 2020
+ * @author Varun Agrawal
+ * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral
+ *        methods
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/basis/Chebyshev2.h>
+#include <gtsam/basis/FitBasis.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost::placeholders;
+
+noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
+
+const size_t N = 32;
+
+//******************************************************************************
+TEST(Chebyshev2, Point) {
+  static const int N = 5;
+  auto points = Chebyshev2::Points(N);
+  Vector expected(N);
+  expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.;
+  static const double tol = 1e-15;  // changing this reveals errors
+  EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol);
+  EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol);
+  EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol);
+  EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol);
+  EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol);
+
+  // Check symmetry
+  EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 0), -Chebyshev2::Point(N, 4), tol);
+  EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 1), -Chebyshev2::Point(N, 3), tol);
+}
+
+//******************************************************************************
+TEST(Chebyshev2, PointInInterval) {
+  static const int N = 5;
+  auto points = Chebyshev2::Points(N, 0, 20);
+  Vector expected(N);
+  expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.;
+  expected *= 10.0;
+  static const double tol = 1e-15;  // changing this reveals errors
+  EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol);
+  EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol);
+  EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol);
+  EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol);
+  EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol);
+
+  // all at once
+  Vector actual = Chebyshev2::Points(N, 0, 20);
+  CHECK(assert_equal(expected, actual));
+}
+
+//******************************************************************************
+// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5]
+TEST(Chebyshev2, Interpolate2) {
+  size_t N = 3;
+  Chebyshev2::EvaluationFunctor fx(N, 0.5);
+  Vector f(N);
+  f << 4, 2, 6;
+  EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9);
+}
+
+//******************************************************************************
+// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5]
+TEST(Chebyshev2, Interpolate2_Interval) {
+  Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2);
+  Vector3 f(4, 2, 6);
+  EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9);
+}
+
+//******************************************************************************
+// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1,
+// 3}}, 0.5]
+TEST(Chebyshev2, Interpolate5) {
+  Chebyshev2::EvaluationFunctor fx(5, 0.5);
+  Vector f(5);
+  f << 4, 2, 6, 3, 3;
+  EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5);
+}
+
+//******************************************************************************
+// Interpolating vectors
+TEST(Chebyshev2, InterpolateVector) {
+  double t = 30, a = 0, b = 100;
+  const size_t N = 3;
+  // Create 2x3 matrix with Vectors at Chebyshev points
+  ParameterMatrix<2> X(N);
+  X.row(0) = Chebyshev2::Points(N, a, b);  // slope 1 ramp
+
+  // Check value
+  Vector expected(2);
+  expected << t, 0;
+  Eigen::Matrix<double, /*2x2N*/ -1, -1> actualH(2, 2 * N);
+
+  Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b);
+  EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
+
+  // Check derivative
+  boost::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
+      &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none);
+  Matrix numericalH =
+      numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, Decomposition) {
+  // Create example sequence
+  Sequence sequence;
+  for (size_t i = 0; i < 16; i++) {
+    double x = (double)i / 16. - 0.99, y = x;
+    sequence[x] = y;
+  }
+
+  // Do Chebyshev Decomposition
+  FitBasis<Chebyshev2> actual(sequence, model, 3);
+
+  // Check
+  Vector expected(3);
+  expected << -1, 0, 1;
+  EXPECT(assert_equal(expected, actual.parameters(), 1e-4));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, DifferentiationMatrix3) {
+  // Trefethen00book, p.55
+  const size_t N = 3;
+  Matrix expected(N, N);
+  // Differentiation matrix computed from Chebfun
+  expected << 1.5000, -2.0000, 0.5000,  //
+      0.5000, -0.0000, -0.5000,         //
+      -0.5000, 2.0000, -1.5000;
+  // multiply by -1 since the cheb points have a phase shift wrt Trefethen
+  // This was verified with chebfun
+  expected = -expected;
+
+  Matrix actual = Chebyshev2::DifferentiationMatrix(N);
+  EXPECT(assert_equal(expected, actual, 1e-4));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, DerivativeMatrix6) {
+  // Trefethen00book, p.55
+  const size_t N = 6;
+  Matrix expected(N, N);
+  expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000,  //
+      2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764,           //
+      -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820,          //
+      0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236,            //
+      -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180,           //
+      0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000;
+  // multiply by -1 since the cheb points have a phase shift wrt Trefethen
+  // This was verified with chebfun
+  expected = -expected;
+
+  Matrix actual = Chebyshev2::DifferentiationMatrix(N);
+  EXPECT(assert_equal((Matrix)expected, actual, 1e-4));
+}
+
+// test function for CalculateWeights and DerivativeWeights
+double f(double x) {
+  // return 3*(x**3) - 2*(x**2) + 5*x - 11
+  return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11;
+}
+
+// its derivative
+double fprime(double x) {
+  // return 9*(x**2) - 4*(x) + 5
+  return 9.0 * pow(x, 2) - 4.0 * x + 5.0;
+}
+
+//******************************************************************************
+TEST(Chebyshev2, CalculateWeights) {
+  Eigen::Matrix<double, -1, 1> fvals(N);
+  for (size_t i = 0; i < N; i++) {
+    fvals(i) = f(Chebyshev2::Point(N, i));
+  }
+  double x1 = 0.7, x2 = -0.376;
+  Weights weights1 = Chebyshev2::CalculateWeights(N, x1);
+  Weights weights2 = Chebyshev2::CalculateWeights(N, x2);
+  EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8);
+  EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8);
+}
+
+TEST(Chebyshev2, CalculateWeights2) {
+  double a = 0, b = 10, x1 = 7, x2 = 4.12;
+
+  Eigen::Matrix<double, -1, 1> fvals(N);
+  for (size_t i = 0; i < N; i++) {
+    fvals(i) = f(Chebyshev2::Point(N, i, a, b));
+  }
+
+  Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b);
+  EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8);
+
+  Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b);
+  double expected2 = f(x2);  // 185.454784
+  double actual2 = weights2 * fvals;
+  EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8);
+}
+
+TEST(Chebyshev2, DerivativeWeights) {
+  Eigen::Matrix<double, -1, 1> fvals(N);
+  for (size_t i = 0; i < N; i++) {
+    fvals(i) = f(Chebyshev2::Point(N, i));
+  }
+  double x1 = 0.7, x2 = -0.376, x3 = 0.0;
+  Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1);
+  EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9);
+
+  Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2);
+  EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9);
+
+  Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3);
+  EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9);
+
+  // test if derivative calculation and cheb point is correct
+  double x4 = Chebyshev2::Point(N, 3);
+  Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4);
+  EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9);
+}
+
+TEST(Chebyshev2, DerivativeWeights2) {
+  double x1 = 5, x2 = 4.12, a = 0, b = 10;
+
+  Eigen::Matrix<double, -1, 1> fvals(N);
+  for (size_t i = 0; i < N; i++) {
+    fvals(i) = f(Chebyshev2::Point(N, i, a, b));
+  }
+
+  Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b);
+  EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8);
+
+  Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b);
+  EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8);
+
+  // test if derivative calculation and cheb point is correct
+  double x3 = Chebyshev2::Point(N, 3, a, b);
+  Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b);
+  EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8);
+}
+
+//******************************************************************************
+// Check two different ways to calculate the derivative weights
+TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) {
+  const size_t N6 = 6;
+  double x1 = 0.311;
+  Matrix D6 = Chebyshev2::DifferentiationMatrix(N6);
+  Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6;
+  Weights actual = Chebyshev2::DerivativeWeights(N6, x1);
+  EXPECT(assert_equal(expected, actual, 1e-12));
+
+  double a = -3, b = 8, x2 = 5.05;
+  Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b);
+  Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2;
+  Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b);
+  EXPECT(assert_equal(expected1, actual1, 1e-12));
+}
+
+//******************************************************************************
+// Check two different ways to calculate the derivative weights
+TEST(Chebyshev2, DerivativeWeights6) {
+  const size_t N6 = 6;
+  Matrix D6 = Chebyshev2::DifferentiationMatrix(N6);
+  Chebyshev2::Parameters x = Chebyshev2::Points(N6);  // ramp with slope 1
+  EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x)));
+}
+
+//******************************************************************************
+// Check two different ways to calculate the derivative weights
+TEST(Chebyshev2, DerivativeWeights7) {
+  const size_t N7 = 7;
+  Matrix D7 = Chebyshev2::DifferentiationMatrix(N7);
+  Chebyshev2::Parameters x = Chebyshev2::Points(N7);  // ramp with slope 1
+  EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x)));
+}
+
+//******************************************************************************
+// Check derivative in two different ways: numerical and using D on f
+Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished();
+double proxy3(double x) {
+  return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points);
+}
+
+TEST(Chebyshev2, Derivative6) {
+  // Check Derivative evaluation at point x=0.2
+
+  // calculate expected values by numerical derivative of synthesis
+  const double x = 0.2;
+  Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy3, x);
+
+  // Calculate derivatives at Chebyshev points using D3, interpolate
+  Matrix D6 = Chebyshev2::DifferentiationMatrix(6);
+  Vector derivative_at_points = D6 * f3_at_6points;
+  Chebyshev2::EvaluationFunctor fx(6, x);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8);
+
+  // Do directly
+  Chebyshev2::DerivativeFunctor dfdx(6, x);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8);
+}
+
+//******************************************************************************
+// Assert that derivative also works in non-standard interval [0,3]
+double proxy4(double x) {
+  return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points);
+}
+
+TEST(Chebyshev2, Derivative6_03) {
+  // Check Derivative evaluation at point x=0.2, in interval [0,3]
+
+  // calculate expected values by numerical derivative of synthesis
+  const double x = 0.2;
+  Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy4, x);
+
+  // Calculate derivatives at Chebyshev points using D3, interpolate
+  Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3);
+  Vector derivative_at_points = D6 * f3_at_6points;
+  Chebyshev2::EvaluationFunctor fx(6, x, 0, 3);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8);
+
+  // Do directly
+  Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8);
+}
+
+//******************************************************************************
+// Test VectorDerivativeFunctor
+TEST(Chebyshev2, VectorDerivativeFunctor) {
+  const size_t N = 3, M = 2;
+  const double x = 0.2;
+  using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
+  VecD fx(N, x, 0, 3);
+  ParameterMatrix<M> X(N);
+  Matrix actualH(M, M * N);
+  EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
+
+  // Test Jacobian
+  Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, M * N>(
+      boost::bind(&VecD::operator(), fx, _1, boost::none), X);
+  EXPECT(assert_equal(expectedH, actualH, 1e-7));
+}
+
+//******************************************************************************
+// Test VectorDerivativeFunctor with polynomial function
+TEST(Chebyshev2, VectorDerivativeFunctor2) {
+  const size_t N = 64, M = 1, T = 15;
+  using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
+
+  const Vector points = Chebyshev2::Points(N, 0, T);
+
+  // Assign the parameter matrix
+  Vector values(N);
+  for (size_t i = 0; i < N; ++i) {
+    values(i) = f(points(i));
+  }
+  ParameterMatrix<M> X(values);
+
+  // Evaluate the derivative at the chebyshev points using
+  // VectorDerivativeFunctor.
+  for (size_t i = 0; i < N; ++i) {
+    VecD d(N, points(i), 0, T);
+    Vector1 Dx = d(X);
+    EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6);
+  }
+
+  // Test Jacobian at the first chebyshev point.
+  Matrix actualH(M, M * N);
+  VecD vecd(N, points(0), 0, T);
+  vecd(X, actualH);
+  Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix<M>, M * N>(
+      boost::bind(&VecD::operator(), vecd, _1, boost::none), X);
+  EXPECT(assert_equal(expectedH, actualH, 1e-6));
+}
+
+//******************************************************************************
+// Test ComponentDerivativeFunctor
+TEST(Chebyshev2, ComponentDerivativeFunctor) {
+  const size_t N = 6, M = 2;
+  const double x = 0.2;
+  using CompFunc = Chebyshev2::ComponentDerivativeFunctor<M>;
+  size_t row = 1;
+  CompFunc fx(N, row, x, 0, 3);
+  ParameterMatrix<M> X(N);
+  Matrix actualH(1, M * N);
+  EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
+
+  Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, M * N>(
+      boost::bind(&CompFunc::operator(), fx, _1, boost::none), X);
+  EXPECT(assert_equal(expectedH, actualH, 1e-7));
+}
+
+//******************************************************************************
+TEST(Chebyshev2, IntegralWeights) {
+  const size_t N7 = 7;
+  Vector actual = Chebyshev2::IntegrationWeights(N7);
+  Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254,
+                     0.457142857142857, 0.520634920634921, 0.457142857142857,
+                     0.253968253968254, 0.0285714285714286)
+                        .finished();
+  EXPECT(assert_equal(expected, actual));
+
+  const size_t N8 = 8;
+  Vector actual2 = Chebyshev2::IntegrationWeights(N8);
+  Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208,
+                      0.352242423718159, 0.437208405798326, 0.437208405798326,
+                      0.352242423718159, 0.190141007218208, 0.0204081632653061)
+                         .finished();
+  EXPECT(assert_equal(expected2, actual2));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/basis/tests/testFourier.cpp

@@ -0,0 +1,254 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testFourier.cpp
+ * @date July 4, 2020
+ * @author Frank Dellaert, Varun Agrawal
+ * @brief Unit tests for Fourier Basis Decompositions with Expressions
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/basis/FitBasis.h>
+#include <gtsam/basis/Fourier.h>
+#include <gtsam/nonlinear/factorTesting.h>
+
+using namespace std;
+using namespace gtsam;
+
+auto model = noiseModel::Unit::Create(1);
+
+// Coefficients for testing, respectively 3 and 7 parameter Fourier basis.
+// They correspond to best approximation of TestFunction(x)
+const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished();
+const Vector7 k7Coefficients =
+    (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943)
+        .finished();
+
+// The test-function used below
+static double TestFunction(double x) { return exp(sin(x) + cos(x)); }
+
+//******************************************************************************
+TEST(Basis, BasisEvaluationFunctor) {
+  // fx(0) takes coefficients c to calculate the value f(c;x==0)
+  FourierBasis::EvaluationFunctor fx(3, 0);
+  EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1],
+                       fx(k3Coefficients), 1e-9);
+}
+
+//******************************************************************************
+TEST(Basis, BasisEvaluationFunctorDerivative) {
+  // If we give the H argument, we get the derivative of fx(0) wrpt coefficients
+  // Needs to be Matrix so it can be used by OptionalJacobian.
+  Matrix H(1, 3);
+  FourierBasis::EvaluationFunctor fx(3, 0);
+  EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1],
+                       fx(k3Coefficients, H), 1e-9);
+
+  Matrix13 expectedH(1, 1, 0);
+  EXPECT(assert_equal(expectedH, H));
+}
+
+//******************************************************************************
+TEST(Basis, Manual) {
+  const size_t N = 3;
+
+  // We will create a linear factor graph
+  GaussianFactorGraph graph;
+
+  // We create an unknown Vector expression for the coefficients
+  Key key(1);
+
+  // We will need values below to test the Jacobians
+  Values values;
+  values.insert<Vector>(key, Vector::Zero(N));  // value does not really matter
+
+  // At 16 different samples points x, check Predict_ expression
+  for (size_t i = 0; i < 16; i++) {
+    const double x = i * M_PI / 8;
+    const double desiredValue = TestFunction(x);
+
+    // Manual JacobianFactor
+    Matrix A(1, N);
+    A << 1, cos(x), sin(x);
+    Vector b(1);
+    b << desiredValue;
+    JacobianFactor linearFactor(key, A, b);
+    graph.add(linearFactor);
+
+    // Create factor to predict value at x
+    EvaluationFactor<FourierBasis> predictFactor(key, desiredValue, model, N,
+                                                 x);
+
+    // Check expression Jacobians
+    EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9);
+
+    auto linearizedFactor = predictFactor.linearize(values);
+    auto linearizedJacobianFactor =
+        boost::dynamic_pointer_cast<JacobianFactor>(linearizedFactor);
+    CHECK(linearizedJacobianFactor);  // makes sure it's indeed a JacobianFactor
+    EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9));
+  }
+
+  // Solve linear graph
+  VectorValues actual = graph.optimize();
+  EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4));
+}
+
+//******************************************************************************
+TEST(Basis, EvaluationFactor) {
+  // Check fitting a function with a 7-parameter Fourier basis
+
+  // Create linear factor graph
+  NonlinearFactorGraph graph;
+  Key key(1);
+  for (size_t i = 0; i < 16; i++) {
+    double x = i * M_PI / 8, desiredValue = TestFunction(x);
+    graph.emplace_shared<EvaluationFactor<FourierBasis>>(key, desiredValue,
+                                                         model, 7, x);
+  }
+
+  // Solve FourierFactorGraph
+  Values values;
+  values.insert<Vector>(key, Vector::Zero(7));
+  GaussianFactorGraph::shared_ptr lfg = graph.linearize(values);
+  VectorValues actual = lfg->optimize();
+
+  EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4));
+}
+
+//******************************************************************************
+TEST(Basis, WeightMatrix) {
+  // The WeightMatrix creates an m*n matrix, where m is the number of sample
+  // points, and n is the number of parameters.
+  Matrix expected(2, 3);
+  expected.row(0) << 1, cos(1), sin(1);
+  expected.row(1) << 1, cos(2), sin(2);
+  Vector2 X(1, 2);
+  Matrix actual = FourierBasis::WeightMatrix(3, X);
+  EXPECT(assert_equal(expected, actual, 1e-9));
+}
+
+//******************************************************************************
+TEST(Basis, Decomposition) {
+  // Create example sequence
+  Sequence sequence;
+  for (size_t i = 0; i < 16; i++) {
+    double x = i * M_PI / 8, y = TestFunction(x);
+    sequence[x] = y;
+  }
+
+  // Do Fourier Decomposition
+  FitBasis<FourierBasis> actual(sequence, model, 3);
+
+  // Check
+  EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4));
+}
+
+//******************************************************************************
+// Check derivative in two different ways: numerical and using D on f
+double proxy(double x) {
+  return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients);
+}
+
+TEST(Basis, Derivative7) {
+  // Check Derivative evaluation at point x=0.2
+
+  // Calculate expected values by numerical derivative of proxy.
+  const double x = 0.2;
+  Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy, x);
+
+  // Calculate derivatives at Chebyshev points using D7, interpolate
+  Matrix D7 = FourierBasis::DifferentiationMatrix(7);
+  Vector derivativeCoefficients = D7 * k7Coefficients;
+  FourierBasis::EvaluationFunctor fx(7, x);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8);
+
+  // Do directly
+  FourierBasis::DerivativeFunctor dfdx(7, x);
+  EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8);
+}
+
+//******************************************************************************
+TEST(Basis, VecDerivativeFunctor) {
+  using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>;
+  const size_t N = 3;
+
+  // MATLAB example, Dec 27 2019, commit 014eded5
+  double h = 2 * M_PI / 16;
+  Vector2 dotShape(0.5556, -0.8315);  // at h/2
+  DotShape dotShapeFunction(N, h / 2);
+  Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
+                           .finished()
+                           .transpose();
+  ParameterMatrix<2> theta(theta_mat);
+  EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4));
+}
+
+//******************************************************************************
+// Suppose we want to parameterize a periodic function with function values at
+// specific times, rather than coefficients. Can we do it? This would be a
+// generalization of the Fourier transform, and constitute a "pseudo-spectral"
+// parameterization. One way to do this is to establish hard constraints that
+// express the relationship between the new parameters and the coefficients.
+// For example, below I'd like the parameters to be the function values at
+// X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients.
+// Because the values f(X) = at these points can be written as f(X) = W(X)*c,
+// we can simply express the coefficents c as c=inv(W(X))*f, which is a
+// generalized Fourier transform. That also means we can create factors with the
+// unknown f-values, as done manually below.
+TEST(Basis, PseudoSpectral) {
+  // We will create a linear factor graph
+  GaussianFactorGraph graph;
+
+  const size_t N = 3;
+  const Key key(1);
+
+  // The correct values at X = {0.1,0.2,0.3} are simply W*c
+  const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished();
+  const Matrix W = FourierBasis::WeightMatrix(N, X);
+  const Vector expected = W * k3Coefficients;
+
+  // Check those values are indeed correct values of Fourier approximation
+  using Eval = FourierBasis::EvaluationFunctor;
+  EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9);
+  EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9);
+  EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9);
+
+  // Calculate "inverse Fourier transform" matrix
+  const Matrix invW = W.inverse();
+
+  // At 16 different samples points x, add a factor on fExpr
+  for (size_t i = 0; i < 16; i++) {
+    const double x = i * M_PI / 8;
+    const double desiredValue = TestFunction(x);
+
+    // Manual JacobianFactor
+    Matrix A(1, 3);
+    A << 1, cos(x), sin(x);
+    Vector b(1);
+    b << desiredValue;
+    JacobianFactor linearFactor(key, A * invW, b);
+    graph.add(linearFactor);
+  }
+
+  // Solve linear graph
+  VectorValues actual = graph.optimize();
+  EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/basis/tests/testParameterMatrix.cpp

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testParameterMatrix.cpp
+ * @date Sep 22, 2020
+ * @author Varun Agrawal, Frank Dellaert
+ * @brief Unit tests for ParameterMatrix.h
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/basis/BasisFactors.h>
+#include <gtsam/basis/Chebyshev2.h>
+#include <gtsam/basis/ParameterMatrix.h>
+#include <gtsam/inference/Symbol.h>
+
+using namespace std;
+using namespace gtsam;
+
+using Parameters = Chebyshev2::Parameters;
+
+const size_t M = 2, N = 5;
+
+//******************************************************************************
+TEST(ParameterMatrix, Constructor) {
+  ParameterMatrix<2> actual1(3);
+  ParameterMatrix<2> expected1(Matrix::Zero(2, 3));
+  EXPECT(assert_equal(expected1, actual1));
+
+  ParameterMatrix<2> actual2(Matrix::Ones(2, 3));
+  ParameterMatrix<2> expected2(Matrix::Ones(2, 3));
+  EXPECT(assert_equal(expected2, actual2));
+  EXPECT(assert_equal(expected2.matrix(), actual2.matrix()));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Dimensions) {
+  ParameterMatrix<M> params(N);
+  EXPECT_LONGS_EQUAL(params.rows(), M);
+  EXPECT_LONGS_EQUAL(params.cols(), N);
+  EXPECT_LONGS_EQUAL(params.dim(), M * N);
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Getters) {
+  ParameterMatrix<M> params(N);
+
+  Matrix expectedMatrix = Matrix::Zero(2, 5);
+  EXPECT(assert_equal(expectedMatrix, params.matrix()));
+
+  Matrix expectedMatrixTranspose = Matrix::Zero(5, 2);
+  EXPECT(assert_equal(expectedMatrixTranspose, params.transpose()));
+
+  ParameterMatrix<M> p2(Matrix::Ones(M, N));
+  Vector expectedRowVector = Vector::Ones(N);
+  for (size_t r = 0; r < M; ++r) {
+    EXPECT(assert_equal(p2.row(r), expectedRowVector));
+  }
+
+  ParameterMatrix<M> p3(2 * Matrix::Ones(M, N));
+  Vector expectedColVector = 2 * Vector::Ones(M);
+  for (size_t c = 0; c < M; ++c) {
+    EXPECT(assert_equal(p3.col(c), expectedColVector));
+  }
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Setters) {
+  ParameterMatrix<M> params(Matrix::Zero(M, N));
+  Matrix expected = Matrix::Zero(M, N);
+
+  // row
+  params.row(0) = Vector::Ones(N);
+  expected.row(0) = Vector::Ones(N);
+  EXPECT(assert_equal(expected, params.matrix()));
+
+  // col
+  params.col(2) = Vector::Ones(M);
+  expected.col(2) = Vector::Ones(M);
+
+  EXPECT(assert_equal(expected, params.matrix()));
+
+  // setZero
+  params.setZero();
+  expected.setZero();
+  EXPECT(assert_equal(expected, params.matrix()));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Addition) {
+  ParameterMatrix<M> params(Matrix::Ones(M, N));
+  ParameterMatrix<M> expected(2 * Matrix::Ones(M, N));
+
+  // Add vector
+  EXPECT(assert_equal(expected, params + Vector::Ones(M * N)));
+  // Add another ParameterMatrix
+  ParameterMatrix<M> actual = params + ParameterMatrix<M>(Matrix::Ones(M, N));
+  EXPECT(assert_equal(expected, actual));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Subtraction) {
+  ParameterMatrix<M> params(2 * Matrix::Ones(M, N));
+  ParameterMatrix<M> expected(Matrix::Ones(M, N));
+
+  // Subtract vector
+  EXPECT(assert_equal(expected, params - Vector::Ones(M * N)));
+  // Subtract another ParameterMatrix
+  ParameterMatrix<M> actual = params - ParameterMatrix<M>(Matrix::Ones(M, N));
+  EXPECT(assert_equal(expected, actual));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, Multiplication) {
+  ParameterMatrix<M> params(Matrix::Ones(M, N));
+  Matrix multiplier = 2 * Matrix::Ones(N, 2);
+  Matrix expected = 2 * N * Matrix::Ones(M, 2);
+  EXPECT(assert_equal(expected, params * multiplier));
+}
+
+//******************************************************************************
+TEST(ParameterMatrix, VectorSpace) {
+  ParameterMatrix<M> params(Matrix::Ones(M, N));
+  // vector
+  EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
+  // identity
+  EXPECT(assert_equal(ParameterMatrix<M>::identity(),
+                      ParameterMatrix<M>(Matrix::Zero(M, 0))));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/geometry/CameraSet.h

@@ -147,113 +147,123 @@ public:
      * G = F' * F - F' * E * P * E' * F
      * g = F' * (b - E * P * E' * b)
      * Fixed size version
-     */
-    template<int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
-    static SymmetricBlockMatrix SchurComplement(
-        const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
-        const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
+    */
+  template <int N,
+            int ND>  // N = 2 or 3 (point dimension), ND is the camera dimension
+  static SymmetricBlockMatrix SchurComplement(
+      const std::vector<
+          Eigen::Matrix<double, ZDim, ND>,
+          Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
+      const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
+    // a single point is observed in m cameras
+    size_t m = Fs.size();
 
-      // a single point is observed in m cameras
-      size_t m = Fs.size();
+    // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
+    size_t M1 = ND * m + 1;
+    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, ND);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
 
-      // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
-      size_t M1 = ND * m + 1;
-      std::vector<DenseIndex> dims(m + 1); // this also includes the b term
-      std::fill(dims.begin(), dims.end() - 1, ND);
-      dims.back() = 1;
-      SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
+    // Blockwise Schur complement
+    for (size_t i = 0; i < m; i++) { // for each camera
 
-      // Blockwise Schur complement
-      for (size_t i = 0; i < m; i++) { // for each camera
+      const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
+      const auto FiT = Fi.transpose();
+      const Eigen::Matrix<double, ZDim, N> Ei_P = //
+          E.block(ZDim * i, 0, ZDim, N) * P;
 
-        const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
-        const auto FiT = Fi.transpose();
-        const Eigen::Matrix<double, ZDim, N> Ei_P = //
-            E.block(ZDim * i, 0, ZDim, N) * P;
+      // D = (Dx2) * ZDim
+      augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
+      - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
 
-        // D = (Dx2) * ZDim
-        augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
-        - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+      augmentedHessian.setDiagonalBlock(i, FiT
+          * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
 
-        // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-        augmentedHessian.setDiagonalBlock(i, FiT
-            * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
+      // upper triangular part of the hessian
+      for (size_t j = i + 1; j < m; j++) { // for each camera
+        const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
 
-        // upper triangular part of the hessian
-        for (size_t j = i + 1; j < m; j++) { // for each camera
-          const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        augmentedHessian.setOffDiagonalBlock(i, j, -FiT
+            * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
+      }
+    } // end of for over cameras
 
-          // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-          augmentedHessian.setOffDiagonalBlock(i, j, -FiT
-              * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
-        }
-      } // end of for over cameras
-
-      augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
-      return augmentedHessian;
-    }
+    augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
+    return augmentedHessian;
+  }
 
   /**
    * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
    * G = F' * F - F' * E * P * E' * F
    * g = F' * (b - E * P * E' * b)
-   * In this version, we allow for the case where the keys in the Jacobian are organized
-   * differently from the keys in the output SymmetricBlockMatrix
-   * In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration)
-   * such that F keeps the block structure that makes the Schur complement trick fast.
+   * In this version, we allow for the case where the keys in the Jacobian are
+   * organized differently from the keys in the output SymmetricBlockMatrix In
+   * particular: each diagonal block of the Jacobian F captures 2 poses (useful
+   * for rolling shutter and extrinsic calibration) such that F keeps the block
+   * structure that makes the Schur complement trick fast.
+   *
+   * N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is
+   * the Hessian block dimension
    */
-  template<int N, int ND, int NDD>  // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension
+  template <int N, int ND, int NDD>
   static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
-      const std::vector<Eigen::Matrix<double, ZDim, ND>,
-          Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND> > >& Fs,
+      const std::vector<
+          Eigen::Matrix<double, ZDim, ND>,
+          Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
       const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b,
-      const KeyVector jacobianKeys, const KeyVector hessianKeys) {
-
+      const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
     size_t nrNonuniqueKeys = jacobianKeys.size();
     size_t nrUniqueKeys = hessianKeys.size();
 
-    // marginalize point: note - we reuse the standard SchurComplement function
-    SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs,E,P,b);
+    // Marginalize point: note - we reuse the standard SchurComplement function.
+    SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);
 
-    // now pack into an Hessian factor
-    std::vector<DenseIndex> dims(nrUniqueKeys + 1);  // this also includes the b term
+    // Pack into an Hessian factor, allow space for b term.
+    std::vector<DenseIndex> dims(nrUniqueKeys + 1);
     std::fill(dims.begin(), dims.end() - 1, NDD);
     dims.back() = 1;
     SymmetricBlockMatrix augmentedHessianUniqueKeys;
 
-    // here we have to deal with the fact that some blocks may share the same keys
-    if (nrUniqueKeys == nrNonuniqueKeys) {  // if there is 1 calibration key per camera
+    // Deal with the fact that some blocks may share the same keys.
+    if (nrUniqueKeys == nrNonuniqueKeys) {
+      // Case when there is 1 calibration key per camera:
       augmentedHessianUniqueKeys = SymmetricBlockMatrix(
           dims, Matrix(augmentedHessian.selfadjointView()));
-    } else {  // if multiple cameras share a calibration we have to rearrange
-      // the results of the Schur complement matrix
-      std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1);  // this also includes the b term
+    } else {
+      // When multiple cameras share a calibration we have to rearrange
+      // the results of the Schur complement matrix.
+      std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1);  // includes b
       std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
       nonuniqueDims.back() = 1;
       augmentedHessian = SymmetricBlockMatrix(
           nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
 
-      // get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
+      // Get map from key to location in the new augmented Hessian matrix (the
+      // one including only unique keys).
       std::map<Key, size_t> keyToSlotMap;
       for (size_t k = 0; k < nrUniqueKeys; k++) {
         keyToSlotMap[hessianKeys[k]] = k;
       }
 
-      // initialize matrix to zero
+      // Initialize matrix to zero.
       augmentedHessianUniqueKeys = SymmetricBlockMatrix(
           dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1));
 
-      // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
-      // and populates an Hessian that only includes the unique keys (that is what we want to return)
+      // Add contributions for each key: note this loops over the hessian with
+      // nonUnique keys (augmentedHessian) and populates an Hessian that only
+      // includes the unique keys (that is what we want to return).
       for (size_t i = 0; i < nrNonuniqueKeys; i++) {  // rows
         Key key_i = jacobianKeys.at(i);
 
-        // update information vector
+        // Update information vector.
         augmentedHessianUniqueKeys.updateOffDiagonalBlock(
             keyToSlotMap[key_i], nrUniqueKeys,
             augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
 
-        // update blocks
+        // Update blocks.
         for (size_t j = i; j < nrNonuniqueKeys; j++) {  // cols
           Key key_j = jacobianKeys.at(j);
           if (i == j) {
@@ -267,45 +277,20 @@ public:
             } else {
               augmentedHessianUniqueKeys.updateDiagonalBlock(
                   keyToSlotMap[key_i],
-                  augmentedHessian.aboveDiagonalBlock(i, j)
-                      + augmentedHessian.aboveDiagonalBlock(i, j).transpose());
+                  augmentedHessian.aboveDiagonalBlock(i, j) +
+                      augmentedHessian.aboveDiagonalBlock(i, j).transpose());
             }
           }
         }
       }
-      // update bottom right element of the matrix
+
+      // Update bottom right element of the matrix.
       augmentedHessianUniqueKeys.updateDiagonalBlock(
           nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
     }
     return augmentedHessianUniqueKeys;
   }
 
-  /**
-   * non-templated version of function above
-   */
-  static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6(
-      const std::vector<Eigen::Matrix<double,ZDim, 12>,
-          Eigen::aligned_allocator<Eigen::Matrix<double,ZDim,12> > >& Fs,
-      const Matrix& E, const Eigen::Matrix<double,3,3>& P, const Vector& b,
-      const KeyVector jacobianKeys, const KeyVector hessianKeys) {
-    return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b,
-                                                       jacobianKeys,
-                                                       hessianKeys);
-  }
-
-  /**
-   * non-templated version of function above
-   */
-  static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_6_6(
-      const std::vector<Eigen::Matrix<double,ZDim, 6>,
-          Eigen::aligned_allocator<Eigen::Matrix<double,ZDim,6> > >& Fs,
-      const Matrix& E, const Eigen::Matrix<double,3,3>& P, const Vector& b,
-      const KeyVector jacobianKeys, const KeyVector hessianKeys) {
-    return SchurComplementAndRearrangeBlocks<3,6,6>(Fs, E, P, b,
-                                                       jacobianKeys,
-                                                       hessianKeys);
-  }
-
   /**
    * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
    * G = F' * F - F' * E * P * E' * F

gtsam/geometry/tests/testCameraSet.cpp

@@ -185,9 +185,8 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
 
     // Actual
     SymmetricBlockMatrix augmentedHessianBM =
-        Set::SchurComplementAndRearrangeBlocks_3_12_6(Fs, E, P, b,
-                                                         nonuniqueKeys,
-                                                         uniqueKeys);
+        Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(
+            Fs, E, P, b, nonuniqueKeys, uniqueKeys);
     Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
 
     // Expected

gtsam/nonlinear/FunctorizedFactor.h

@@ -110,7 +110,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
   bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
     const FunctorizedFactor<R, T> *e =
         dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
-    return e && Base::equals(other, tol) &&
+    return e != nullptr && Base::equals(other, tol) &&
            traits<R>::Equals(this->measured_, e->measured_, tol);
   }
   /// @}

gtsam/nonlinear/tests/testFunctorizedFactor.cpp

@@ -20,8 +20,12 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/basis/Basis.h>
+#include <gtsam/basis/BasisFactors.h>
+#include <gtsam/basis/Chebyshev2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/FunctorizedFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/factorTesting.h>
 
 using namespace std;
@@ -60,7 +64,7 @@ class ProjectionFunctor {
     if (H1) {
       H1->resize(x.size(), A.size());
       *H1 << I_3x3, I_3x3, I_3x3;
-    } 
+    }
     if (H2) *H2 = A;
     return A * x;
   }
@@ -255,18 +259,148 @@ TEST(FunctorizedFactor, Lambda2) {
     if (H1) {
       H1->resize(x.size(), A.size());
       *H1 << I_3x3, I_3x3, I_3x3;
-    } 
+    }
     if (H2) *H2 = A;
     return A * x;
   };
   // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
-  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement, model2, lambda);
+  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
+                                                       model2, lambda);
 
   Vector error = factor.evaluateError(A, x);
 
   EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
 }
 
+const size_t N = 2;
+
+//******************************************************************************
+TEST(FunctorizedFactor, Print2) {
+  const size_t M = 1;
+
+  Vector measured = Vector::Ones(M) * 42;
+
+  auto model = noiseModel::Isotropic::Sigma(M, 1.0);
+  VectorEvaluationFactor<Chebyshev2, M> priorFactor(key, measured, model, N, 0);
+
+  string expected =
+      "  keys = { X0 }\n"
+      "  noise model: unit (1) \n"
+      "FunctorizedFactor(X0)\n"
+      "  measurement: [\n"
+      "	42\n"
+      "]\n"
+      "  noise model sigmas: 1\n";
+
+  EXPECT(assert_print_equal(expected, priorFactor));
+}
+
+//******************************************************************************
+TEST(FunctorizedFactor, VectorEvaluationFactor) {
+  const size_t M = 4;
+
+  Vector measured = Vector::Zero(M);
+
+  auto model = noiseModel::Isotropic::Sigma(M, 1.0);
+  VectorEvaluationFactor<Chebyshev2, M> priorFactor(key, measured, model, N, 0);
+
+  NonlinearFactorGraph graph;
+  graph.add(priorFactor);
+
+  ParameterMatrix<M> stateMatrix(N);
+
+  Values initial;
+  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.verbosity = NonlinearOptimizerParams::SILENT;
+  parameters.verbosityLM = LevenbergMarquardtParams::SILENT;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+//******************************************************************************
+TEST(FunctorizedFactor, VectorComponentFactor) {
+  const int P = 4;
+  const size_t i = 2;
+  const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0;
+  auto model = noiseModel::Isotropic::Sigma(1, 1.0);
+  VectorComponentFactor<Chebyshev2, P> controlPrior(key, measured, model, N, i,
+                                                    t, a, b);
+
+  NonlinearFactorGraph graph;
+  graph.add(controlPrior);
+
+  ParameterMatrix<P> stateMatrix(N);
+
+  Values initial;
+  initial.insert<ParameterMatrix<P>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.verbosity = NonlinearOptimizerParams::SILENT;
+  parameters.verbosityLM = LevenbergMarquardtParams::SILENT;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+//******************************************************************************
+TEST(FunctorizedFactor, VecDerivativePrior) {
+  const size_t M = 4;
+
+  Vector measured = Vector::Zero(M);
+  auto model = noiseModel::Isotropic::Sigma(M, 1.0);
+  VectorDerivativeFactor<Chebyshev2, M> vecDPrior(key, measured, model, N, 0);
+
+  NonlinearFactorGraph graph;
+  graph.add(vecDPrior);
+
+  ParameterMatrix<M> stateMatrix(N);
+
+  Values initial;
+  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.verbosity = NonlinearOptimizerParams::SILENT;
+  parameters.verbosityLM = LevenbergMarquardtParams::SILENT;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+//******************************************************************************
+TEST(FunctorizedFactor, ComponentDerivativeFactor) {
+  const size_t M = 4;
+
+  double measured = 0;
+  auto model = noiseModel::Isotropic::Sigma(1, 1.0);
+  ComponentDerivativeFactor<Chebyshev2, M> controlDPrior(key, measured, model,
+                                                         N, 0, 0);
+
+  NonlinearFactorGraph graph;
+  graph.add(controlDPrior);
+
+  Values initial;
+  ParameterMatrix<M> stateMatrix(N);
+  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.verbosity = NonlinearOptimizerParams::SILENT;
+  parameters.verbosityLM = LevenbergMarquardtParams::SILENT;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/SmartProjectionFactorP.h

@@ -305,7 +305,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
     // Build augmented Hessian (with last row/column being the information vector)
     // Note: we need to get the augumented hessian wrt the unique keys in key_
     SymmetricBlockMatrix augmentedHessianUniqueKeys =
-        Base::Cameras::SchurComplementAndRearrangeBlocks_3_6_6(
+        Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
             Fs, E, P, b, nonUniqueKeys_, this->keys_);
 
     return boost::make_shared < RegularHessianFactor<DimPose>

gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp

@@ -23,7 +23,6 @@ Vector ProjectionFactorRollingShutter::evaluateError(
     const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
     boost::optional<Matrix&> H3) const {
-
   try {
     Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2);
     gtsam::Matrix Hprj;
@@ -32,12 +31,10 @@ Vector ProjectionFactorRollingShutter::evaluateError(
         gtsam::Matrix HbodySensor;
         PinholeCamera<Cal3_S2> camera(
             pose.compose(*body_P_sensor_, HbodySensor), *K_);
-        Point2 reprojectionError(
-            camera.project(point, Hprj, H3, boost::none) - measured_);
-        if (H1)
-          *H1 = Hprj * HbodySensor * (*H1);
-        if (H2)
-          *H2 = Hprj * HbodySensor * (*H2);
+        Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
+                                 measured_);
+        if (H1) *H1 = Hprj * HbodySensor * (*H1);
+        if (H2) *H2 = Hprj * HbodySensor * (*H2);
         return reprojectionError;
       } else {
         PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_);
@@ -45,29 +42,23 @@ Vector ProjectionFactorRollingShutter::evaluateError(
       }
     } else {
       PinholeCamera<Cal3_S2> camera(pose, *K_);
-      Point2 reprojectionError(
-          camera.project(point, Hprj, H3, boost::none) - measured_);
-      if (H1)
-        *H1 = Hprj * (*H1);
-      if (H2)
-        *H2 = Hprj * (*H2);
+      Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
+                               measured_);
+      if (H1) *H1 = Hprj * (*H1);
+      if (H2) *H2 = Hprj * (*H2);
       return reprojectionError;
     }
   } catch (CheiralityException& e) {
-    if (H1)
-      *H1 = Matrix::Zero(2, 6);
-    if (H2)
-      *H2 = Matrix::Zero(2, 6);
-    if (H3)
-      *H3 = Matrix::Zero(2, 3);
+    if (H1) *H1 = Matrix::Zero(2, 6);
+    if (H2) *H2 = Matrix::Zero(2, 6);
+    if (H3) *H3 = Matrix::Zero(2, 3);
     if (verboseCheirality_)
       std::cout << e.what() << ": Landmark "
-          << DefaultKeyFormatter(this->key2()) << " moved behind camera "
-          << DefaultKeyFormatter(this->key1()) << std::endl;
-    if (throwCheirality_)
-      throw CheiralityException(this->key2());
+                << DefaultKeyFormatter(this->key2()) << " moved behind camera "
+                << DefaultKeyFormatter(this->key1()) << std::endl;
+    if (throwCheirality_) throw CheiralityException(this->key2());
   }
   return Vector2::Constant(2.0 * K_->fx());
 }
 
-}  //namespace gtsam
+}  // namespace gtsam

gtsam_unstable/slam/ProjectionFactorRollingShutter.h

@@ -17,41 +17,47 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
 #include <boost/optional.hpp>
 
 namespace gtsam {
 
 /**
- * Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. The calibration is known here.
- * This version takes rolling shutter information into account as follows: consider two consecutive poses A and B,
- * and a Point2 measurement taken starting at time A using a rolling shutter camera.
- * Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B)
- * corresponding to the time of exposure of the row of the image the pixel belongs to.
- * Let us define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by
- * the alpha to project the corresponding landmark to Point2.
+ * Non-linear factor for 2D projection measurement obtained using a rolling
+ * shutter camera. The calibration is known here. This version takes rolling
+ * shutter information into account as follows: consider two consecutive poses A
+ * and B, and a Point2 measurement taken starting at time A using a rolling
+ * shutter camera. Pose A has timestamp t_A, and Pose B has timestamp t_B. The
+ * Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) corresponding
+ * to the time of exposure of the row of the image the pixel belongs to. Let us
+ * define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose
+ * interpolated between A and B by the alpha to project the corresponding
+ * landmark to Point2.
  * @addtogroup SLAM
  */
 
-class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
-    Point3> {
+class ProjectionFactorRollingShutter
+    : public NoiseModelFactor3<Pose3, Pose3, Point3> {
  protected:
-
   // Keep a copy of measurement and calibration for I/O
-  Point2 measured_;                   ///< 2D measurement
-  double alpha_;  ///< interpolation parameter in [0,1] corresponding to the point2 measurement
+  Point2 measured_;  ///< 2D measurement
+  double alpha_;     ///< interpolation parameter in [0,1] corresponding to the
+                     ///< point2 measurement
   boost::shared_ptr<Cal3_S2> K_;  ///< shared pointer to calibration object
-  boost::optional<Pose3> body_P_sensor_;  ///< The pose of the sensor in the body frame
+  boost::optional<Pose3>
+      body_P_sensor_;  ///< The pose of the sensor in the body frame
 
   // verbosity handling for Cheirality Exceptions
-  bool throwCheirality_;  ///< If true, rethrows Cheirality exceptions (default: false)
-  bool verboseCheirality_;  ///< If true, prints text for Cheirality exceptions (default: false)
+  bool throwCheirality_;  ///< If true, rethrows Cheirality exceptions (default:
+                          ///< false)
+  bool verboseCheirality_;  ///< If true, prints text for Cheirality exceptions
+                            ///< (default: false)
 
  public:
-
   /// shorthand for base class type
   typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
 
@@ -66,72 +72,72 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
       : measured_(0, 0),
         alpha_(0),
         throwCheirality_(false),
-        verboseCheirality_(false) {
-  }
+        verboseCheirality_(false) {}
 
   /**
    * Constructor
-   * @param measured is the 2-dimensional pixel location of point in the image (the measurement)
+   * @param measured is the 2-dimensional pixel location of point in the image
+   * (the measurement)
    * @param alpha in [0,1] is the rolling shutter parameter for the measurement
    * @param model is the noise model
    * @param poseKey_a is the key of the first camera
    * @param poseKey_b is the key of the second camera
    * @param pointKey is the key of the landmark
    * @param K shared pointer to the constant calibration
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from body to sensor frame (default
+   * identity)
    */
-  ProjectionFactorRollingShutter(const Point2& measured, double alpha,
-                                 const SharedNoiseModel& model, Key poseKey_a,
-                                 Key poseKey_b, Key pointKey,
-                                 const boost::shared_ptr<Cal3_S2>& K,
-                                 boost::optional<Pose3> body_P_sensor =
-                                     boost::none)
+  ProjectionFactorRollingShutter(
+      const Point2& measured, double alpha, const SharedNoiseModel& model,
+      Key poseKey_a, Key poseKey_b, Key pointKey,
+      const boost::shared_ptr<Cal3_S2>& K,
+      boost::optional<Pose3> body_P_sensor = boost::none)
       : Base(model, poseKey_a, poseKey_b, pointKey),
         measured_(measured),
         alpha_(alpha),
         K_(K),
         body_P_sensor_(body_P_sensor),
         throwCheirality_(false),
-        verboseCheirality_(false) {
-  }
+        verboseCheirality_(false) {}
 
   /**
    * Constructor with exception-handling flags
-   * @param measured is the 2-dimensional pixel location of point in the image (the measurement)
+   * @param measured is the 2-dimensional pixel location of point in the image
+   * (the measurement)
    * @param alpha in [0,1] is the rolling shutter parameter for the measurement
    * @param model is the noise model
    * @param poseKey_a is the key of the first camera
    * @param poseKey_b is the key of the second camera
    * @param pointKey is the key of the landmark
    * @param K shared pointer to the constant calibration
-   * @param throwCheirality determines whether Cheirality exceptions are rethrown
-   * @param verboseCheirality determines whether exceptions are printed for Cheirality
-   * @param body_P_sensor is the transform from body to sensor frame  (default identity)
+   * @param throwCheirality determines whether Cheirality exceptions are
+   * rethrown
+   * @param verboseCheirality determines whether exceptions are printed for
+   * Cheirality
+   * @param body_P_sensor is the transform from body to sensor frame  (default
+   * identity)
    */
-  ProjectionFactorRollingShutter(const Point2& measured, double alpha,
-                                 const SharedNoiseModel& model, Key poseKey_a,
-                                 Key poseKey_b, Key pointKey,
-                                 const boost::shared_ptr<Cal3_S2>& K,
-                                 bool throwCheirality, bool verboseCheirality,
-                                 boost::optional<Pose3> body_P_sensor =
-                                     boost::none)
+  ProjectionFactorRollingShutter(
+      const Point2& measured, double alpha, const SharedNoiseModel& model,
+      Key poseKey_a, Key poseKey_b, Key pointKey,
+      const boost::shared_ptr<Cal3_S2>& K, bool throwCheirality,
+      bool verboseCheirality,
+      boost::optional<Pose3> body_P_sensor = boost::none)
       : Base(model, poseKey_a, poseKey_b, pointKey),
         measured_(measured),
         alpha_(alpha),
         K_(K),
         body_P_sensor_(body_P_sensor),
         throwCheirality_(throwCheirality),
-        verboseCheirality_(verboseCheirality) {
-  }
+        verboseCheirality_(verboseCheirality) {}
 
   /** Virtual destructor */
-  virtual ~ProjectionFactorRollingShutter() {
-  }
+  virtual ~ProjectionFactorRollingShutter() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-    return boost::static_pointer_cast < gtsam::NonlinearFactor
-        > (gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /**
@@ -139,8 +145,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter useful for printing Symbols
    */
-  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-                 DefaultKeyFormatter) const {
+  void print(
+      const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "ProjectionFactorRollingShutter, z = ";
     traits<Point2>::Print(measured_);
     std::cout << " rolling shutter interpolation param = " << alpha_;
@@ -150,71 +157,61 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
   }
 
   /// equals
-  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
-    const This *e = dynamic_cast<const This*>(&p);
-    return e && Base::equals(p, tol) && (alpha_ == e->alpha())
-        && traits<Point2>::Equals(this->measured_, e->measured_, tol)
-        && this->K_->equals(*e->K_, tol)
-        && (this->throwCheirality_ == e->throwCheirality_)
-        && (this->verboseCheirality_ == e->verboseCheirality_)
-        && ((!body_P_sensor_ && !e->body_P_sensor_)
-            || (body_P_sensor_ && e->body_P_sensor_
-                && body_P_sensor_->equals(*e->body_P_sensor_)));
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
+    const This* e = dynamic_cast<const This*>(&p);
+    return e && Base::equals(p, tol) && (alpha_ == e->alpha()) &&
+           traits<Point2>::Equals(this->measured_, e->measured_, tol) &&
+           this->K_->equals(*e->K_, tol) &&
+           (this->throwCheirality_ == e->throwCheirality_) &&
+           (this->verboseCheirality_ == e->verboseCheirality_) &&
+           ((!body_P_sensor_ && !e->body_P_sensor_) ||
+            (body_P_sensor_ && e->body_P_sensor_ &&
+             body_P_sensor_->equals(*e->body_P_sensor_)));
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
-  Vector evaluateError(const Pose3& pose_a, const Pose3& pose_b,
-                       const Point3& point, boost::optional<Matrix&> H1 =
-                           boost::none,
-                       boost::optional<Matrix&> H2 = boost::none,
-                       boost::optional<Matrix&> H3 = boost::none) const;
+  Vector evaluateError(
+      const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none,
+      boost::optional<Matrix&> H3 = boost::none) const override;
 
   /** return the measurement */
-  const Point2& measured() const {
-    return measured_;
-  }
+  const Point2& measured() const { return measured_; }
 
   /** return the calibration object */
-  inline const boost::shared_ptr<Cal3_S2> calibration() const {
-    return K_;
-  }
+  inline const boost::shared_ptr<Cal3_S2> calibration() const { return K_; }
 
   /** returns the rolling shutter interp param*/
-  inline double alpha() const {
-    return alpha_;
-  }
+  inline double alpha() const { return alpha_; }
 
   /** return verbosity */
-  inline bool verboseCheirality() const {
-    return verboseCheirality_;
-  }
+  inline bool verboseCheirality() const { return verboseCheirality_; }
 
   /** return flag for throwing cheirality exceptions */
-  inline bool throwCheirality() const {
-    return throwCheirality_;
-  }
+  inline bool throwCheirality() const { return throwCheirality_; }
 
  private:
-
   /// Serialization function
   friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar & BOOST_SERIALIZATION_NVP(measured_);
-    ar & BOOST_SERIALIZATION_NVP(alpha_);
-    ar & BOOST_SERIALIZATION_NVP(K_);
-    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
-    ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
-    ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar& BOOST_SERIALIZATION_NVP(measured_);
+    ar& BOOST_SERIALIZATION_NVP(alpha_);
+    ar& BOOST_SERIALIZATION_NVP(K_);
+    ar& BOOST_SERIALIZATION_NVP(body_P_sensor_);
+    ar& BOOST_SERIALIZATION_NVP(throwCheirality_);
+    ar& BOOST_SERIALIZATION_NVP(verboseCheirality_);
   }
+
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
-template<> struct traits<ProjectionFactorRollingShutter> : public Testable<
-    ProjectionFactorRollingShutter> {
-};
+template <>
+struct traits<ProjectionFactorRollingShutter>
+    : public Testable<ProjectionFactorRollingShutter> {};
 
-}  //namespace gtsam
+}  // namespace gtsam

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -11,12 +11,14 @@
 
 /**
  * @file   SmartProjectionPoseFactorRollingShutter.h
- * @brief  Smart projection factor on poses modeling rolling shutter effect with given readout time
+ * @brief  Smart projection factor on poses modeling rolling shutter effect with
+ * given readout time
  * @author Luca Carlone
  */
 
 #pragma once
 
+#include <gtsam/geometry/CameraSet.h>
 #include <gtsam/slam/SmartProjectionFactor.h>
 
 namespace gtsam {
@@ -32,25 +34,28 @@ namespace gtsam {
  */
 
 /**
- * This factor optimizes two consecutive poses of the body assuming a rolling shutter model of the camera with given readout time.
- * The factor requires that values contain (for each pixel observation) two consecutive camera poses
- * from which the pixel observation pose can be interpolated.
+ * This factor optimizes two consecutive poses of the body assuming a rolling
+ * shutter model of the camera with given readout time. The factor requires that
+ * values contain (for each pixel observation) two consecutive camera poses from
+ * which the pixel observation pose can be interpolated.
  * @addtogroup SLAM
  */
-template<class CAMERA>
-class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAMERA> {
-
+template <class CAMERA>
+class SmartProjectionPoseFactorRollingShutter
+    : public SmartProjectionFactor<CAMERA> {
  public:
   typedef typename CAMERA::CalibrationType CALIBRATION;
 
  protected:
   /// shared pointer to calibration object (one for each observation)
-  std::vector<boost::shared_ptr<CALIBRATION> > K_all_;
+  std::vector<boost::shared_ptr<CALIBRATION>> K_all_;
 
-  /// The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation
+  /// The keys of the pose of the body (with respect to an external world
+  /// frame): two consecutive poses for each observation
   std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
 
-  /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses
+  /// interpolation factor (one for each observation) to interpolate between
+  /// pair of consecutive poses
   std::vector<double> alphas_;
 
   /// Pose of the camera in the body frame
@@ -60,7 +65,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for base class type
-  typedef SmartProjectionFactor<PinholePose<CALIBRATION> > Base;
+  typedef SmartProjectionFactor<PinholePose<CALIBRATION>> Base;
 
   /// shorthand for this class
   typedef SmartProjectionPoseFactorRollingShutter This;
@@ -68,11 +73,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
-  static const int DimBlock = 12;  ///< size of the variable stacking 2 poses from which the observation pose is interpolated
+  static const int DimBlock =
+      12;  ///< size of the variable stacking 2 poses from which the observation
+           ///< pose is interpolated
   static const int DimPose = 6;  ///< Pose3 dimension
-  static const int ZDim = 2;  ///< Measurement dimension (Point2)
-  typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD;  // F blocks (derivatives wrt block of 2 poses)
-  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;  // vector of F blocks
+  static const int ZDim = 2;     ///< Measurement dimension (Point2)
+  typedef Eigen::Matrix<double, ZDim, DimBlock>
+      MatrixZD;  // F blocks (derivatives wrt block of 2 poses)
+  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
+      FBlocks;  // vector of F blocks
 
   /**
    * Constructor
@@ -82,25 +91,29 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   SmartProjectionPoseFactorRollingShutter(
       const SharedNoiseModel& sharedNoiseModel,
       const SmartProjectionParams& params = SmartProjectionParams())
-  : Base(sharedNoiseModel, params) {
-  }
+      : Base(sharedNoiseModel, params) {}
 
   /** Virtual destructor */
   ~SmartProjectionPoseFactorRollingShutter() override = default;
 
   /**
-   * add a new measurement, with 2 pose keys, interpolation factor, camera (intrinsic and extrinsic) calibration, and observed pixel.
-   * @param measured 2-dimensional location of the projection of a
-   * single landmark in a single view (the measurement), interpolated from the 2 poses
-   * @param world_P_body_key1 key corresponding to the first body poses (time <= time pixel is acquired)
-   * @param world_P_body_key2 key corresponding to the second body poses (time >= time pixel is acquired)
-   * @param alpha interpolation factor in [0,1], such that if alpha = 0 the interpolated pose is the same as world_P_body_key1
+   * add a new measurement, with 2 pose keys, interpolation factor, camera
+   * (intrinsic and extrinsic) calibration, and observed pixel.
+   * @param measured 2-dimensional location of the projection of a single
+   *  landmark in a single view (the measurement), interpolated from the 2 poses
+   * @param world_P_body_key1 key corresponding to the first body poses (time <=
+   *  time pixel is acquired)
+   * @param world_P_body_key2 key corresponding to the second body poses (time
+   *  >= time pixel is acquired)
+   * @param alpha interpolation factor in [0,1], such that if alpha = 0 the
+   *  interpolated pose is the same as world_P_body_key1
    * @param K (fixed) camera intrinsic calibration
    * @param body_P_sensor (fixed) camera extrinsic calibration
    */
   void add(const Point2& measured, const Key& world_P_body_key1,
            const Key& world_P_body_key2, const double& alpha,
-           const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
+           const boost::shared_ptr<CALIBRATION>& K,
+           const Pose3& body_P_sensor = Pose3::identity()) {
     // store measurements in base class
     this->measured_.push_back(measured);
 
@@ -108,10 +121,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
     world_P_body_key_pairs_.push_back(
         std::make_pair(world_P_body_key1, world_P_body_key2));
 
-    //  also store keys in the keys_ vector: these keys are assumed to be unique, so we avoid duplicates here
-    if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) == this->keys_.end())
+    //  also store keys in the keys_ vector: these keys are assumed to be
+    //  unique, so we avoid duplicates here
+    if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) ==
+        this->keys_.end())
       this->keys_.push_back(world_P_body_key1);  // add only unique keys
-    if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) == this->keys_.end())
+    if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) ==
+        this->keys_.end())
       this->keys_.push_back(world_P_body_key2);  // add only unique keys
 
     // store interpolation factor
@@ -125,12 +141,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   }
 
   /**
-   * Variant of the previous "add" function in which we include multiple measurements
+   * Variant of the previous "add" function in which we include multiple
+   * measurements
    * @param measurements vector of the 2m dimensional location of the projection
-   * of a single landmark in the m views (the measurements)
-   * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding
-   * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated
-   * @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order)
+   *  of a single landmark in the m views (the measurements)
+   * @param world_P_body_key_pairs vector where the i-th element contains a pair
+   *  of keys corresponding to the pair of poses from which the observation pose
+   *  for the i0-th measurement can be interpolated
+   * @param alphas vector of interpolation params (in [0,1]), one for each
+   * measurement (in the same order)
    * @param Ks vector of (fixed) intrinsic calibration objects
    * @param body_P_sensors vector of (fixed) extrinsic calibration objects
    */
@@ -138,7 +157,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
            const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
            const std::vector<double>& alphas,
            const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
-           const std::vector<Pose3> body_P_sensors) {
+           const std::vector<Pose3>& body_P_sensors) {
     assert(world_P_body_key_pairs.size() == measurements.size());
     assert(world_P_body_key_pairs.size() == alphas.size());
     assert(world_P_body_key_pairs.size() == Ks.size());
@@ -150,20 +169,24 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   }
 
   /**
-   * Variant of the previous "add" function in which we include multiple measurements
-   * with the same (intrinsic and extrinsic) calibration
+   * Variant of the previous "add" function in which we include multiple
+   * measurements with the same (intrinsic and extrinsic) calibration
    * @param measurements vector of the 2m dimensional location of the projection
-   * of a single landmark in the m views (the measurements)
-   * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding
-   * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated
-   * @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order)
+   *  of a single landmark in the m views (the measurements)
+   * @param world_P_body_key_pairs vector where the i-th element contains a pair
+   *  of keys corresponding to the pair of poses from which the observation pose
+   *  for the i0-th measurement can be interpolated
+   * @param alphas vector of interpolation params (in [0,1]), one for each
+   *  measurement (in the same order)
    * @param K (fixed) camera intrinsic calibration (same for all measurements)
-   * @param body_P_sensor (fixed) camera extrinsic calibration (same for all measurements)
+   * @param body_P_sensor (fixed) camera extrinsic calibration (same for all
+   *  measurements)
    */
   void add(const Point2Vector& measurements,
            const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
            const std::vector<double>& alphas,
-           const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
+           const boost::shared_ptr<CALIBRATION>& K,
+           const Pose3& body_P_sensor = Pose3::identity()) {
     assert(world_P_body_key_pairs.size() == measurements.size());
     assert(world_P_body_key_pairs.size() == alphas.size());
     for (size_t i = 0; i < measurements.size(); i++) {
@@ -173,39 +196,37 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   }
 
   /// return the calibration object
-  inline std::vector<boost::shared_ptr<CALIBRATION>> calibration() const {
+  const std::vector<boost::shared_ptr<CALIBRATION>>& calibration() const {
     return K_all_;
   }
 
-  /// return (for each observation) the keys of the pair of poses from which we interpolate
-  const std::vector<std::pair<Key, Key>> world_P_body_key_pairs() const {
+  /// return (for each observation) the keys of the pair of poses from which we
+  /// interpolate
+  const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs() const {
     return world_P_body_key_pairs_;
   }
 
   /// return the interpolation factors alphas
-  const std::vector<double> alphas() const {
-    return alphas_;
-  }
+  const std::vector<double>& alphas() const { return alphas_; }
 
   /// return the extrinsic camera calibration body_P_sensors
-  const std::vector<Pose3> body_P_sensors() const {
-    return body_P_sensors_;
-  }
+  const std::vector<Pose3>& body_P_sensors() const { return body_P_sensors_; }
 
   /**
    * print
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter useful for printing Symbols
    */
-  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const override {
+  void print(
+      const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
     for (size_t i = 0; i < K_all_.size(); i++) {
       std::cout << "-- Measurement nr " << i << std::endl;
       std::cout << " pose1 key: "
-          << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
+                << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
       std::cout << " pose2 key: "
-          << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
+                << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
       std::cout << " alpha: " << alphas_[i] << std::endl;
       body_P_sensors_[i].print("extrinsic calibration:\n");
       K_all_[i]->print("intrinsic calibration = ");
@@ -216,41 +237,50 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   /// equals
   bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
     const SmartProjectionPoseFactorRollingShutter<CAMERA>* e =
-        dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(&p);
+        dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(
+            &p);
 
     double keyPairsEqual = true;
-    if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){
-      for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){
+    if (this->world_P_body_key_pairs_.size() ==
+        e->world_P_body_key_pairs().size()) {
+      for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) {
         const Key key1own = world_P_body_key_pairs_[k].first;
         const Key key1e = e->world_P_body_key_pairs()[k].first;
         const Key key2own = world_P_body_key_pairs_[k].second;
         const Key key2e = e->world_P_body_key_pairs()[k].second;
-        if ( !(key1own == key1e) || !(key2own == key2e) ){
-          keyPairsEqual = false; break;
+        if (!(key1own == key1e) || !(key2own == key2e)) {
+          keyPairsEqual = false;
+          break;
         }
       }
-    }else{ keyPairsEqual = false; }
+    } else {
+      keyPairsEqual = false;
+    }
 
     double extrinsicCalibrationEqual = true;
-    if(this->body_P_sensors_.size() == e->body_P_sensors().size()){
-      for(size_t i=0; i< this->body_P_sensors_.size(); i++){
-        if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){
-          extrinsicCalibrationEqual = false; break;
+    if (this->body_P_sensors_.size() == e->body_P_sensors().size()) {
+      for (size_t i = 0; i < this->body_P_sensors_.size(); i++) {
+        if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) {
+          extrinsicCalibrationEqual = false;
+          break;
         }
       }
-    }else{ extrinsicCalibrationEqual = false; }
+    } else {
+      extrinsicCalibrationEqual = false;
+    }
 
-    return e && Base::equals(p, tol) && K_all_ == e->calibration()
-        && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
+    return e && Base::equals(p, tol) && K_all_ == e->calibration() &&
+           alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
   }
 
   /**
    * Compute jacobian F, E and error vector at a given linearization point
    * @param values Values structure which must contain camera poses
    * corresponding to keys involved in this factor
-   * @return Return arguments are the camera jacobians Fs (including the jacobian with
-   * respect to both body poses we interpolate from), the point Jacobian E,
-   * and the error vector b. Note that the jacobians are computed for a given point.
+   * @return Return arguments are the camera jacobians Fs (including the
+   * jacobian with respect to both body poses we interpolate from), the point
+   * Jacobian E, and the error vector b. Note that the jacobians are computed
+   * for a given point.
    */
   void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
                                              const Values& values) const {
@@ -258,33 +288,38 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
       throw("computeJacobiansWithTriangulatedPoint");
     } else {  // valid result: compute jacobians
       size_t numViews = this->measured_.size();
-      E = Matrix::Zero(2 * numViews, 3);  // a Point2 for each view (point jacobian)
+      E = Matrix::Zero(2 * numViews,
+                       3);  // a Point2 for each view (point jacobian)
       b = Vector::Zero(2 * numViews);  // a Point2 for each view
       // intermediate Jacobians
       Eigen::Matrix<double, ZDim, DimPose> dProject_dPoseCam;
       Eigen::Matrix<double, DimPose, DimPose> dInterpPose_dPoseBody1,
-      dInterpPose_dPoseBody2, dPoseCam_dInterpPose;
+          dInterpPose_dPoseBody2, dPoseCam_dInterpPose;
       Eigen::Matrix<double, ZDim, 3> Ei;
 
       for (size_t i = 0; i < numViews; i++) {  // for each camera/measurement
-        const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
-        const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
+        auto w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
+        auto w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
         double interpolationFactor = alphas_[i];
         // get interpolated pose:
-        const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
-        const Pose3& body_P_cam = body_P_sensors_[i];
-        const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
+        auto w_P_body =
+            interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
+                               dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
+        auto body_P_cam = body_P_sensors_[i];
+        auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
         PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]);
 
         // get jacobians and error vector for current measurement
-        Point2 reprojectionError_i = Point2(
-            camera.project(*this->result_, dProject_dPoseCam, Ei)
-            - this->measured_.at(i));
+        Point2 reprojectionError_i =
+            Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) -
+                   this->measured_.at(i));
         Eigen::Matrix<double, ZDim, DimBlock> J;  // 2 x 12
-        J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose
-            * dInterpPose_dPoseBody1;  // (2x6) * (6x6) * (6x6)
-        J.block(0, 6, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose
-            * dInterpPose_dPoseBody2;  // (2x6) * (6x6) * (6x6)
+        J.block(0, 0, ZDim, 6) =
+            dProject_dPoseCam * dPoseCam_dInterpPose *
+            dInterpPose_dPoseBody1;  // (2x6) * (6x6) * (6x6)
+        J.block(0, 6, ZDim, 6) =
+            dProject_dPoseCam * dPoseCam_dInterpPose *
+            dInterpPose_dPoseBody2;  // (2x6) * (6x6) * (6x6)
 
         // fit into the output structures
         Fs.push_back(J);
@@ -296,37 +331,40 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   }
 
   /// linearize and return a Hessianfactor that is an approximation of error(p)
-  boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
-      const Values& values, const double lambda = 0.0, bool diagonalDamping =
-          false) const {
-
-    // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation),
-    // hence the number of unique keys may be smaller than 2 * nrMeasurements
-    size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys
+  boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
+      const Values& values, const double lambda = 0.0,
+      bool diagonalDamping = false) const {
+    // we may have multiple observation sharing the same keys (due to the
+    // rolling shutter interpolation), hence the number of unique keys may be
+    // smaller than 2 * nrMeasurements
+    size_t nrUniqueKeys =
+        this->keys_
+            .size();  // note: by construction, keys_ only contains unique keys
 
     // Create structures for Hessian Factors
     KeyVector js;
-    std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
-    std::vector < Vector > gs(nrUniqueKeys);
+    std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
+    std::vector<Vector> gs(nrUniqueKeys);
 
-    if (this->measured_.size() != this->cameras(values).size()) // 1 observation per interpolated camera
-      throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
-                               "measured_.size() inconsistent with input");
+    if (this->measured_.size() !=
+        this->cameras(values).size())  // 1 observation per interpolated camera
+      throw std::runtime_error(
+          "SmartProjectionPoseFactorRollingShutter: "
+          "measured_.size() inconsistent with input");
 
     // triangulate 3D point at given linearization point
     this->triangulateSafe(this->cameras(values));
 
     if (!this->result_) {  // failed: return "empty/zero" Hessian
       if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
-        for (Matrix& m : Gs)
-          m = Matrix::Zero(DimPose, DimPose);
-        for (Vector& v : gs)
-          v = Vector::Zero(DimPose);
-        return boost::make_shared < RegularHessianFactor<DimPose>
-            > (this->keys_, Gs, gs, 0.0);
+        for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
+        for (Vector& v : gs) v = Vector::Zero(DimPose);
+        return boost::make_shared<RegularHessianFactor<DimPose>>(this->keys_,
+                                                                 Gs, gs, 0.0);
       } else {
-        throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
-                                       "only supported degeneracy mode is ZERO_ON_DEGENERACY");
+        throw std::runtime_error(
+            "SmartProjectionPoseFactorRollingShutter: "
+            "only supported degeneracy mode is ZERO_ON_DEGENERACY");
       }
     }
     // compute Jacobian given triangulated 3D Point
@@ -342,21 +380,23 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
 
     Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
 
-    // Collect all the key pairs: these are the keys that correspond to the blocks in Fs (on which we apply the Schur Complement)
+    // Collect all the key pairs: these are the keys that correspond to the
+    // blocks in Fs (on which we apply the Schur Complement)
     KeyVector nonuniqueKeys;
     for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
       nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first);
       nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second);
     }
 
-    // Build augmented Hessian (with last row/column being the information vector)
-    // Note: we need to get the augumented hessian wrt the unique keys in key_
+    // Build augmented Hessian (with last row/column being the information
+    // vector) Note: we need to get the augumented hessian wrt the unique keys
+    // in key_
     SymmetricBlockMatrix augmentedHessianUniqueKeys =
-        Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6(
+        Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>(
             Fs, E, P, b, nonuniqueKeys, this->keys_);
 
-    return boost::make_shared < RegularHessianFactor<DimPose>
-        > (this->keys_, augmentedHessianUniqueKeys);
+    return boost::make_shared<RegularHessianFactor<DimPose>>(
+        this->keys_, augmentedHessianUniqueKeys);
   }
 
   /**
@@ -365,7 +405,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   double error(const Values& values) const override {
     if (this->active(values)) {
       return this->totalReprojectionError(this->cameras(values));
-    } else { // else of active flag
+    } else {  // else of active flag
       return 0.0;
     }
   }
@@ -385,10 +425,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
 
     typename Base::Cameras cameras;
     for (size_t i = 0; i < numViews; i++) {  // for each measurement
-      const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
-      const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
+      const Pose3& w_P_body1 =
+          values.at<Pose3>(world_P_body_key_pairs_[i].first);
+      const Pose3& w_P_body2 =
+          values.at<Pose3>(world_P_body_key_pairs_[i].second);
       double interpolationFactor = alphas_[i];
-      const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
+      const Pose3& w_P_body =
+          interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
       const Pose3& body_P_cam = body_P_sensors_[i];
       const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
       cameras.emplace_back(w_P_cam, K_all_[i]);
@@ -397,44 +440,46 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
   }
 
   /**
-   * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
-   * @param values Values structure which must contain camera poses and extrinsic pose for this factor
+   * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
+   * LM)
+   * @param values Values structure which must contain camera poses and
+   * extrinsic pose for this factor
    * @return a Gaussian factor
    */
   boost::shared_ptr<GaussianFactor> linearizeDamped(
       const Values& values, const double lambda = 0.0) const {
-    // depending on flag set on construction we may linearize to different linear factors
+    // depending on flag set on construction we may linearize to different
+    // linear factors
     switch (this->params_.linearizationMode) {
       case HESSIAN:
         return this->createHessianFactor(values, lambda);
       default:
         throw std::runtime_error(
-            "SmartProjectionPoseFactorRollingShutter: unknown linearization mode");
+            "SmartProjectionPoseFactorRollingShutter: unknown linearization "
+            "mode");
     }
   }
 
   /// linearize
-  boost::shared_ptr<GaussianFactor> linearize(const Values& values) const
-          override {
+  boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const override {
     return this->linearizeDamped(values);
   }
 
  private:
   /// Serialization function
   friend class boost::serialization::access;
-  template<class ARCHIVE>
+  template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar & BOOST_SERIALIZATION_NVP(K_all_);
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar& BOOST_SERIALIZATION_NVP(K_all_);
   }
-
 };
 // end of class declaration
 
 /// traits
-template<class CAMERA>
-struct traits<SmartProjectionPoseFactorRollingShutter<CAMERA> > :
-public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA> > {
-};
+template <class CAMERA>
+struct traits<SmartProjectionPoseFactorRollingShutter<CAMERA>>
+    : public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA>> {};
 
 }  // namespace gtsam

gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp

@@ -16,34 +16,33 @@
  *  @date July 2021
  */
 
-#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/TestableAssertions.h>
-#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
-#include <gtsam/inference/Symbol.h>
+#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Point2.h>
-
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
 
 using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
 // make a realistic calibration matrix
-static double fov = 60; // degrees
-static size_t w=640,h=480;
-static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
+static double fov = 60;  // degrees
+static size_t w = 640, h = 480;
+static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h));
 
 // Create a noise model for the pixel error
 static SharedNoiseModel model(noiseModel::Unit::Create(2));
 
 // Convenience for named keys
-using symbol_shorthand::X;
 using symbol_shorthand::L;
 using symbol_shorthand::T;
+using symbol_shorthand::X;
 
 // Convenience to define common variables across many tests
 static Key poseKey1(X(1));
@@ -51,74 +50,84 @@ static Key poseKey2(X(2));
 static Key pointKey(L(1));
 static double interp_params = 0.5;
 static Point2 measurement(323.0, 240.0);
-static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
+                           Point3(0.25, -0.10, 1.0));
 
 /* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, Constructor) {
-  ProjectionFactorRollingShutter factor(measurement, interp_params, model, poseKey1, poseKey2, pointKey, K);
-}
-
-/* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, ConstructorWithTransform) {
+TEST(ProjectionFactorRollingShutter, Constructor) {
   ProjectionFactorRollingShutter factor(measurement, interp_params, model,
-                                        poseKey1, poseKey2, pointKey, K, body_P_sensor);
+                                        poseKey1, poseKey2, pointKey, K);
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, Equals ) {
-  { // factors are equal
-    ProjectionFactorRollingShutter factor1(measurement, interp_params,
-                                           model, poseKey1, poseKey2, pointKey, K);
-    ProjectionFactorRollingShutter factor2(measurement, interp_params,
-                                           model, poseKey1, poseKey2, pointKey, K);
+TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) {
+  ProjectionFactorRollingShutter factor(measurement, interp_params, model,
+                                        poseKey1, poseKey2, pointKey, K,
+                                        body_P_sensor);
+}
+
+/* ************************************************************************* */
+TEST(ProjectionFactorRollingShutter, Equals) {
+  {  // factors are equal
+    ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
+                                           poseKey1, poseKey2, pointKey, K);
+    ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
+                                           poseKey1, poseKey2, pointKey, K);
     CHECK(assert_equal(factor1, factor2));
   }
-  { // factors are NOT equal (keys are different)
-    ProjectionFactorRollingShutter factor1(measurement, interp_params,
-                                           model, poseKey1, poseKey2, pointKey, K);
-    ProjectionFactorRollingShutter factor2(measurement, interp_params,
-                                           model, poseKey1, poseKey1, pointKey, K);
-    CHECK(!assert_equal(factor1, factor2)); // not equal
+  {  // factors are NOT equal (keys are different)
+    ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
+                                           poseKey1, poseKey2, pointKey, K);
+    ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
+                                           poseKey1, poseKey1, pointKey, K);
+    CHECK(!assert_equal(factor1, factor2));  // not equal
   }
-  { // factors are NOT equal (different interpolation)
-    ProjectionFactorRollingShutter factor1(measurement, 0.1,
-                                           model, poseKey1, poseKey1, pointKey, K);
-    ProjectionFactorRollingShutter factor2(measurement, 0.5,
-                                           model, poseKey1, poseKey2, pointKey, K);
-    CHECK(!assert_equal(factor1, factor2)); // not equal
+  {  // factors are NOT equal (different interpolation)
+    ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1,
+                                           poseKey1, pointKey, K);
+    ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1,
+                                           poseKey2, pointKey, K);
+    CHECK(!assert_equal(factor1, factor2));  // not equal
   }
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) {
-  { // factors are equal
+TEST(ProjectionFactorRollingShutter, EqualsWithTransform) {
+  {  // factors are equal
     ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
-                                           poseKey1, poseKey2, pointKey, K, body_P_sensor);
+                                           poseKey1, poseKey2, pointKey, K,
+                                           body_P_sensor);
     ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
-                                           poseKey1, poseKey2, pointKey, K, body_P_sensor);
+                                           poseKey1, poseKey2, pointKey, K,
+                                           body_P_sensor);
     CHECK(assert_equal(factor1, factor2));
   }
-  { // factors are NOT equal
+  {  // factors are NOT equal
     ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
-                                           poseKey1, poseKey2, pointKey, K, body_P_sensor);
-    Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor
+                                           poseKey1, poseKey2, pointKey, K,
+                                           body_P_sensor);
+    Pose3 body_P_sensor2(
+        Rot3::RzRyRx(0.0, 0.0, 0.0),
+        Point3(0.25, -0.10, 1.0));  // rotation different from body_P_sensor
     ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
-                                           poseKey1, poseKey2, pointKey, K, body_P_sensor2);
+                                           poseKey1, poseKey2, pointKey, K,
+                                           body_P_sensor2);
     CHECK(!assert_equal(factor1, factor2));
   }
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, Error ) {
+TEST(ProjectionFactorRollingShutter, Error) {
   {
     // Create the factor with a measurement that is 3 pixels off in x
     // Camera pose corresponds to the first camera
     double t = 0.0;
-    ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K);
+    ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
+                                          poseKey2, pointKey, K);
 
     // Set the linearization point
-    Pose3 pose1(Rot3(), Point3(0,0,-6));
-    Pose3 pose2(Rot3(), Point3(0,0,-4));
+    Pose3 pose1(Rot3(), Point3(0, 0, -6));
+    Pose3 pose2(Rot3(), Point3(0, 0, -4));
     Point3 point(0.0, 0.0, 0.0);
 
     // Use the factor to calculate the error
@@ -134,11 +143,12 @@ TEST( ProjectionFactorRollingShutter, Error ) {
     // Create the factor with a measurement that is 3 pixels off in x
     // Camera pose is actually interpolated now
     double t = 0.5;
-    ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K);
+    ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
+                                          poseKey2, pointKey, K);
 
     // Set the linearization point
-    Pose3 pose1(Rot3(), Point3(0,0,-8));
-    Pose3 pose2(Rot3(), Point3(0,0,-4));
+    Pose3 pose1(Rot3(), Point3(0, 0, -8));
+    Pose3 pose2(Rot3(), Point3(0, 0, -4));
     Point3 point(0.0, 0.0, 0.0);
 
     // Use the factor to calculate the error
@@ -153,15 +163,16 @@ TEST( ProjectionFactorRollingShutter, Error ) {
   {
     // Create measurement by projecting 3D landmark
     double t = 0.3;
-    Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0));
-    Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1));
+    Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
+    Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
     Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
     PinholeCamera<Cal3_S2> camera(poseInterp, *K);
-    Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
+    Point3 point(0.0, 0.0, 5.0);  // 5 meters in front of the camera
     Point2 measured = camera.project(point);
 
     // create factor
-    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K);
+    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
+                                          poseKey2, pointKey, K);
 
     // Use the factor to calculate the error
     Vector actualError(factor.evaluateError(pose1, pose2, point));
@@ -175,19 +186,20 @@ TEST( ProjectionFactorRollingShutter, Error ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) {
+TEST(ProjectionFactorRollingShutter, ErrorWithTransform) {
   // Create measurement by projecting 3D landmark
   double t = 0.3;
-  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0));
-  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1));
+  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
+  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
   Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
-  Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1));
-  PinholeCamera<Cal3_S2> camera(poseInterp*body_P_sensor3, *K);
-  Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
+  Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1));
+  PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
+  Point3 point(0.0, 0.0, 5.0);  // 5 meters in front of the camera
   Point2 measured = camera.project(point);
 
   // create factor
-  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3);
+  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
+                                        pointKey, K, body_P_sensor3);
 
   // Use the factor to calculate the error
   Vector actualError(factor.evaluateError(pose1, pose2, point));
@@ -200,18 +212,19 @@ TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, Jacobian ) {
+TEST(ProjectionFactorRollingShutter, Jacobian) {
   // Create measurement by projecting 3D landmark
   double t = 0.3;
-  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0));
-  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1));
+  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
+  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
   Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
   PinholeCamera<Cal3_S2> camera(poseInterp, *K);
-  Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
+  Point3 point(0.0, 0.0, 5.0);  // 5 meters in front of the camera
   Point2 measured = camera.project(point);
 
   // create factor
-  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K);
+  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
+                                        pointKey, K);
 
   // Use the factor to calculate the Jacobians
   Matrix H1Actual, H2Actual, H3Actual;
@@ -222,22 +235,25 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) {
       std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
           std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                     std::placeholders::_1, std::placeholders::_2,
-                    std::placeholders::_3, boost::none, boost::none, boost::none)),
-                    pose1, pose2, point);
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      pose1, pose2, point);
 
   Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
       std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
           std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                     std::placeholders::_1, std::placeholders::_2,
-                    std::placeholders::_3, boost::none, boost::none, boost::none)),
-                    pose1, pose2, point);
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      pose1, pose2, point);
 
   Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
       std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
           std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                     std::placeholders::_1, std::placeholders::_2,
-                    std::placeholders::_3, boost::none, boost::none, boost::none)),
-                    pose1, pose2, point);
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      pose1, pose2, point);
 
   CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
   CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@@ -245,19 +261,20 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) {
+TEST(ProjectionFactorRollingShutter, JacobianWithTransform) {
   // Create measurement by projecting 3D landmark
   double t = 0.6;
-  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0));
-  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1));
+  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
+  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
   Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
-  Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1));
-  PinholeCamera<Cal3_S2> camera(poseInterp*body_P_sensor3, *K);
-  Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
+  Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1));
+  PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
+  Point3 point(0.0, 0.0, 5.0);  // 5 meters in front of the camera
   Point2 measured = camera.project(point);
 
   // create factor
-  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3);
+  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
+                                        pointKey, K, body_P_sensor3);
 
   // Use the factor to calculate the Jacobians
   Matrix H1Actual, H2Actual, H3Actual;
@@ -268,22 +285,25 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) {
       std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
           std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                     std::placeholders::_1, std::placeholders::_2,
-                    std::placeholders::_3, boost::none, boost::none, boost::none)),
-                    pose1, pose2, point);
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      pose1, pose2, point);
 
   Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
       std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
           std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                     std::placeholders::_1, std::placeholders::_2,
-                    std::placeholders::_3, boost::none, boost::none, boost::none)),
-                    pose1, pose2, point);
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      pose1, pose2, point);
 
   Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
       std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
           std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                     std::placeholders::_1, std::placeholders::_2,
-                    std::placeholders::_3, boost::none, boost::none, boost::none)),
-                    pose1, pose2, point);
+                    std::placeholders::_3, boost::none, boost::none,
+                    boost::none)),
+      pose1, pose2, point);
 
   CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
   CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@@ -291,41 +311,48 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactorRollingShutter, cheirality ) {
+TEST(ProjectionFactorRollingShutter, cheirality) {
   // Create measurement by projecting 3D landmark behind camera
   double t = 0.3;
-  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0));
-  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1));
+  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
+  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
   Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
   PinholeCamera<Cal3_S2> camera(poseInterp, *K);
-  Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera
+  Point3 point(0.0, 0.0, -5.0);  // 5 meters behind the camera
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  Point2 measured = Point2(0.0,0.0); // project would throw an exception
-  { // check that exception is thrown if we set throwCheirality = true
+  Point2 measured = Point2(0.0, 0.0);  // project would throw an exception
+  {  // check that exception is thrown if we set throwCheirality = true
     bool throwCheirality = true;
     bool verboseCheirality = true;
-    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality);
+    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
+                                          poseKey2, pointKey, K,
+                                          throwCheirality, verboseCheirality);
     CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point),
                     CheiralityException);
   }
-  { // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct
-    bool throwCheirality = false; // default
-    bool verboseCheirality = false; // default
-    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality);
+  {  // check that exception is NOT thrown if we set throwCheirality = false,
+     // and outputs are correct
+    bool throwCheirality = false;    // default
+    bool verboseCheirality = false;  // default
+    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
+                                          poseKey2, pointKey, K,
+                                          throwCheirality, verboseCheirality);
 
     // Use the factor to calculate the error
     Matrix H1Actual, H2Actual, H3Actual;
-    Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual));
+    Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual,
+                                            H2Actual, H3Actual));
 
     // The expected error is zero
-    Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera
+    Vector expectedError = Vector2::Constant(
+        2.0 * K->fx());  // this is what we return when point is behind camera
 
     // Verify we get the expected error
     CHECK(assert_equal(expectedError, actualError, 1e-9));
-    CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5));
-    CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5));
-    CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5));
+    CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5));
+    CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5));
+    CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5));
   }
 #else
   {
@@ -333,7 +360,8 @@ TEST( ProjectionFactorRollingShutter, cheirality ) {
     Point2 measured = camera.project(point);
 
     // create factor
-    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K);
+    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
+                                          poseKey2, pointKey, K);
 
     // Use the factor to calculate the Jacobians
     Matrix H1Actual, H2Actual, H3Actual;
@@ -344,22 +372,25 @@ TEST( ProjectionFactorRollingShutter, cheirality ) {
         std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
             std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                       std::placeholders::_1, std::placeholders::_2,
-                      std::placeholders::_3, boost::none, boost::none, boost::none)),
-                      pose1, pose2, point);
+                      std::placeholders::_3, boost::none, boost::none,
+                      boost::none)),
+        pose1, pose2, point);
 
     Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
         std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
             std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                       std::placeholders::_1, std::placeholders::_2,
-                      std::placeholders::_3, boost::none, boost::none, boost::none)),
-                      pose1, pose2, point);
+                      std::placeholders::_3, boost::none, boost::none,
+                      boost::none)),
+        pose1, pose2, point);
 
     Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
         std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
             std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
                       std::placeholders::_1, std::placeholders::_2,
-                      std::placeholders::_3, boost::none, boost::none, boost::none)),
-                      pose1, pose2, point);
+                      std::placeholders::_3, boost::none, boost::none,
+                      boost::none)),
+        pose1, pose2, point);
 
     CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
     CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@@ -368,8 +399,9 @@ TEST( ProjectionFactorRollingShutter, cheirality ) {
 #endif
 }
 
-
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
-

gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp

@@ -16,17 +16,19 @@
  *  @date   July 2021
  */
 
-#include "gtsam/slam/tests/smartFactorScenarios.h"
-#include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/slam/PoseTranslationPrior.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
-#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/serializationTestHelpers.h>
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/slam/PoseTranslationPrior.h>
+#include <gtsam/slam/ProjectionFactor.h>
+#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
+#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
+
 #include <boost/assign/std/map.hpp>
 #include <iostream>
+
+#include "gtsam/slam/tests/smartFactorScenarios.h"
 #define DISABLE_TIMING
 
 using namespace gtsam;
@@ -39,8 +41,8 @@ static const double sigma = 0.1;
 static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
 
 // Convenience for named keys
-using symbol_shorthand::X;
 using symbol_shorthand::L;
+using symbol_shorthand::X;
 
 // tests data
 static Symbol x1('X', 1);
@@ -48,8 +50,8 @@ static Symbol x2('X', 2);
 static Symbol x3('X', 3);
 static Symbol x4('X', 4);
 static Symbol l0('L', 0);
-static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2),
-                                   Point3(0.1, 0.0, 0.0));
+static Pose3 body_P_sensor =
+    Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0));
 
 static Point2 measurement1(323.0, 240.0);
 static Point2 measurement2(200.0, 220.0);
@@ -64,38 +66,39 @@ static double interp_factor3 = 0.5;
 namespace vanillaPoseRS {
 typedef PinholePose<Cal3_S2> Camera;
 static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
-Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1);
-Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2);
-Pose3 interp_pose3 = interpolate<Pose3>(pose_above,level_pose,interp_factor3);
+Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
+Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
+Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
 Camera cam1(interp_pose1, sharedK);
 Camera cam2(interp_pose2, sharedK);
 Camera cam3(interp_pose3, sharedK);
-}
+}  // namespace vanillaPoseRS
 
 LevenbergMarquardtParams lmParams;
-typedef SmartProjectionPoseFactorRollingShutter< PinholePose<Cal3_S2> > SmartFactorRS;
+typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
+    SmartFactorRS;
 
 /* ************************************************************************* */
-TEST( SmartProjectionPoseFactorRollingShutter, Constructor) {
+TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
   SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
 }
 
 /* ************************************************************************* */
-TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) {
+TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
   SmartProjectionParams params;
   params.setRankTolerance(rankTol);
   SmartFactorRS factor1(model, params);
 }
 
 /* ************************************************************************* */
-TEST( SmartProjectionPoseFactorRollingShutter, add) {
+TEST(SmartProjectionPoseFactorRollingShutter, add) {
   using namespace vanillaPose;
   SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
   factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor);
 }
 
 /* ************************************************************************* */
-TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
+TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
   using namespace vanillaPose;
 
   // create fake measurements
@@ -104,10 +107,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
   measurements.push_back(measurement2);
   measurements.push_back(measurement3);
 
-  std::vector<std::pair<Key,Key>> key_pairs;
-  key_pairs.push_back(std::make_pair(x1,x2));
-  key_pairs.push_back(std::make_pair(x2,x3));
-  key_pairs.push_back(std::make_pair(x3,x4));
+  std::vector<std::pair<Key, Key>> key_pairs;
+  key_pairs.push_back(std::make_pair(x1, x2));
+  key_pairs.push_back(std::make_pair(x2, x3));
+  key_pairs.push_back(std::make_pair(x3, x4));
 
   std::vector<boost::shared_ptr<Cal3_S2>> intrinsicCalibrations;
   intrinsicCalibrations.push_back(sharedK);
@@ -126,57 +129,67 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
 
   // create by adding a batch of measurements with a bunch of calibrations
   SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model));
-  factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations);
+  factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations,
+               extrinsicCalibrations);
 
   // create by adding a batch of measurements with a single calibrations
   SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model));
   factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor);
 
-  { // create equal factors and show equal returns true
+  {  // create equal factors and show equal returns true
     SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
     factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
     factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor);
     factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
 
-    EXPECT(assert_equal(*factor1, *factor2));
-    EXPECT(assert_equal(*factor1, *factor3));
+    EXPECT(factor1->equals(*factor2));
+    EXPECT(factor1->equals(*factor3));
   }
-  { // create slightly different factors (different keys) and show equal returns false
+  {  // create slightly different factors (different keys) and show equal
+     // returns false
     SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
     factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
-    factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different!
+    factor1->add(measurement2, x2, x2, interp_factor2, sharedK,
+                 body_P_sensor);  // different!
     factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
 
-    EXPECT(!assert_equal(*factor1, *factor2));
-    EXPECT(!assert_equal(*factor1, *factor3));
+    EXPECT(!factor1->equals(*factor2));
+    EXPECT(!factor1->equals(*factor3));
   }
-  { // create slightly different factors (different extrinsics) and show equal returns false
+  {  // create slightly different factors (different extrinsics) and show equal
+     // returns false
     SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
     factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
-    factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different!
+    factor1->add(measurement2, x2, x3, interp_factor2, sharedK,
+                 body_P_sensor * body_P_sensor);  // different!
     factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
 
-    EXPECT(!assert_equal(*factor1, *factor2));
-    EXPECT(!assert_equal(*factor1, *factor3));
+    EXPECT(!factor1->equals(*factor2));
+    EXPECT(!factor1->equals(*factor3));
   }
-  { // create slightly different factors (different interp factors) and show equal returns false
+  {  // create slightly different factors (different interp factors) and show
+     // equal returns false
     SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
     factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
-    factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different!
+    factor1->add(measurement2, x2, x3, interp_factor1, sharedK,
+                 body_P_sensor);  // different!
     factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
 
-    EXPECT(!assert_equal(*factor1, *factor2));
-    EXPECT(!assert_equal(*factor1, *factor3));
+    EXPECT(!factor1->equals(*factor2));
+    EXPECT(!factor1->equals(*factor3));
   }
 }
 
-static const int DimBlock = 12;  ///< size of the variable stacking 2 poses from which the observation pose is interpolated
-static const int ZDim = 2;  ///< Measurement dimension (Point2)
-typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD;  // F blocks (derivatives wrt camera)
-typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;  // vector of F blocks
+static const int DimBlock = 12;  ///< size of the variable stacking 2 poses from
+                                 ///< which the observation pose is interpolated
+static const int ZDim = 2;       ///< Measurement dimension (Point2)
+typedef Eigen::Matrix<double, ZDim, DimBlock>
+    MatrixZD;  // F blocks (derivatives wrt camera)
+typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
+    FBlocks;  // vector of F blocks
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) {
+TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
   using namespace vanillaPoseRS;
 
   // Project two landmarks into two cameras
@@ -188,7 +201,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) {
   factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId);
   factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId);
 
-  Values values; // it's a pose factor, hence these are poses
+  Values values;  // it's a pose factor, hence these are poses
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
   values.insert(x3, pose_above);
@@ -200,41 +213,56 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) {
   // Check triangulation
   factor.triangulateSafe(factor.cameras(values));
   TriangulationResult point = factor.point();
-  EXPECT(point.valid()); // check triangulated point is valid
-  EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark
+  EXPECT(point.valid());  // check triangulated point is valid
+  EXPECT(assert_equal(
+      landmark1,
+      *point));  // check triangulation result matches expected 3D landmark
 
   // Check Jacobians
   // -- actual Jacobians
   FBlocks actualFs;
   Matrix actualE;
   Vector actualb;
-  factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values);
-  EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3);
-  EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1);
+  factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb,
+                                               values);
+  EXPECT(actualE.rows() == 4);
+  EXPECT(actualE.cols() == 3);
+  EXPECT(actualb.rows() == 4);
+  EXPECT(actualb.cols() == 1);
   EXPECT(actualFs.size() == 2);
 
   // -- expected Jacobians from ProjectionFactorsRollingShutter
-  ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId);
+  ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
+                                         x2, l0, sharedK, body_P_sensorId);
   Matrix expectedF11, expectedF12, expectedE1;
-  Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1);
-  EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5));
-  EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5));
-  EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5));
-  // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError
-  EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
+  Vector expectedb1 = factor1.evaluateError(
+      level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1);
+  EXPECT(
+      assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
+  EXPECT(
+      assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
+  EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
+  // by definition computeJacobiansWithTriangulatedPoint returns minus
+  // reprojectionError
+  EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
 
-  ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId);
+  ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
+                                         x2, x3, l0, sharedK, body_P_sensorId);
   Matrix expectedF21, expectedF22, expectedE2;
-  Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2);
-  EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5));
-  EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5));
-  EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5));
-  // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError
-  EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
+  Vector expectedb2 = factor2.evaluateError(
+      pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2);
+  EXPECT(
+      assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
+  EXPECT(
+      assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
+  EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
+  // by definition computeJacobiansWithTriangulatedPoint returns minus
+  // reprojectionError
+  EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
+TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
   // also includes non-identical extrinsic calibration
   using namespace vanillaPoseRS;
 
@@ -246,9 +274,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
 
   SmartFactorRS factor(model);
   factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId);
-  factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId);
+  factor.add(level_uv_right, x2, x3, interp_factor2, sharedK,
+             body_P_sensorNonId);
 
-  Values values; // it's a pose factor, hence these are poses
+  Values values;  // it's a pose factor, hence these are poses
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
   values.insert(x3, pose_above);
@@ -256,7 +285,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
   // Perform triangulation
   factor.triangulateSafe(factor.cameras(values));
   TriangulationResult point = factor.point();
-  EXPECT(point.valid()); // check triangulated point is valid
+  EXPECT(point.valid());  // check triangulated point is valid
   Point3 landmarkNoisy = *point;
 
   // Check Jacobians
@@ -264,32 +293,48 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
   FBlocks actualFs;
   Matrix actualE;
   Vector actualb;
-  factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values);
-  EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3);
-  EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1);
+  factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb,
+                                               values);
+  EXPECT(actualE.rows() == 4);
+  EXPECT(actualE.cols() == 3);
+  EXPECT(actualb.rows() == 4);
+  EXPECT(actualb.cols() == 1);
   EXPECT(actualFs.size() == 2);
 
   // -- expected Jacobians from ProjectionFactorsRollingShutter
-  ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId);
+  ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
+                                         x2, l0, sharedK, body_P_sensorNonId);
   Matrix expectedF11, expectedF12, expectedE1;
-  Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1);
-  EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5));
-  EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5));
-  EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5));
-  // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError
-  EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
+  Vector expectedb1 =
+      factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11,
+                            expectedF12, expectedE1);
+  EXPECT(
+      assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
+  EXPECT(
+      assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
+  EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
+  // by definition computeJacobiansWithTriangulatedPoint returns minus
+  // reprojectionError
+  EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
 
-  ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId);
+  ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
+                                         x2, x3, l0, sharedK,
+                                         body_P_sensorNonId);
   Matrix expectedF21, expectedF22, expectedE2;
-  Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2);
-  EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5));
-  EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5));
-  EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5));
-  // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError
-  EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
+  Vector expectedb2 =
+      factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21,
+                            expectedF22, expectedE2);
+  EXPECT(
+      assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
+  EXPECT(
+      assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
+  EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
+  // by definition computeJacobiansWithTriangulatedPoint returns minus
+  // reprojectionError
+  EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
 
   // Check errors
-  double actualError = factor.error(values); // from smart factor
+  double actualError = factor.error(values);  // from smart factor
   NonlinearFactorGraph nfg;
   nfg.add(factor1);
   nfg.add(factor2);
@@ -299,8 +344,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
-
+TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
   using namespace vanillaPoseRS;
   Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
 
@@ -310,10 +354,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
 
   // create inputs
-  std::vector<std::pair<Key,Key>> key_pairs;
-  key_pairs.push_back(std::make_pair(x1,x2));
-  key_pairs.push_back(std::make_pair(x2,x3));
-  key_pairs.push_back(std::make_pair(x3,x1));
+  std::vector<std::pair<Key, Key>> key_pairs;
+  key_pairs.push_back(std::make_pair(x1, x2));
+  key_pairs.push_back(std::make_pair(x2, x3));
+  key_pairs.push_back(std::make_pair(x3, x1));
 
   std::vector<double> interp_factors;
   interp_factors.push_back(interp_factor1);
@@ -344,20 +388,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
   groundTruth.insert(x3, pose_above);
   DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
 
-  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
+  //  Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
-                           Point3(0.1, 0.1, 0.1)); // smaller noise
+                           Point3(0.1, 0.1, 0.1));  // smaller noise
   Values values;
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
-  // initialize third pose with some noise, we expect it to move back to original pose_above
+  // initialize third pose with some noise, we expect it to move back to
+  // original pose_above
   values.insert(x3, pose_above * noise_pose);
-  EXPECT( // check that the pose is actually noisy
-      assert_equal(
-          Pose3(
-              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-                   -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
-                   Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+  EXPECT(  // check that the pose is actually noisy
+      assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
+                              -0.0313952598, -0.000986635786, 0.0314107591,
+                              -0.999013364, -0.0313952598),
+                         Point3(0.1, -0.1, 1.9)),
+                   values.at<Pose3>(x3)));
 
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
@@ -366,11 +412,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
-  // here we replicate a test in SmartProjectionPoseFactor by setting interpolation
-  // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements)
-  // Note: this is a quite extreme test since in typical camera you would not have more than
-  // 1 measurement per landmark at each interpolated pose
+TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
+  // here we replicate a test in SmartProjectionPoseFactor by setting
+  // interpolation factors to 0 and 1 (such that the rollingShutter measurements
+  // falls back to standard pixel measurements) Note: this is a quite extreme
+  // test since in typical camera you would not have more than 1 measurement per
+  // landmark at each interpolated pose
   using namespace vanillaPose;
 
   // Default cameras for simple derivatives
@@ -423,7 +470,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
   perturbedDelta.insert(x2, delta);
   double expectedError = 2500;
 
-  // After eliminating the point, A1 and A2 contain 2-rank information on cameras:
+  // After eliminating the point, A1 and A2 contain 2-rank information on
+  // cameras:
   Matrix16 A1, A2;
   A1 << -10, 0, 0, 0, 1, 0;
   A2 << 10, 0, 1, 0, -1, 0;
@@ -449,8 +497,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
   Values values;
   values.insert(x1, pose1);
   values.insert(x2, pose2);
-  boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1
-      ->createHessianFactor(values);
+  boost::shared_ptr<RegularHessianFactor<6>> actual =
+      smartFactor1->createHessianFactor(values);
   EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
   EXPECT(assert_equal(expected, *actual, 1e-6));
   EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
@@ -458,7 +506,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
+TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
   using namespace vanillaPoseRS;
   Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
 
@@ -478,7 +526,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
   interp_factors.push_back(interp_factor2);
   interp_factors.push_back(interp_factor3);
 
-  double excludeLandmarksFutherThanDist = 1e10; //very large
+  double excludeLandmarksFutherThanDist = 1e10;  // very large
   SmartProjectionParams params;
   params.setRankTolerance(1.0);
   params.setLinearizationMode(gtsam::HESSIAN);
@@ -486,13 +534,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
   params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
   params.setEnableEPI(true);
 
-  SmartFactorRS smartFactor1(model,params);
+  SmartFactorRS smartFactor1(model, params);
   smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK);
 
-  SmartFactorRS smartFactor2(model,params);
+  SmartFactorRS smartFactor2(model, params);
   smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK);
 
-  SmartFactorRS smartFactor3(model,params);
+  SmartFactorRS smartFactor3(model, params);
   smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -509,7 +557,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
   Values values;
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
-  // initialize third pose with some noise, we expect it to move back to original pose_above
+  // initialize third pose with some noise, we expect it to move back to
+  // original pose_above
   values.insert(x3, pose_above * noise_pose);
 
   // Optimization should correct 3rd pose
@@ -520,7 +569,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) {
+TEST(SmartProjectionPoseFactorRollingShutter,
+     optimization_3poses_landmarkDistance) {
   using namespace vanillaPoseRS;
   Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
 
@@ -548,13 +598,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista
   params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
   params.setEnableEPI(false);
 
-  SmartFactorRS smartFactor1(model,params);
+  SmartFactorRS smartFactor1(model, params);
   smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK);
 
-  SmartFactorRS smartFactor2(model,params);
+  SmartFactorRS smartFactor2(model, params);
   smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK);
 
-  SmartFactorRS smartFactor3(model,params);
+  SmartFactorRS smartFactor3(model, params);
   smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -571,10 +621,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista
   Values values;
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
-  // initialize third pose with some noise, we expect it to move back to original pose_above
+  // initialize third pose with some noise, we expect it to move back to
+  // original pose_above
   values.insert(x3, pose_above * noise_pose);
 
-  // All factors are disabled (due to the distance threshold) and pose should remain where it is
+  // All factors are disabled (due to the distance threshold) and pose should
+  // remain where it is
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
   result = optimizer.optimize();
@@ -582,7 +634,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) {
+TEST(SmartProjectionPoseFactorRollingShutter,
+     optimization_3poses_dynamicOutlierRejection) {
   using namespace vanillaPoseRS;
   // add fourth landmark
   Point3 landmark4(5, -0.5, 1);
@@ -594,7 +647,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4);
-  measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10);  // add outlier
+  measurements_lmk4.at(0) =
+      measurements_lmk4.at(0) + Point2(10, 10);  // add outlier
 
   // create inputs
   std::vector<std::pair<Key, Key>> key_pairs;
@@ -608,7 +662,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
   interp_factors.push_back(interp_factor3);
 
   double excludeLandmarksFutherThanDist = 1e10;
-  double dynamicOutlierRejectionThreshold = 3;  // max 3 pixel of average reprojection error
+  double dynamicOutlierRejectionThreshold =
+      3;  // max 3 pixel of average reprojection error
 
   SmartProjectionParams params;
   params.setRankTolerance(1.0);
@@ -640,12 +695,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
   graph.addPrior(x1, level_pose, noisePrior);
   graph.addPrior(x2, pose_right, noisePrior);
 
-  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
-                           Point3(0.01, 0.01, 0.01));  // smaller noise, otherwise outlier rejection will kick in
+  Pose3 noise_pose = Pose3(
+      Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
+      Point3(0.01, 0.01,
+             0.01));  // smaller noise, otherwise outlier rejection will kick in
   Values values;
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
-  // initialize third pose with some noise, we expect it to move back to original pose_above
+  // initialize third pose with some noise, we expect it to move back to
+  // original pose_above
   values.insert(x3, pose_above * noise_pose);
 
   // Optimization should correct 3rd pose
@@ -656,8 +714,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) {
-
+TEST(SmartProjectionPoseFactorRollingShutter,
+     hessianComparedToProjFactorsRollingShutter) {
   using namespace vanillaPoseRS;
   Point2Vector measurements_lmk1;
 
@@ -683,10 +741,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
   Values values;
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
-  // initialize third pose with some noise to get a nontrivial linearization point
+  // initialize third pose with some noise to get a nontrivial linearization
+  // point
   values.insert(x3, pose_above * noise_pose);
   EXPECT(  // check that the pose is actually noisy
-      assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+      assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
+                              -0.0313952598, -0.000986635786, 0.0314107591,
+                              -0.999013364, -0.0313952598),
+                         Point3(0.1, -0.1, 1.9)),
+                   values.at<Pose3>(x3)));
 
   // linearization point for the poses
   Pose3 pose1 = level_pose;
@@ -695,8 +758,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
 
   // ==== check Hessian of smartFactor1 =====
   // -- compute actual Hessian
-  boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize(
-      values);
+  boost::shared_ptr<GaussianFactor> linearfactor1 =
+      smartFactor1->linearize(values);
   Matrix actualHessian = linearfactor1->information();
 
   // -- compute expected Hessian from manual Schur complement from Jacobians
@@ -714,46 +777,52 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
   ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1,
                                           model, x1, x2, l0, sharedK);
   Matrix H1Actual, H2Actual, H3Actual;
-  // note: b is minus the reprojection error, cf the smart factor jacobian computation
-  b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual);
+  // note: b is minus the reprojection error, cf the smart factor jacobian
+  // computation
+  b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
+                                            H2Actual, H3Actual);
   F.block<2, 6>(0, 0) = H1Actual;
   F.block<2, 6>(0, 6) = H2Actual;
   E.block<2, 3>(0, 0) = H3Actual;
 
   ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2,
                                           model, x2, x3, l0, sharedK);
-  b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual);
+  b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
+                                            H2Actual, H3Actual);
   F.block<2, 6>(2, 6) = H1Actual;
   F.block<2, 6>(2, 12) = H2Actual;
   E.block<2, 3>(2, 0) = H3Actual;
 
   ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3,
                                           model, x3, x1, l0, sharedK);
-  b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual);
+  b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
+                                            H2Actual, H3Actual);
   F.block<2, 6>(4, 12) = H1Actual;
   F.block<2, 6>(4, 0) = H2Actual;
   E.block<2, 3>(4, 0) = H3Actual;
 
   // whiten
-  F = (1/sigma) * F;
-  E = (1/sigma) * E;
-  b = (1/sigma) * b;
+  F = (1 / sigma) * F;
+  E = (1 / sigma) * E;
+  b = (1 / sigma) * b;
   //* G = F' * F - F' * E * P * E' * F
   Matrix P = (E.transpose() * E).inverse();
-  Matrix expectedHessian = F.transpose() * F
-      - (F.transpose() * E * P * E.transpose() * F);
+  Matrix expectedHessian =
+      F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
   EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
 
   // ==== check Information vector of smartFactor1 =====
   GaussianFactorGraph gfg;
   gfg.add(linearfactor1);
   Matrix actualHessian_v2 = gfg.hessian().first;
-  EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian
+  EXPECT(assert_equal(actualHessian_v2, actualHessian,
+                      1e-6));  // sanity check on hessian
 
   // -- compute actual information vector
   Vector actualInfoVector = gfg.hessian().second;
 
-  // -- compute expected information vector from manual Schur complement from Jacobians
+  // -- compute expected information vector from manual Schur complement from
+  // Jacobians
   //* g = F' * (b - E * P * E' * b)
   Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
   EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
@@ -771,9 +840,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) {
-  // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark
-  // at a single pose, a setup that occurs in multi-camera systems
+TEST(SmartProjectionPoseFactorRollingShutter,
+     hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) {
+  // in this test we make sure the fact works even if we have multiple pixel
+  // measurements of the same landmark at a single pose, a setup that occurs in
+  // multi-camera systems
 
   using namespace vanillaPoseRS;
   Point2Vector measurements_lmk1;
@@ -783,7 +854,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
 
   // create redundant measurements:
   Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
-  measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
+  measurements_lmk1_redundant.push_back(
+      measurements_lmk1.at(0));  // we readd the first measurement
 
   // create inputs
   std::vector<std::pair<Key, Key>> key_pairs;
@@ -799,17 +871,23 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
   interp_factors.push_back(interp_factor1);
 
   SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
-  smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK);
+  smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors,
+                    sharedK);
 
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                            Point3(0.1, 0.1, 0.1));  // smaller noise
   Values values;
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
-  // initialize third pose with some noise to get a nontrivial linearization point
+  // initialize third pose with some noise to get a nontrivial linearization
+  // point
   values.insert(x3, pose_above * noise_pose);
   EXPECT(  // check that the pose is actually noisy
-      assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+      assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
+                              -0.0313952598, -0.000986635786, 0.0314107591,
+                              -0.999013364, -0.0313952598),
+                         Point3(0.1, -0.1, 1.9)),
+                   values.at<Pose3>(x3)));
 
   // linearization point for the poses
   Pose3 pose1 = level_pose;
@@ -818,8 +896,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
 
   // ==== check Hessian of smartFactor1 =====
   // -- compute actual Hessian
-  boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize(
-      values);
+  boost::shared_ptr<GaussianFactor> linearfactor1 =
+      smartFactor1->linearize(values);
   Matrix actualHessian = linearfactor1->information();
 
   // -- compute expected Hessian from manual Schur complement from Jacobians
@@ -828,62 +906,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
   TriangulationResult point = smartFactor1->point();
   EXPECT(point.valid());  // check triangulated point is valid
 
-  // Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians
+  // Use standard ProjectionFactorRollingShutter factor to calculate the
+  // Jacobians
   Matrix F = Matrix::Zero(2 * 4, 6 * 3);
   Matrix E = Matrix::Zero(2 * 4, 3);
   Vector b = Vector::Zero(8);
 
   // create projection factors rolling shutter
-  ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1,
-                                          model, x1, x2, l0, sharedK);
+  ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0],
+                                          interp_factor1, model, x1, x2, l0,
+                                          sharedK);
   Matrix H1Actual, H2Actual, H3Actual;
-  // note: b is minus the reprojection error, cf the smart factor jacobian computation
-  b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual);
+  // note: b is minus the reprojection error, cf the smart factor jacobian
+  // computation
+  b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
+                                            H2Actual, H3Actual);
   F.block<2, 6>(0, 0) = H1Actual;
   F.block<2, 6>(0, 6) = H2Actual;
   E.block<2, 3>(0, 0) = H3Actual;
 
-  ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2,
-                                          model, x2, x3, l0, sharedK);
-  b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual);
+  ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1],
+                                          interp_factor2, model, x2, x3, l0,
+                                          sharedK);
+  b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
+                                            H2Actual, H3Actual);
   F.block<2, 6>(2, 6) = H1Actual;
   F.block<2, 6>(2, 12) = H2Actual;
   E.block<2, 3>(2, 0) = H3Actual;
 
-  ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3,
-                                          model, x3, x1, l0, sharedK);
-  b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual);
+  ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2],
+                                          interp_factor3, model, x3, x1, l0,
+                                          sharedK);
+  b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
+                                            H2Actual, H3Actual);
   F.block<2, 6>(4, 12) = H1Actual;
   F.block<2, 6>(4, 0) = H2Actual;
   E.block<2, 3>(4, 0) = H3Actual;
 
-  ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1,
-                                          model, x1, x2, l0, sharedK);
-  b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual);
+  ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3],
+                                          interp_factor1, model, x1, x2, l0,
+                                          sharedK);
+  b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
+                                            H2Actual, H3Actual);
   F.block<2, 6>(6, 0) = H1Actual;
   F.block<2, 6>(6, 6) = H2Actual;
   E.block<2, 3>(6, 0) = H3Actual;
 
   // whiten
-  F = (1/sigma) * F;
-  E = (1/sigma) * E;
-  b = (1/sigma) * b;
+  F = (1 / sigma) * F;
+  E = (1 / sigma) * E;
+  b = (1 / sigma) * b;
   //* G = F' * F - F' * E * P * E' * F
   Matrix P = (E.transpose() * E).inverse();
-  Matrix expectedHessian = F.transpose() * F
-      - (F.transpose() * E * P * E.transpose() * F);
+  Matrix expectedHessian =
+      F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
   EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
 
   // ==== check Information vector of smartFactor1 =====
   GaussianFactorGraph gfg;
   gfg.add(linearfactor1);
   Matrix actualHessian_v2 = gfg.hessian().first;
-  EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian
+  EXPECT(assert_equal(actualHessian_v2, actualHessian,
+                      1e-6));  // sanity check on hessian
 
   // -- compute actual information vector
   Vector actualInfoVector = gfg.hessian().second;
 
-  // -- compute expected information vector from manual Schur complement from Jacobians
+  // -- compute expected information vector from manual Schur complement from
+  // Jacobians
   //* g = F' * (b - E * P * E' * b)
   Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
   EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
@@ -902,8 +992,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
 }
 
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) {
-
+TEST(SmartProjectionPoseFactorRollingShutter,
+     optimization_3poses_measurementsFromSamePose) {
   using namespace vanillaPoseRS;
   Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
 
@@ -913,27 +1003,32 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
 
   // create inputs
-  std::vector<std::pair<Key,Key>> key_pairs;
-  key_pairs.push_back(std::make_pair(x1,x2));
-  key_pairs.push_back(std::make_pair(x2,x3));
-  key_pairs.push_back(std::make_pair(x3,x1));
+  std::vector<std::pair<Key, Key>> key_pairs;
+  key_pairs.push_back(std::make_pair(x1, x2));
+  key_pairs.push_back(std::make_pair(x2, x3));
+  key_pairs.push_back(std::make_pair(x3, x1));
 
   std::vector<double> interp_factors;
   interp_factors.push_back(interp_factor1);
   interp_factors.push_back(interp_factor2);
   interp_factors.push_back(interp_factor3);
 
-  // For first factor, we create redundant measurement (taken by the same keys as factor 1, to
-  // make sure the redundancy in the keys does not create problems)
+  // For first factor, we create redundant measurement (taken by the same keys
+  // as factor 1, to make sure the redundancy in the keys does not create
+  // problems)
   Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
-  measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
-  std::vector<std::pair<Key,Key>> key_pairs_redundant = key_pairs;
-  key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys
+  measurements_lmk1_redundant.push_back(
+      measurements_lmk1.at(0));  // we readd the first measurement
+  std::vector<std::pair<Key, Key>> key_pairs_redundant = key_pairs;
+  key_pairs_redundant.push_back(
+      key_pairs.at(0));  // we readd the first pair of keys
   std::vector<double> interp_factors_redundant = interp_factors;
-  interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor
+  interp_factors_redundant.push_back(
+      interp_factors.at(0));  // we readd the first interp factor
 
   SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
-  smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK);
+  smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
+                    interp_factors_redundant, sharedK);
 
   SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model));
   smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK);
@@ -956,20 +1051,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF
   groundTruth.insert(x3, pose_above);
   DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
 
-  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
+  //  Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
-                           Point3(0.1, 0.1, 0.1)); // smaller noise
+                           Point3(0.1, 0.1, 0.1));  // smaller noise
   Values values;
   values.insert(x1, level_pose);
   values.insert(x2, pose_right);
-  // initialize third pose with some noise, we expect it to move back to original pose_above
+  // initialize third pose with some noise, we expect it to move back to
+  // original pose_above
   values.insert(x3, pose_above * noise_pose);
-  EXPECT( // check that the pose is actually noisy
-      assert_equal(
-          Pose3(
-              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-                   -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
-                   Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+  EXPECT(  // check that the pose is actually noisy
+      assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
+                              -0.0313952598, -0.000986635786, 0.0314107591,
+                              -0.999013364, -0.0313952598),
+                         Point3(0.1, -0.1, 1.9)),
+                   values.at<Pose3>(x3)));
 
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
@@ -980,11 +1077,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF
 #ifndef DISABLE_TIMING
 #include <gtsam/base/timing.h>
 // -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0)
-//|   -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0)
-//|   -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0)
+//|   -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min:
+// 0 max: 0) |   -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02
+// children, min: 0 max: 0)
 /* *************************************************************************/
-TEST( SmartProjectionPoseFactorRollingShutter, timing ) {
-
+TEST(SmartProjectionPoseFactorRollingShutter, timing) {
   using namespace vanillaPose;
 
   // Default cameras for simple derivatives
@@ -1007,14 +1104,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) {
 
   size_t nrTests = 1000;
 
-  for(size_t i = 0; i<nrTests; i++){
+  for (size_t i = 0; i < nrTests; i++) {
     SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model));
     double interp_factor = 0;  // equivalent to measurement taken at pose 1
-    smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple,
-                       body_P_sensorId);
+    smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor,
+                       sharedKSimple, body_P_sensorId);
     interp_factor = 1;  // equivalent to measurement taken at pose 2
-    smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple,
-                       body_P_sensorId);
+    smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor,
+                       sharedKSimple, body_P_sensorId);
 
     Values values;
     values.insert(x1, pose1);
@@ -1024,7 +1121,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) {
     gttoc_(SF_RS_LINEARIZE);
   }
 
-  for(size_t i = 0; i<nrTests; i++){
+  for (size_t i = 0; i < nrTests; i++) {
     SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple));
     smartFactor->add(measurements_lmk1[0], x1);
     smartFactor->add(measurements_lmk1[1], x2);
@@ -1046,4 +1143,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

python/CMakeLists.txt

@@ -61,6 +61,7 @@ set(interface_headers
     ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i
     ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i
     ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i
+    ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i
 )
 
 

python/gtsam/preamble/basis.h

@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */

python/gtsam/specializations/basis.h

@@ -0,0 +1,12 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */