Standard formatting

gtsam/geometry/PinholeCamera.h

@@ -39,11 +39,11 @@ private:
   GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
 
   // Get dimensions of calibration type at compile time
-  static const int DimK = FixedDimension<Calibration>::value;
+static  const int DimK = FixedDimension<Calibration>::value;
 
 public:
 
-  enum { dimension = 6 + DimK };
+  enum {dimension = 6 + DimK};
 
   /// @name Standard Constructors
   /// @{
@@ -292,7 +292,7 @@ public:
 
     if (!Dpose && !Dpoint && !Dcal) {
       const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
-      const Point2 pn = project_to_camera(pc); // project the point to the camera
+      const Point2 pn = project_to_camera(pc);// project the point to the camera
       return K_.uncalibrate(pn);
     }
 
@@ -305,11 +305,11 @@ public:
     Dpc_pose.leftCols<3>() = Dpc_rot;
 
     // camera to normalized image coordinate
-    Matrix23 Dpn_pc; // 2*3
+    Matrix23 Dpn_pc;// 2*3
     const Point2 pn = project_to_camera(pc, Dpn_pc);
 
     // uncalibration
-    Matrix2 Dpi_pn; // 2*2
+    Matrix2 Dpi_pn;// 2*2
     const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
 
     // chain the Jacobian matrices
@@ -317,7 +317,7 @@ public:
     if (Dpose)
     *Dpose = Dpi_pc * Dpc_pose;
     if (Dpoint)
-      *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
+    *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only)
     return pi;
   }
 
@@ -346,7 +346,7 @@ public:
 
       if (Dcamera) { // TODO why does leftCols<6>() not compile ??
         calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
-        (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
+        (*Dcamera).rightCols(DimK) = Dcal;// Jacobian wrt calib
       }
       if (Dpoint) {
         calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
@@ -467,7 +467,7 @@ private:
     Matrix26 Dpn_pose;
     Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
     assert(Dpose.rows()==2 && Dpose.cols()==6);
-    const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
+    const_cast<Eigen::MatrixBase<Derived>&>(Dpose) =//
     Dpi_pn * Dpn_pose;
   }
 
@@ -485,12 +485,12 @@ private:
     // optimized version of derivatives, see CalibratedCamera.nb
     const double u = pn.x(), v = pn.y();
     Matrix23 Dpn_point;
-    Dpn_point << //
+    Dpn_point <<//
-        R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
+    R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2),//
     /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
     Dpn_point *= d;
     assert(Dpoint.rows()==2 && Dpoint.cols()==3);
-    const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
+    const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) =//
     Dpi_pn * Dpn_point;
   }
 
@@ -504,11 +504,14 @@ private:
 
 };
 
+template<typename Calibration>
+struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
+    PinholeCamera<Calibration> > {
+};
 
 template<typename Calibration>
-struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
+struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
-
+    PinholeCamera<Calibration> > {
-template<typename Calibration>
+};
-struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
 
 } // \ gtsam

gtsam/geometry/PinholePose.h

@@ -40,7 +40,9 @@ private:
 
 public:
 
-  enum { dimension = 6 };
+  enum {
+    dimension = 6
+  };
 
   /// @name Standard Constructors
   /// @{
@@ -70,8 +72,8 @@ public:
    * (theta 0 = looking in direction of positive X axis)
    * @param height camera height
    */
-  static PinholePose Level(const boost::shared_ptr<Calibration>& K, const Pose2& pose2,
+  static PinholePose Level(const boost::shared_ptr<Calibration>& K,
-      double height) {
+      const Pose2& pose2, double height) {
     const double st = sin(pose2.theta()), ct = cos(pose2.theta());
     const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
     const Rot3 wRc(x, y, z);
@@ -235,8 +237,7 @@ public:
    *  @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
    *  @param Dpoint is the Jacobian w.r.t. point3
    */
-  Point2 project2(
+  Point2 project2(const Point3& pw,
-      const Point3& pw,
       OptionalJacobian<2, 6> Dcamera = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none) const {
 
@@ -388,11 +389,14 @@ private:
 
 };
 
+template<typename Calibration>
+struct traits<PinholePose<Calibration> > : public internal::Manifold<
+    PinholePose<Calibration> > {
+};
 
 template<typename Calibration>
-struct traits< PinholePose<Calibration> > : public internal::Manifold<PinholePose<Calibration> > {};
+struct traits<const PinholePose<Calibration> > : public internal::Manifold<
-
+    PinholePose<Calibration> > {
-template<typename Calibration>
+};
-struct traits< const PinholePose<Calibration> > : public internal::Manifold<PinholePose<Calibration> > {};
 
 } // \ gtsam