diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 1700b4960..9b881dc78 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -397,16 +397,20 @@ public: double range( const PinholeCamera& camera, // OptionalJacobian<1, DimC> Dcamera = boost::none, - OptionalJacobian<1, 6 + traits::dimension::value> Dother = +// OptionalJacobian<1, 6 + traits::dimension::value> Dother = + boost::optional Dother = boost::none) const { - Matrix16 Dpose_, Dpose2; - double result = pose_.range(camera.pose(), Dcamera ? &Dpose_ : 0, - Dother ? &Dpose2 : 0); - if (Dcamera) - *Dcamera << Dpose_, Eigen::Matrix::Zero(); + Matrix16 Dcamera_, Dother_; + double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, + Dother ? &Dother_ : 0); + if (Dcamera) { + Dcamera->resize(1, 6 + DimK); + *Dcamera << Dcamera_, Eigen::Matrix::Zero(); + } if (Dother) { + Dother->resize(1, 6+traits::dimension::value); Dother->setZero(); - Dother->block(0, 0, 1, 6) = Dpose2; + Dother->block(0, 0, 1, 6) = Dother_; } return result; } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 1cfb0ba25..6cb84ec85 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -229,9 +229,8 @@ static double range0(const Camera& camera, const Point3& point) { } /* ************************************************************************* */ -typedef Eigen::Matrix Matrix1_11; TEST( PinholeCamera, range0) { - Matrix1_11 D1; Matrix13 D2; + Matrix D1; Matrix D2; double result = camera.range(point1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); @@ -248,7 +247,7 @@ static double range1(const Camera& camera, const Pose3& pose) { /* ************************************************************************* */ TEST( PinholeCamera, range1) { - Matrix1_11 D1; Matrix16 D2; + Matrix D1; Matrix D2; double result = camera.range(pose1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); @@ -267,7 +266,7 @@ static double range2(const Camera& camera, const Camera2& camera2) { /* ************************************************************************* */ TEST( PinholeCamera, range2) { - Matrix1_11 D1; Matrix19 D2; + Matrix D1; Matrix D2; double result = camera.range(camera2, D1, D2); Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); @@ -284,7 +283,7 @@ static double range3(const Camera& camera, const CalibratedCamera& camera3) { /* ************************************************************************* */ TEST( PinholeCamera, range3) { - Matrix1_11 D1; Matrix16 D2; + Matrix D1; Matrix D2; double result = camera.range(camera3, D1, D2); Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);