diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 0d64d0184..b198643b0 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/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) : - f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) { -} +Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double 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; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4787f565b..96765f899 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/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_); } /// @} diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 8665f7716..a565ac140 100644 --- a/gtsam/geometry/Point3.cpp +++ b/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 &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; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 33b5836f8..b425af8a4 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -62,6 +62,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q, /// mean template 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(); diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 8de049fa4..448600266 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/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::Equals(pn, pn_hat, 1e-5)); + CHECK(traits::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; - CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7)); +} + +/* ************************************************************************* */ +TEST( Cal3Bundler, Dcalibrate) +{ + 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)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index a9b68bdec..ad6f4e921 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/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 camera(wTc, calibration); + Point2 p(50, 100); + camera.backproject(p, 10); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d9aa23893..e5f323d72 100644 --- a/gtsam/gtsam.i +++ b/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 PinholeCameraCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -//typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; #include 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 camera(size_t idx) const; + gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; }; diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index e45b17e4a..b249af5c5 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/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