bug fix for Cal3DS2, and unit tests for Cal3Bundler + Cal3DS2

gtsam/geometry/Cal3DS2.cpp

@@ -102,10 +102,14 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
 	const double r2 = r*r ;
 	const double fx = fx_, fy = fy_, s = s_ ;
 	const double g = (1+k1_*r+k2_*r2) ;
+  const double dx = 2*k3_*xy + k4_*(r+2*xx) ;
+  const double dy = 2*k4_*xy + k3_*(r+2*yy) ;
+	const double pnx = g*x + dx;
+	const double pny = g*y + dy;
 
 	return Matrix_(2, 9,
-	g*x, 0.0, g*y, 1.0, 0.0, fx*x*r + s*y*r, fx*x*r2 + s*y*r2, fx*2*xy + s*(r+2*yy), fx*(r+2*xx) + s*(2*xy),
+	pnx, 0.0, pny, 1.0, 0.0, fx*x*r + s*y*r, fx*x*r2 + s*y*r2, fx*2*xy + s*(r+2*yy), fx*(r+2*xx) + s*(2*xy),
-	0.0, g*y, 0.0, 0.0, 1.0, fy*y*r 		 , fy*y*r2         , fy*(r+2*yy)         , fy*(2*xy)	);
+	0.0, pny, 0.0, 0.0, 1.0, fy*y*r 		 , fy*y*r2         , fy*(r+2*yy)         , fy*(2*xy)	);
 }
 
 Cal3DS2 Cal3DS2::expmap(const Vector& d) const { return Cal3DS2(vector() + d) ; }

gtsam/geometry/Cal3DS2.h

@@ -18,8 +18,6 @@
 
 
 #pragma once
-#ifndef CAL3DS2_H_
-#define CAL3DS2_H_
 
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
@@ -38,6 +36,7 @@ namespace gtsam {
 		// r = Pn.x^2 + Pn.y^2
 		// \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ;
 		//										  k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+		// pi = K*pn
 
 	public:
 		// Default Constructor with only unit focal length
@@ -92,4 +91,3 @@ namespace gtsam {
 	};
 }
 
-#endif /* CAL3DS2_H_ */

gtsam/geometry/Makefile.am

@@ -17,7 +17,7 @@ check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoin
 # Cameras
 headers += GeneralCameraT.h
 sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp
-check_PROGRAMS += tests/testCal3_S2 tests/testCalibratedCamera tests/testSimpleCamera
+check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler tests/testCalibratedCamera tests/testSimpleCamera
 
 # Stereo
 sources += StereoPoint2.cpp StereoCamera.cpp

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -0,0 +1,66 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  testCal3Bundler.cpp
+ * @brief Unit tests for transform derivatives
+ */
+
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+
+using namespace gtsam;
+
+Cal3Bundler K(500, 1e-3, 1e-3);
+Point2 p(2,3);
+
+/* ************************************************************************* */
+TEST( Cal3Bundler, calibrate)
+{
+  Vector v = K.vector() ;
+  double r = p.x()*p.x() + p.y()*p.y() ;
+  double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
+  Point2 q_hat (g*p.x(), g*p.y()) ;
+  Point2 q = K.uncalibrate(p);
+  CHECK(assert_equal(q,q_hat));
+}
+
+
+Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
+
+/* ************************************************************************* */
+TEST( Cal3Bundler, Duncalibrate1)
+{
+  Matrix computed;
+  K.uncalibrate(p, computed, boost::none);
+  Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical,computed,1e-7));
+}
+
+/* ************************************************************************* */
+TEST( Cal3Bundler, Duncalibrate2)
+{
+  Matrix computed; K.uncalibrate(p, boost::none, computed);
+  Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical,computed,1e-7));
+}
+
+/* ************************************************************************* */
+TEST( Cal3Bundler, assert_equal)
+{
+  CHECK(assert_equal(K,K,1e-7));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

gtsam/geometry/tests/testCal3DS2.cpp

@@ -0,0 +1,69 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  testCal3DS2.cpp
+ * @brief Unit tests for transform derivatives
+ */
+
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2.h>
+
+using namespace gtsam;
+
+Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
+Point2 p(2,3);
+
+/* ************************************************************************* */
+TEST( Cal3DS2, calibrate)
+{
+  Vector k = K.k() ;
+  double r = p.x()*p.x() + p.y()*p.y() ;
+  double g = 1+k[0]*r+k[1]*r*r ;
+  double tx = 2*k[2]*p.x()*p.y()       +   k[3]*(r+2*p.x()*p.x()) ;
+  double ty =   k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
+  Vector v_hat = Vector_(3, g*p.x() + tx, g*p.y() + ty, 1.0) ;
+  Vector v_i = K.K() * v_hat ;
+  Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
+  Point2 q = K.uncalibrate(p);
+  CHECK(assert_equal(q,p_i));
+}
+
+Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
+
+/* ************************************************************************* */
+TEST( Cal3DS2, Duncalibrate1)
+{
+  Matrix computed;
+  K.uncalibrate(p, computed, boost::none);
+  Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
+  CHECK(assert_equal(numerical,computed,1e-5));
+}
+
+/* ************************************************************************* */
+TEST( Cal3DS2, Duncalibrate2)
+{
+  Matrix computed; K.uncalibrate(p, boost::none, computed);
+  Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
+  CHECK(assert_equal(numerical,computed,1e-5));
+}
+
+/* ************************************************************************* */
+TEST( Cal3DS2, assert_equal)
+{
+  CHECK(assert_equal(K,K,1e-5));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */