testing mode: step 1: need to figure out the manifold stuff

gtsam/geometry/PinholePose.h

@@ -121,6 +121,13 @@ public:
     return _project(pw, Dpose, Dpoint, Dcal);
   }
 
+  /// project a 3D point from world coordinates into the image
+  Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none,
+      OptionalJacobian<2, DimK> Dcal = boost::none) const {
+    return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
+  }
+
   /// project a point at infinity from world coordinates into the image
   Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none,

gtsam/geometry/SphericalCamera.cpp

@@ -65,11 +65,25 @@ Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
   return pose().transformFrom(depth * pu);
 }
 
+/* ************************************************************************* */
+Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const {
+  return pose().rotation().rotate(p);
+}
+
 /* ************************************************************************* */
 Unit3 SphericalCamera::project(const Point3& point,
     OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
   return project2(point, Dcamera, Dpoint);
 }
 
+/* ************************************************************************* */
+Vector2 SphericalCamera::reprojectionError(const Point3& point, const Unit3& measured,
+                                           OptionalJacobian<2, 6> Dpose,
+                                           OptionalJacobian<2, 3> Dpoint) const {
+  // onmanifold version of: camera.project(point) - zi
+  std::cout << "SphericalCam:reprojectionError fix jacobians " << std::endl;
+  return measured.localCoordinates( project2(point, Dpose, Dpoint) );
+}
+
 /* ************************************************************************* */
 }

gtsam/geometry/SphericalCamera.h

@@ -29,6 +29,19 @@
 
 namespace gtsam {
 
+class GTSAM_EXPORT EmptyCal {
+ protected:
+  double d_ = 0;
+ public:
+  ///< shared pointer to calibration object
+  EmptyCal()
+      : d_(0) {
+  }
+  /// Default destructor
+  virtual ~EmptyCal() = default;
+  using shared_ptr = boost::shared_ptr<EmptyCal>;
+};
+
 /**
  * A spherical camera class that has a Pose3 and measures bearing vectors
  * @addtogroup geometry
@@ -36,61 +49,69 @@ namespace gtsam {
  */
 class GTSAM_EXPORT SphericalCamera {
 
-public:
+ public:
 
   enum {
     dimension = 6
   };
 
-  typedef Point2 Measurement;
+  typedef Unit3 Measurement;
-  typedef Point2Vector MeasurementVector;
+  typedef std::vector<Unit3> MeasurementVector;
+  typedef EmptyCal CalibrationType;
 
-private:
+ private:
 
   Pose3 pose_;  ///< 3D pose of camera
 
-protected:
+ protected:
 
-  /// @name Derivatives
+  EmptyCal::shared_ptr emptyCal_;
-  /// @{
 
-//  /**
+ public:
-//   * Calculate Jacobian with respect to pose
-//   * @param pn projection in normalized coordinates
-//   * @param d disparity (inverse depth)
-//   */
-//  static Matrix26 Dpose(const Point2& pn, double d);
-//
-//  /**
-//   * Calculate Jacobian with respect to point
-//   * @param pn projection in normalized coordinates
-//   * @param d disparity (inverse depth)
-//   * @param Rt transposed rotation matrix
-//   */
-//  static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
-//
-//  /// @}
-
-public:
 
   /// @}
   /// @name Standard Constructors
   /// @{
 
   /// Default constructor
-  SphericalCamera() {}
+  SphericalCamera()
+      : pose_(Pose3::identity()),
+        emptyCal_(boost::make_shared<EmptyCal>()) {
+  }
 
   /// Constructor with pose
-  explicit SphericalCamera(const Pose3& pose) : pose_(pose) {}
+  explicit SphericalCamera(const Pose3& pose)
+      : pose_(pose),
+        emptyCal_(boost::make_shared<EmptyCal>()) {
+  }
+
+  /// Constructor with empty intrinsics (needed for smart factors)
+  explicit SphericalCamera(const Pose3& pose,
+                           const boost::shared_ptr<EmptyCal>& cal)
+      : pose_(pose),
+        emptyCal_(boost::make_shared<EmptyCal>()) {
+  }
 
   /// @}
   /// @name Advanced Constructors
   /// @{
-  explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
+  explicit SphericalCamera(const Vector& v)
+      : pose_(Pose3::Expmap(v)) {
+  }
 
   /// Default destructor
   virtual ~SphericalCamera() = default;
 
+  /// return shared pointer to calibration
+  const boost::shared_ptr<EmptyCal>& sharedCalibration() const {
+    return emptyCal_;
+  }
+
+  /// return calibration
+  const EmptyCal& calibration() const {
+    return *emptyCal_;
+  }
+
   /// @}
   /// @name Testable
   /// @{
@@ -120,8 +141,8 @@ public:
     return pose_.translation();
   }
 
-//  /// return pose, with derivative
+  //  /// return pose, with derivative
-//  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
+  //  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
 
   /// @}
   /// @name Transformations and measurement functions
@@ -135,19 +156,30 @@ public:
    * @param point 3D point in world coordinates
    * @return the intrinsic coordinates of the projected point
    */
-  Unit3 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
+  Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
-      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
+                 OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
   Point3 backproject(const Unit3& p, const double depth) const;
 
+  /// backproject point at infinity
+  Unit3 backprojectPointAtInfinity(const Unit3& p) const;
+
   /** Project point into the image
    * (note: there is no CheiralityException for a spherical camera)
    * @param point 3D point in world coordinates
    * @return the intrinsic coordinates of the projected point
    */
-  Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose =
+  Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
-      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
+                OptionalJacobian<2, 3> Dpoint = boost::none) const;
+
+  /** Compute reprojection error for a given 3D point in world coordinates
+   * @param point 3D point in world coordinates
+   * @return the tangent space error between the projection and the measurement
+   */
+  Vector2 reprojectionError(const Point3& point, const Unit3& measured,
+                            OptionalJacobian<2, 6> Dpose = boost::none,
+                            OptionalJacobian<2, 3> Dpoint = boost::none) const;
   /// @}
 
   /// move a cameras according to d
@@ -165,8 +197,7 @@ public:
     return SphericalCamera(Pose3::identity());  // assumes that the default constructor is valid
   }
 
-
+ private:
-private:
 
   /** Serialization function */
   friend class boost::serialization::access;
@@ -177,4 +208,12 @@ private:
 };
 // end of class SphericalCamera
 
+template<>
+struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {
+};
+
+template<>
+struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {
+};
+
 }

gtsam/geometry/triangulation.h

@@ -474,8 +474,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 #endif
         // Check reprojection error
         if (params.dynamicOutlierRejectionThreshold > 0) {
-          const Point2& zi = measured.at(i);
+          const typename CAMERA::Measurement& zi = measured.at(i);
-          Point2 reprojectionError(camera.project(point) - zi);
+          Point2 reprojectionError = camera.reprojectionError(point, zi);
           maxReprojError = std::max(maxReprojError, reprojectionError.norm());
         }
         i += 1;

gtsam/slam/SmartProjectionFactorP.h

@@ -53,6 +53,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
   typedef SmartProjectionFactor<CAMERA> Base;
   typedef SmartProjectionFactorP<CAMERA> This;
   typedef typename CAMERA::CalibrationType CALIBRATION;
+  typedef typename CAMERA::Measurement MEASUREMENT;
+  typedef typename CAMERA::MeasurementVector MEASUREMENTS;
 
   static const int DimPose = 6;  ///< Pose3 dimension
   static const int ZDim = 2;  ///< Measurement dimension (Point2)
@@ -108,7 +110,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
    * @param K (fixed) camera intrinsic calibration
    * @param body_P_sensor (fixed) camera extrinsic calibration
    */
-  void add(const Point2& measured, const Key& poseKey,
+  void add(const MEASUREMENT& measured, const Key& poseKey,
            const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor =
                Pose3::identity()) {
     // store measurement and key
@@ -133,7 +135,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
    * @param Ks vector of (fixed) intrinsic calibration objects
    * @param body_P_sensors vector of (fixed) extrinsic calibration objects
    */
-  void add(const Point2Vector& measurements, const std::vector<Key>& poseKeys,
+  void add(const MEASUREMENTS& measurements, const std::vector<Key>& poseKeys,
            const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
            const std::vector<Pose3> body_P_sensors = std::vector<Pose3>()) {
     assert(poseKeys.size() == measurements.size());

gtsam/slam/tests/smartFactorScenarios.h

@@ -23,6 +23,7 @@
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/SphericalCamera.h>
 
 using namespace std;
 using namespace gtsam;
@@ -123,6 +124,19 @@ Camera cam1(level_pose, sharedBundlerK);
 Camera cam2(pose_right, sharedBundlerK);
 Camera cam3(pose_above, sharedBundlerK);
 }
+
+/* ************************************************************************* */
+// sphericalCamera
+namespace sphericalCamera {
+typedef SphericalCamera Camera;
+typedef SmartProjectionFactorP<Camera> SmartFactorP;
+static EmptyCal::shared_ptr emptyK;
+Camera level_camera(level_pose);
+Camera level_camera_right(pose_right);
+Camera cam1(level_pose);
+Camera cam2(pose_right);
+Camera cam3(pose_above);
+}
 /* *************************************************************************/
 
 template<class CAMERA>
@@ -137,9 +151,9 @@ CAMERA perturbCameraPose(const CAMERA& camera) {
 template<class CAMERA>
 void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2,
     const CAMERA& cam3, Point3 landmark, typename CAMERA::MeasurementVector& measurements_cam) {
-  Point2 cam1_uv1 = cam1.project(landmark);
+  typename CAMERA::Measurement cam1_uv1 = cam1.project(landmark);
-  Point2 cam2_uv1 = cam2.project(landmark);
+  typename CAMERA::Measurement cam2_uv1 = cam2.project(landmark);
-  Point2 cam3_uv1 = cam3.project(landmark);
+  typename CAMERA::Measurement cam3_uv1 = cam3.project(landmark);
   measurements_cam.push_back(cam1_uv1);
   measurements_cam.push_back(cam2_uv1);
   measurements_cam.push_back(cam3_uv1);

gtsam/slam/tests/testSmartProjectionFactorP.cpp

@@ -1086,6 +1086,133 @@ TEST( SmartProjectionFactorP, timing ) {
 }
 #endif
 
+/* *************************************************************************/
+TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
+
+  using namespace sphericalCamera;
+  Camera::MeasurementVector measurements_lmk1, measurements_lmk2, measurements_lmk3;
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
+  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
+  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3);
+
+  // create inputs
+  std::vector<Key> keys;
+  keys.push_back(x1);
+  keys.push_back(x2);
+  keys.push_back(x3);
+
+  std::vector<EmptyCal::shared_ptr> emptyKs;
+  emptyKs.push_back(emptyK);
+  emptyKs.push_back(emptyK);
+  emptyKs.push_back(emptyK);
+
+  SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
+  smartFactor1->add(measurements_lmk1, keys, emptyKs);
+
+//  SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model));
+//  smartFactor2->add(measurements_lmk2, keys, sharedKs);
+//
+//  SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model));
+//  smartFactor3->add(measurements_lmk3, keys, sharedKs);
+//
+//  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+//
+//  NonlinearFactorGraph graph;
+//  graph.push_back(smartFactor1);
+//  graph.push_back(smartFactor2);
+//  graph.push_back(smartFactor3);
+//  graph.addPrior(x1, level_pose, noisePrior);
+//  graph.addPrior(x2, pose_right, noisePrior);
+//
+//  Values groundTruth;
+//  groundTruth.insert(x1, level_pose);
+//  groundTruth.insert(x2, pose_right);
+//  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 / 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, 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)));
+//
+//  Values result;
+//  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
+//  result = optimizer.optimize();
+//  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
+}
+
+//#ifndef DISABLE_TIMING
+//#include <gtsam/base/timing.h>
+//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor
+////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0)
+////|   -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0)
+////|   -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0)
+///* *************************************************************************/
+//TEST( SmartProjectionFactorP, timing ) {
+//
+//  using namespace vanillaPose;
+//
+//  // Default cameras for simple derivatives
+//  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
+//
+//  Rot3 R = Rot3::identity();
+//  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
+//  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
+//  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
+//  Pose3 body_P_sensorId = Pose3::identity();
+//
+//  // one landmarks 1m in front of camera
+//  Point3 landmark1(0, 0, 10);
+//
+//  Point2Vector measurements_lmk1;
+//
+//  // Project 2 landmarks into 2 cameras
+//  measurements_lmk1.push_back(cam1.project(landmark1));
+//  measurements_lmk1.push_back(cam2.project(landmark1));
+//
+//  size_t nrTests = 1000;
+//
+//  for(size_t i = 0; i<nrTests; i++){
+//    SmartFactorP::shared_ptr smartFactorP(new SmartFactorP(model));
+//    smartFactorP->add(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId);
+//    smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId);
+//
+//    Values values;
+//    values.insert(x1, pose1);
+//    values.insert(x2, pose2);
+//    gttic_(SmartFactorP_LINEARIZE);
+//    smartFactorP->linearize(values);
+//    gttoc_(SmartFactorP_LINEARIZE);
+//  }
+//
+//  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);
+//
+//    Values values;
+//    values.insert(x1, pose1);
+//    values.insert(x2, pose2);
+//    gttic_(SmartPoseFactor_LINEARIZE);
+//    smartFactor->linearize(values);
+//    gttoc_(SmartPoseFactor_LINEARIZE);
+//  }
+//  tictoc_print_();
+//}
+//#endif
+
 /* ************************************************************************* */
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");