Merge branch 'develop' into fix/means

# Conflicts:
#	gtsam/geometry/Point3.cpp

gtsam/geometry/Cal3Bundler.cpp

@@ -25,13 +25,13 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Cal3Bundler::Cal3Bundler() :
-    f_(1), k1_(0), k2_(0), u0_(0), v0_(0) {
+    f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
 }
 
 /* ************************************************************************* */
-Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
+Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
-    f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) {
+                         double tol)
-}
+    : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
 
 /* ************************************************************************* */
 Matrix3 Cal3Bundler::K() const {
@@ -94,21 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
+Point2 Cal3Bundler::calibrate(const Point2& pi,
+                              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
-  const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
+  double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
+  const Point2 invKPi(x, y);
 
   // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
-  Point2 pn = invKPi;
+  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;
-    const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
+    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);
     pn = invKPi / g;
@@ -118,6 +121,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
     throw std::runtime_error(
         "Cal3Bundler::calibrate fails to converge. need a better initialization");
 
+  // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
+  // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
+  // df/pi = -I (pn and pi are independent args)
+  // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
+  // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+  Matrix23 H_uncal_K;
+  Matrix22 H_uncal_pn, H_uncal_pn_inv;
+
+  if (Dcal || Dp) {
+    // Compute uncalibrate Jacobians
+    uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
+
+    H_uncal_pn_inv = H_uncal_pn.inverse();
+
+    if (Dp) *Dp = H_uncal_pn_inv;
+    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
+
+  }
+
   return pn;
 }
 

gtsam/geometry/Cal3Bundler.h

@@ -33,6 +33,7 @@ private:
   double f_; ///< focal length
   double k1_, k2_; ///< radial distortion
   double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
+  double tol_; ///< tolerance value when calibrating
 
 public:
 
@@ -51,8 +52,10 @@ public:
    *  @param k2 second radial distortion coefficient (quartic)
    *  @param u0 optional image center (default 0), considered a constant
    *  @param v0 optional image center (default 0), considered a constant
+   *  @param tol optional calibration tolerance value
    */
-  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
+  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
+              double tol = 1e-5);
 
   virtual ~Cal3Bundler() {}
 
@@ -117,8 +120,17 @@ public:
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
       OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// Convert a pixel coordinate to ideal coordinate
+  /**
-  Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
+   * Convert a pixel coordinate to ideal coordinate xy
+   * @param p point in image coordinates
+   * @param tol optional tolerance threshold value for iterative minimization
+   * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& pi,
+                   OptionalJacobian<2, 3> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
   Matrix2 D2d_intrinsic(const Point2& p) const;
@@ -164,6 +176,7 @@ private:
     ar & BOOST_SERIALIZATION_NVP(k2_);
     ar & BOOST_SERIALIZATION_NVP(u0_);
     ar & BOOST_SERIALIZATION_NVP(v0_);
+    ar & BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}

gtsam/geometry/Point3.cpp

@@ -77,6 +77,7 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
 
 Point3Pair means(const std::vector<Point3Pair> &abPointPairs) {
   const size_t n = abPointPairs.size();
+  if (n == 0) throw std::invalid_argument("Point3::mean input Point3Pair vector is empty");
   Point3 aSum(0, 0, 0), bSum(0, 0, 0);
   for (const Point3Pair &abPair : abPointPairs) {
     aSum += abPair.first;

gtsam/geometry/Point3.h

@@ -62,6 +62,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
 /// mean
 template <class CONTAINER>
 GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
+  if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
   Point3 sum(0, 0, 0);
   sum = std::accumulate(points.begin(), points.end(), sum);
   return sum / points.size();

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -52,12 +52,14 @@ TEST( Cal3Bundler, 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));
+  CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
 }
 
 /* ************************************************************************* */
 Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
 
+Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
+
 /* ************************************************************************* */
 TEST( Cal3Bundler, Duncalibrate)
 {
@@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate)
   Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
   CHECK(assert_equal(numerical1,Dcal,1e-7));
   CHECK(assert_equal(numerical2,Dp,1e-7));
-  CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7));
+}
-  CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7));
+
-  Matrix Dcombined(2,5);
+/* ************************************************************************* */
-  Dcombined << Dp, Dcal;
+TEST( Cal3Bundler, Dcalibrate)
-  CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7));
+{
+  Matrix Dcal, Dp;
+  Point2 pn(0.5, 0.5);
+  Point2 pi = K.uncalibrate(pn);
+  Point2 actual = K.calibrate(pi, Dcal, Dp);
+  CHECK(assert_equal(pn, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  CHECK(assert_equal(numerical2,Dp,1e-5));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
   EXPECT(assert_equal(Hexpected2, D2, 1e-7));
 }
 
+/* ************************************************************************* */
+TEST( PinholeCamera, Cal3Bundler) {
+  Cal3Bundler calibration;
+  Pose3 wTc;
+  PinholeCamera<Cal3Bundler> camera(wTc, calibration);
+  Point2 p(50, 100);
+  camera.backproject(p, 10);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/gtsam.i

@@ -959,6 +959,7 @@ class Cal3Bundler {
   // Standard Constructors
   Cal3Bundler();
   Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
 
   // Testable
   void print(string s) const;
@@ -971,7 +972,7 @@ class Cal3Bundler {
   Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
 
   // Action on Point2
-  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
 
   // Standard Interface
@@ -1105,7 +1106,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
 //typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
 //typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
 #include <gtsam/geometry/StereoCamera.h>
 class StereoCamera {
@@ -2462,6 +2463,8 @@ class ISAM2Result {
   size_t getVariablesRelinearized() const;
   size_t getVariablesReeliminated() const;
   size_t getCliques() const;
+  double getErrorBefore() const;
+  double getErrorAfter() const;
 };
 
 class ISAM2 {
@@ -2766,8 +2769,7 @@ class SfmTrack {
 class SfmData {
   size_t number_cameras() const;
   size_t number_tracks() const;
-  //TODO(Varun) Need to fix issue #237 first before this can work
+  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
-  // gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
   gtsam::SfmTrack track(size_t idx) const;
 };
 

gtsam/nonlinear/ISAM2Result.h

@@ -176,6 +176,8 @@ struct ISAM2Result {
   size_t getVariablesRelinearized() const { return variablesRelinearized; }
   size_t getVariablesReeliminated() const { return variablesReeliminated; }
   size_t getCliques() const { return cliques; }
+  double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
+  double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }
 };
 
 }  // namespace gtsam