amended

gtsam/geometry/Pose3.cpp

@@ -423,8 +423,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
   return os;
 }
 
-Pose3 pose3_interp(const Pose3& X, const Pose3& Y, double t, Matrix& Hx, Matrix& Hy) {
-  return X.interp(t, Y, Hx, Hy);
-}
-
 } // namespace gtsam

gtsam/geometry/Pose3.h

@@ -353,15 +353,6 @@ public:
     return std::make_pair(0, 2);
   }
 
-  /**
-   * @brief Spherical Linear interpolation between *this and other
-   * @param s a value between 0 and 1.5
-   * @param other final point of iterpolation geodesic on manifold
-   * @param Hx jacobian of the interpolation on this
-   & @param Hy jacobian of the interpolation on other
-   */
-  Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, OptionalJacobian<6, 6> Hy = boost::none) const;
-
   /// Output stream operator
   GTSAM_EXPORT
   friend std::ostream &operator<<(std::ostream &os, const Pose3& p);

gtsam_unstable/slam/ProjectionFactorRollingShutter.h

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file RollingShutterProjectionFactor.h
+ * @file ProjectionFactorRollingShutter.h
- * @brief Basic bearing factor from 2D measurement for rolling shutter cameras
+ * @brief Basic projection factor for rolling shutter cameras
  * @author Yotam Stern
  */
 
@@ -34,7 +34,7 @@ namespace gtsam {
    * @addtogroup SLAM
    */
 
-  class RollingShutterProjectionFactor: public NoiseModelFactor3<Pose3, Pose3, Point3> {
+  class ProjectionFactorRollingShutter: public NoiseModelFactor3<Pose3, Pose3, Point3> {
   protected:
 
     // Keep a copy of measurement and calibration for I/O
@@ -53,13 +53,13 @@ namespace gtsam {
     typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
 
     /// shorthand for this class
-    typedef RollingShutterProjectionFactor This;
+    typedef ProjectionFactorRollingShutter This;
 
     /// shorthand for a smart pointer to a factor
     typedef boost::shared_ptr<This> shared_ptr;
 
     /// Default constructor
-  RollingShutterProjectionFactor() :
+  ProjectionFactorRollingShutter() :
       measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) {
   }
 
@@ -74,7 +74,7 @@ namespace gtsam {
      * @param K shared pointer to the constant calibration
      * @param body_P_sensor is the transform from body to sensor frame (default identity)
      */
-    RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model,
+    ProjectionFactorRollingShutter(const Point2& measured, double interp_param, 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), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor),
@@ -93,7 +93,7 @@ namespace gtsam {
      * @param verboseCheirality determines whether exceptions are printed for Cheirality
      * @param body_P_sensor is the transform from body to sensor frame  (default identity)
      */
-    RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model,
+    ProjectionFactorRollingShutter(const Point2& measured, double interp_param, 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) :
@@ -101,7 +101,7 @@ namespace gtsam {
           throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
 
     /** Virtual destructor */
-    virtual ~RollingShutterProjectionFactor() {}
+    virtual ~ProjectionFactorRollingShutter() {}
 
     /// @return a deep copy of this factor
     virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@@ -114,7 +114,7 @@ namespace gtsam {
      * @param keyFormatter optional formatter useful for printing Symbols
      */
     void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-      std::cout << s << "RollingShutterProjectionFactor, z = ";
+      std::cout << s << "ProjectionFactorRollingShutter, z = ";
       traits<Point2>::Print(measured_);
       std::cout << " rolling shutter interpolation param = " << interp_param_;
       if(this->body_P_sensor_)
@@ -141,7 +141,7 @@ namespace gtsam {
       gtsam::Matrix Hprj;
 
       //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2);
-      pose = pose_a.interp(interp_param_, pose_b, H1, H2);
+      pose = interpolate<Pose3>(pose_a, pose_b, interp_param_, H1, H2);
       try {
         if(body_P_sensor_) {
           if(H1 && H2) {
@@ -211,4 +211,4 @@ namespace gtsam {
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   }; // rolling shutter projection factor
-} //namespace gtsam
+} //namespace gtsam

gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp

@@ -0,0 +1,233 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testProjectionFactorRollingShutter.cpp
+ *  @brief Unit tests for ProjectionFactorRollingShutter Class
+ *  @author Luca Carlone
+ *  @date July 2021
+ */
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
+#include <gtsam/inference/Symbol.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>
+
+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));
+
+// 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;
+
+//typedef ProjectionFactorPPP<Pose3, Point3> TestProjectionFactor;
+
+///// traits
+//namespace gtsam {
+//template<>
+//struct traits<TestProjectionFactor> : public Testable<TestProjectionFactor> {
+//};
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, nonStandard ) {
+//  ProjectionFactorPPP<Pose3, Point3, Cal3DS2> f;
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, Constructor) {
+//  Key poseKey(X(1));
+//  Key transformKey(T(1));
+//  Key pointKey(L(1));
+//
+//  Point2 measurement(323.0, 240.0);
+//
+//  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, ConstructorWithTransform) {
+//  Key poseKey(X(1));
+//  Key transformKey(T(1));
+//  Key pointKey(L(1));
+//
+//  Point2 measurement(323.0, 240.0);
+//  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, Equals ) {
+//  // Create two identical factors and make sure they're equal
+//  Point2 measurement(323.0, 240.0);
+//
+//  TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
+//  TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
+//
+//  CHECK(assert_equal(factor1, factor2));
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, EqualsWithTransform ) {
+//  // Create two identical factors and make sure they're equal
+//  Point2 measurement(323.0, 240.0);
+//  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+//
+//  TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
+//  TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
+//
+//  CHECK(assert_equal(factor1, factor2));
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, Error ) {
+//  // Create the factor with a measurement that is 3 pixels off in x
+//  Key poseKey(X(1));
+//  Key transformKey(T(1));
+//  Key pointKey(L(1));
+//  Point2 measurement(323.0, 240.0);
+//  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
+//
+//  // Set the linearization point
+//  Pose3 pose(Rot3(), Point3(0,0,-6));
+//  Point3 point(0.0, 0.0, 0.0);
+//
+//  // Use the factor to calculate the error
+//  Vector actualError(factor.evaluateError(pose, Pose3(), point));
+//
+//  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
+//  Vector expectedError = Vector2(-3.0, 0.0);
+//
+//  // Verify we get the expected error
+//  CHECK(assert_equal(expectedError, actualError, 1e-9));
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, ErrorWithTransform ) {
+//  // Create the factor with a measurement that is 3 pixels off in x
+//  Key poseKey(X(1));
+//  Key transformKey(T(1));
+//  Key pointKey(L(1));
+//  Point2 measurement(323.0, 240.0);
+//  Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+//  TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K);
+//
+//  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
+//  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
+//  Point3 point(0.0, 0.0, 0.0);
+//
+//  // Use the factor to calculate the error
+//  Vector actualError(factor.evaluateError(pose, transform, point));
+//
+//  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
+//  Vector expectedError = Vector2(-3.0, 0.0);
+//
+//  // Verify we get the expected error
+//  CHECK(assert_equal(expectedError, actualError, 1e-9));
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, Jacobian ) {
+//  // Create the factor with a measurement that is 3 pixels off in x
+//  Key poseKey(X(1));
+//  Key transformKey(T(1));
+//  Key pointKey(L(1));
+//  Point2 measurement(323.0, 240.0);
+//  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
+//
+//  // Set the linearization point
+//  Pose3 pose(Rot3(), Point3(0,0,-6));
+//  Point3 point(0.0, 0.0, 0.0);
+//
+//  // Use the factor to calculate the Jacobians
+//  Matrix H1Actual, H2Actual, H3Actual;
+//  factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual);
+//
+//  // The expected Jacobians
+//  Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
+//  Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
+//
+//  // Verify the Jacobians are correct
+//  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
+//  CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
+//
+//  // Verify H2 with numerical derivative
+//  Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
+//      std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
+//          std::bind(&TestProjectionFactor::evaluateError, &factor,
+//                    std::placeholders::_1, std::placeholders::_2,
+//                    std::placeholders::_3, boost::none, boost::none,
+//                    boost::none)),
+//      pose, Pose3(), point);
+//
+//  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
+//}
+//
+///* ************************************************************************* */
+//TEST( ProjectionFactorPPP, JacobianWithTransform ) {
+//  // Create the factor with a measurement that is 3 pixels off in x
+//  Key poseKey(X(1));
+//  Key transformKey(T(1));
+//  Key pointKey(L(1));
+//  Point2 measurement(323.0, 240.0);
+//  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+//  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
+//
+//  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
+//  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
+//  Point3 point(0.0, 0.0, 0.0);
+//
+//  // Use the factor to calculate the Jacobians
+//  Matrix H1Actual, H2Actual, H3Actual;
+//  factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual);
+//
+//  // The expected Jacobians
+//  Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
+//  Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();
+//
+//  // Verify the Jacobians are correct
+//  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
+//  CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
+//
+//  // Verify H2 with numerical derivative
+//  Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
+//      std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
+//          std::bind(&TestProjectionFactor::evaluateError, &factor,
+//                    std::placeholders::_1, std::placeholders::_2,
+//                    std::placeholders::_3, boost::none, boost::none,
+//                    boost::none)),
+//      pose, body_P_sensor, point);
+//
+//  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
+//
+//
+//}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
+