Consistent and better formatting

gtsam/geometry/Cal3Bundler.cpp

@@ -15,11 +15,11 @@
  * @author ydjian
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3Bundler.h>
 
 namespace gtsam {
 
@@ -39,9 +39,7 @@ Vector4 Cal3Bundler::k() const {
 }
 
 /* ************************************************************************* */
-Vector3 Cal3Bundler::vector() const {
+Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
-  return Vector3(fx_, k1_, k2_);
-}
 
 /* ************************************************************************* */
 std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
@@ -52,7 +50,8 @@ std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
 
 /* ************************************************************************* */
 void Cal3Bundler::print(const std::string& s) const {
-  gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(), s + ".K");
+  gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(),
+               s + ".K");
 }
 
 /* ************************************************************************* */
@@ -64,8 +63,8 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p, //
+Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
-    OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
+                                OptionalJacobian<2, 2> Dp) const {
   //  r = x² + y²;
   //  g = (1 + k(1)*r + k(2)*r²);
   //  pi(:,i) = g * pn(:,i)
@@ -93,23 +92,22 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi,
+Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
-                              OptionalJacobian<2, 3> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
   // Copied from Cal3DS2
   // but specialized with k1, k2 non-zero only and fx=fy and s=0
   double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
   const Point2 invKPi(x, y);
 
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
+  // initialize by ignoring the distortion at all, might be problematic for
+  // pixels around boundary
   Point2 pn(x, y);
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
   for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (distance2(uncalibrate(pn), pi) <= tol_)
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
-      break;
     const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
@@ -118,7 +116,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
 
   if (iteration >= maxIterations)
     throw std::runtime_error(
-        "Cal3Bundler::calibrate fails to converge. need a better initialization");
+        "Cal3Bundler::calibrate fails to converge. need a better "
+        "initialization");
 
   calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
 

gtsam/geometry/Cal3Bundler.h

@@ -30,7 +30,6 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3Bundler : public Cal3 {
-
  private:
   double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
   double tol_ = 1e-5;             ///< tolerance value when calibrating
@@ -40,7 +39,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   // but are treated as constants.
 
  public:
-
   enum { dimension = 3 };
 
   /// @name Standard Constructors
@@ -83,24 +81,16 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @{
 
   /// distorsion parameter k1
-  inline double k1() const {
+  inline double k1() const { return k1_; }
-    return k1_;
-  }
 
   /// distorsion parameter k2
-  inline double k2() const {
+  inline double k2() const { return k2_; }
-    return k2_;
-  }
 
   /// image center in x
-  inline double px() const {
+  inline double px() const { return u0_; }
-    return u0_;
-  }
 
   /// image center in y
-  inline double py() const {
+  inline double py() const { return v0_; }
-    return v0_;
-  }
 
   Matrix3 K() const override;  ///< Standard 3*3 calibration matrix
   Vector4 k() const;  ///< Radial distortion parameters (4 of them, 2 0)
@@ -109,14 +99,10 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /// get parameter u0
-  inline double u0() const {
+  inline double u0() const { return u0_; }
-    return u0_;
-  }
 
   /// get parameter v0
-  inline double v0() const {
+  inline double v0() const { return v0_; }
-    return v0_;
-  }
 #endif
 
   /**
@@ -138,8 +124,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& pi,
+  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
-                   OptionalJacobian<2, 3> Dcal = boost::none,
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
@@ -172,7 +157,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   }
 
  private:
-
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -189,7 +173,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   }
 
   /// @}
-
 };
 
 template <>

gtsam/geometry/Cal3DS2.cpp

@@ -16,11 +16,11 @@
  * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2.h>
 
 namespace gtsam {
 
@@ -31,9 +31,7 @@ std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
 }
 
 /* ************************************************************************* */
-void Cal3DS2::print(const std::string& s_) const {
+void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
-  Base::print(s_);
-}
 
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
@@ -50,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const {
 Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
   return T2.vector() - vector();
 }
-
 }
 /* ************************************************************************* */
-
-

gtsam/geometry/Cal3DS2.h

@@ -11,7 +11,8 @@
 
 /**
  * @file Cal3DS2.h
- * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
+ * @brief Calibration of a camera with radial distortion, calculations in base
+ * class Cal3DS2_Base
  * @date Feb 28, 2010
  * @author ydjian
  * @autho Varun Agrawal
@@ -31,11 +32,9 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
-
   using Base = Cal3DS2_Base;
 
  public:
-
   enum { dimension = 9 };
 
   /// @name Standard Constructors
@@ -97,23 +96,19 @@ public:
 
   /// @}
 
-
  private:
-
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & boost::serialization::make_nvp("Cal3DS2",
+        "Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
   }
 
   /// @}
-
 };
 
 template <>
@@ -121,6 +116,4 @@ struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
 template <>
 struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
-
 }
-

gtsam/geometry/Cal3DS2_Base.cpp

@@ -16,11 +16,11 @@
  * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2_Base.h>
 
 namespace gtsam {
 
@@ -54,9 +54,9 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
 }
 
 /* ************************************************************************* */
-static Matrix29 D2dcalibration(double x, double y, double xx,
+static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
-    double yy, double xy, double rr, double r4, double pnx, double pny,
+                               double xy, double rr, double r4, double pnx,
-    const Matrix2& DK) {
+                               double pny, const Matrix2& DK) {
   Matrix25 DR1;
   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
   Matrix24 DR2;
@@ -68,8 +68,8 @@ static Matrix29 D2dcalibration(double x, double y, double xx,
 }
 
 /* ************************************************************************* */
-static Matrix2 D2dintrinsic(double x, double y, double rr,
+static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
-    double g, double k1, double k2, double p1, double p2,
+                            double k2, double p1, double p2,
                             const Matrix2& DK) {
   const double drdx = 2. * x;
   const double drdy = 2. * y;
@@ -190,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   DK << fx_, s_, 0.0, fy_;
   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 }
-
 }
 /* ************************************************************************* */
-
-

gtsam/geometry/Cal3Unified.h

@@ -28,20 +28,21 @@
 namespace gtsam {
 
 /**
- * @brief Calibration of a omni-directional camera with mirror + lens radial distortion
+ * @brief Calibration of a omni-directional camera with mirror + lens radial
+ * distortion
  * @addtogroup geometry
  * \nosubgrouping
  *
  * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² + P.y² + 1})]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² +
+ * P.y² + 1})]
  * r² = Pn.x² + Pn.y²
  * \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ;
  *                      k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
-
   using This = Cal3Unified;
   using Base = Cal3DS2_Base;
 
@@ -49,7 +50,6 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   double xi_ = 0.0f;  ///< mirror parameter
 
  public:
-
   enum { dimension = 10 };
 
   /// @name Standard Constructors
@@ -135,17 +135,14 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   /// @}
 
  private:
-
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & boost::serialization::make_nvp("Cal3Unified",
+        "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
     ar& BOOST_SERIALIZATION_NVP(xi_);
   }
-
 };
 
 template <>
@@ -153,6 +150,4 @@ struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
 
 template <>
 struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
-
 }
-

gtsam/geometry/Cal3_S2Stereo.cpp

@@ -38,7 +38,8 @@ void Cal3_S2Stereo::print(const std::string& s) const {
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
   const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
-  return (Cal3_S2::equals(*base, tol) && std::fabs(b_ - other.baseline()) < tol);
+  return (Cal3_S2::equals(*base, tol) &&
+          std::fabs(b_ - other.baseline()) < tol);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3_S2Stereo.h

@@ -32,7 +32,6 @@ namespace gtsam {
   double b_ = 1.0f;  ///< Stereo baseline.
 
  public:
-
   enum { dimension = 6 };
 
   ///< shared pointer to stereo calibration object
@@ -45,8 +44,7 @@ namespace gtsam {
   Cal3_S2Stereo() = default;
 
   /// constructor from doubles
-    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0,
+  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
-                  double b)
       : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
 
   /// constructor from vector
@@ -120,20 +118,17 @@ namespace gtsam {
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
-    void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-    {
     ar& boost::serialization::make_nvp(
         "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
     ar& BOOST_SERIALIZATION_NVP(b_);
   }
   /// @}
-
 };
 
 // Define GTSAM traits
 template <>
-  struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
+struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
-  };
 
 template <>
 struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -54,9 +54,13 @@ TEST(Cal3Bundler, calibrate ) {
 }
 
 /* ************************************************************************* */
-Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
-Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
+Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
+  return k.calibrate(pt);
+}
 
 /* ************************************************************************* */
 TEST(Cal3Bundler, Duncalibrate) {
@@ -84,9 +88,7 @@ TEST(Cal3Bundler, Dcalibrate) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, assert_equal) {
+TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
-  CHECK(assert_equal(K,K,1e-7));
-}
 
 /* ************************************************************************* */
 TEST(Cal3Bundler, retract) {
@@ -114,5 +116,8 @@ TEST(Cal3_S2, Print) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3DFisheye.cpp

@@ -191,8 +191,7 @@ Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Fisheye, Dcalibrate)
+TEST(Cal3Fisheye, Dcalibrate) {
-{
   Point2 p(0.5, 0.5);
   Point2 pi = K.uncalibrate(p);
   Matrix Dcal, Dp;

gtsam/geometry/tests/testCal3DS2.cpp

@@ -14,7 +14,6 @@
  * @brief Unit tests for Cal3DS2 calibration model.
  */
 
-
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
@@ -26,12 +25,12 @@ using namespace gtsam;
 GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2)
 
-static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
+static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
+                 4.0 * 1e-3);
 static Point2 p(2, 3);
 
 /* ************************************************************************* */
-TEST(Cal3DS2, uncalibrate)
+TEST(Cal3DS2, Uncalibrate) {
-{
   Vector k = K.k();
   double r = p.x() * p.x() + p.y() * p.y();
   double g = 1 + k[0] * r + k[1] * r * r;
@@ -44,19 +43,19 @@ TEST(Cal3DS2, uncalibrate)
   CHECK(assert_equal(q, p_i));
 }
 
-TEST(Cal3DS2, calibrate )
+TEST(Cal3DS2, Calibrate) {
-{
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
   CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
 }
 
-Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST(Cal3DS2, Duncalibrate1)
+TEST(Cal3DS2, Duncalibrate1) {
-{
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
@@ -66,9 +65,9 @@ TEST(Cal3DS2, Duncalibrate1)
 }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, Duncalibrate2)
+TEST(Cal3DS2, Duncalibrate2) {
-{
+  Matrix computed;
-  Matrix computed; K.uncalibrate(p, boost::none, computed);
+  K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
   CHECK(assert_equal(numerical, computed, 1e-5));
   Matrix separate = K.D2d_intrinsic(p);
@@ -80,8 +79,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, Dcalibrate)
+TEST(Cal3DS2, Dcalibrate) {
-{
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Matrix Dcal, Dp;
@@ -93,10 +91,10 @@ TEST(Cal3DS2, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
+TEST(Cal3DS2, Equal) { CHECK(assert_equal(K, K, 1e-5)); }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, retract) {
+TEST(Cal3DS2, Retract) {
   Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
                    2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
 
@@ -122,5 +120,8 @@ TEST(Cal3DS2, Print) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3Unified.cpp

@@ -20,8 +20,8 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unified.h>
 
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>
+#include <gtsam/nonlinear/Values.h>
 
 using namespace gtsam;
 
@@ -36,38 +36,37 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
 matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
 */
 
-static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1);
+static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
+                     4.0 * 1e-3, 0.1);
 static Point2 p(0.5, 0.7);
 
 /* ************************************************************************* */
-TEST(Cal3Unified, uncalibrate)
+TEST(Cal3Unified, Uncalibrate) {
-{
   Point2 p_i(364.7791831734982, 305.6677211952602);
   Point2 q = K.uncalibrate(p);
   CHECK(assert_equal(q, p_i));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, spaceNplane)
+TEST(Cal3Unified, SpaceNplane) {
-{
   Point2 q = K.spaceToNPlane(p);
   CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
   CHECK(assert_equal(p, K.nPlaneToSpace(q)));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, calibrate)
+TEST(Cal3Unified, Calibrate) {
-{
   Point2 pi = K.uncalibrate(p);
   Point2 pn_hat = K.calibrate(pi);
   CHECK(traits<Point2>::Equals(p, pn_hat, 1e-8));
 }
 
-Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST(Cal3Unified, Duncalibrate1)
+TEST(Cal3Unified, Duncalibrate1) {
-{
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
@@ -75,8 +74,7 @@ TEST(Cal3Unified, Duncalibrate1)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, Duncalibrate2)
+TEST(Cal3Unified, Duncalibrate2) {
-{
   Matrix computed;
   K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
@@ -88,8 +86,7 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, Dcalibrate)
+TEST(Cal3Unified, Dcalibrate) {
-{
   Point2 pi = K.uncalibrate(p);
   Matrix Dcal, Dp;
   K.calibrate(pi, Dcal, Dp);
@@ -100,15 +97,13 @@ TEST(Cal3Unified, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, assert_equal)
+TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); }
-{
-  CHECK(assert_equal(K,K,1e-9));
-}
 
 /* ************************************************************************* */
-TEST(Cal3Unified, retract) {
+TEST(Cal3Unified, Retract) {
-  Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
+  Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7,
-      1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
+                       2.0 * 1e-3 + 8, 3.0 * 1e-3 + 9, 4.0 * 1e-3 + 10,
+                       0.1 + 1);
 
   EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
   EXPECT_LONGS_EQUAL(expected.dim(), 10);
@@ -121,8 +116,7 @@ TEST(Cal3Unified, retract) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Unified, DerivedValue)
+TEST(Cal3Unified, DerivedValue) {
-{
   Values values;
   Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
   Key key = 1;
@@ -146,5 +140,8 @@ TEST(Cal3Unified, Print) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3_S2.cpp

@@ -63,14 +63,16 @@ Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
 }
 
 TEST(Cal3_S2, Duncalibrate1) {
-  Matrix25 computed; K.uncalibrate(p, computed, boost::none);
+  Matrix25 computed;
+  K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
   CHECK(assert_equal(numerical, computed, 1e-8));
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Duncalibrate2) {
-  Matrix computed; K.uncalibrate(p, boost::none, computed);
+  Matrix computed;
+  K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
   CHECK(assert_equal(numerical, computed, 1e-9));
 }
@@ -145,4 +147,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam/geometry/tests/testCal3_S2Stereo.cpp

@@ -31,7 +31,7 @@ static Point2 p_uv(1320.3, 1740);
 static Point2 p_xy(2, 3);
 
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, easy_constructor) {
+TEST(Cal3_S2Stereo, Constructor) {
   Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
 
   double fov = 60;  // degrees
@@ -42,7 +42,7 @@ TEST(Cal3_S2Stereo, easy_constructor) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, calibrate) {
+TEST(Cal3_S2Stereo, Calibrate) {
   Point2 intrinsic(2, 3);
   Point2 expectedimage(1320.3, 1740);
   Point2 imagecoordinates = K.uncalibrate(intrinsic);
@@ -51,56 +51,14 @@ TEST(Cal3_S2Stereo, calibrate) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, calibrate_homogeneous) {
+TEST(Cal3_S2Stereo, CalibrateHomogeneous) {
   Vector3 intrinsic(2, 3, 1);
   Vector3 image(1320.3, 1740, 1);
   CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
 }
 
-//TODO(Varun) Add calibrate and uncalibrate methods
-// /* ************************************************************************* */
-// Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
-//   return k.uncalibrate(pt);
-// }
-
-// TEST(Cal3_S2Stereo, Duncalibrate1) {
-//   Matrix26 computed;
-//   K.uncalibrate(p, computed, boost::none);
-//   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
-//   CHECK(assert_equal(numerical, computed, 1e-8));
-// }
-
-// /* ************************************************************************* */
-// TEST(Cal3_S2Stereo, Duncalibrate2) {
-//   Matrix computed;
-//   K.uncalibrate(p, boost::none, computed);
-//   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
-//   CHECK(assert_equal(numerical, computed, 1e-9));
-// }
-
-// Point2 calibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
-//   return k.calibrate(pt);
-// }
-// /* ************************************************************************* */
-// TEST(Cal3_S2Stereo, Dcalibrate1) {
-//   Matrix computed;
-//   Point2 expected = K.calibrate(p_uv, computed, boost::none);
-//   Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
-//   CHECK(assert_equal(expected, p_xy, 1e-8));
-//   CHECK(assert_equal(numerical, computed, 1e-8));
-// }
-
-// /* ************************************************************************* */
-// TEST(Cal3_S2Stereo, Dcalibrate2) {
-//   Matrix computed;
-//   Point2 expected = K.calibrate(p_uv, boost::none, computed);
-//   Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
-//   CHECK(assert_equal(expected, p_xy, 1e-8));
-//   CHECK(assert_equal(numerical, computed, 1e-8));
-// }
-
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, assert_equal) {
+TEST(Cal3_S2Stereo, Equal) {
   CHECK(assert_equal(K, K, 1e-9));
 
   Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
@@ -108,7 +66,7 @@ TEST(Cal3_S2Stereo, assert_equal) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2Stereo, retract) {
+TEST(Cal3_S2Stereo, Retract) {
   Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
                          7);
   EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);