testing mode: still stuck, getting closer to the problem

gtsam/geometry/SphericalCamera.cpp

@@ -60,6 +60,21 @@ Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose,
   return pu;
 }
 
+/* ************************************************************************* */
+Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose,
+    OptionalJacobian<2, 2> Dpoint) const {
+
+  Matrix23 Dtf_rot;
+  Matrix2 Dtf_point; // calculated by transformTo if needed
+  const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0, Dpoint ? &Dtf_point : 0);
+
+  if (Dpose)
+    *Dpose << Dtf_rot, Matrix::Zero(2,3); //2x6 (translation part is zero)
+  if (Dpoint)
+    *Dpoint = Dtf_point; //2x2
+  return pu;
+}
+
 /* ************************************************************************* */
 Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
   return pose().transformFrom(depth * pu);

gtsam/geometry/SphericalCamera.h

@@ -31,15 +31,20 @@ namespace gtsam {
 
 class GTSAM_EXPORT EmptyCal {
  protected:
-  double d_ = 0;
+  Matrix3 K_;
  public:
+
   ///< shared pointer to calibration object
   EmptyCal()
-      : d_(0) {
+      : K_(Matrix3::Identity()) {
   }
   /// Default destructor
   virtual ~EmptyCal() = default;
   using shared_ptr = boost::shared_ptr<EmptyCal>;
+  void print(const std::string& s) const {
+    std::cout << "empty calibration: " <<  s << std::endl;
+  }
+  Matrix3 K() const {return K_;}
 };
 
 /**
@@ -159,6 +164,14 @@ class GTSAM_EXPORT SphericalCamera {
   Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
                  OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
+  /** Project point into the image
+   * (note: there is no CheiralityException for a spherical camera)
+   * @param point 3D direction in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
+                 OptionalJacobian<2, 2> 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;