addressed comments and added an additional test

gtsam/geometry/Cal3Bundler.cpp

@@ -96,11 +96,9 @@ 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
-  double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
+  double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
-  const Point2 invKPi(x, y);
+  const Point2 invKPi(px, py);
-
   Point2 pn;
-  double px = pi.x(), py = pi.y();
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -89,6 +89,33 @@ TEST(Cal3Bundler, DcalibrateDefault) {
   CHECK(assert_equal(numerical2, Dp, 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
+  Cal3Bundler K(5, 0, 0, 2, 2);
+  Matrix Dcal, Dp;
+  Point2 actual = K.uncalibrate(p, Dcal, Dp);
+  Point2 expected(12, 17);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-7));
+  CHECK(assert_equal(numerical2, Dp, 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Bundler, DcalibratePrincipalPoint) {
+  Cal3Bundler K(2, 0, 0, 2, 2);
+  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));
+}
+
 /* ************************************************************************* */
 TEST(Cal3Bundler, Duncalibrate) {
   Matrix Dcal, Dp;