diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h index 24b7918e3..fc8d546d3 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/geometry/TriangulationFactor.h @@ -115,7 +115,7 @@ public: Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Point2 error(camera_.project(point, boost::none, H2) - measured_); + Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) @@ -155,7 +155,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project(point, boost::none, A) - measured_).vector(); + b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector(); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 9ced28dca..366d09d49 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -125,7 +125,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) { TEST( SimpleCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; - Point2 result = camera.project(point1, Dpose, Dpoint); + Point2 result = camera.project(point1, Dpose, Dpoint, boost::none); Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1); CHECK(assert_equal(result, Point2(-100, 100) )); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 3e093c7c4..db37e6b8d 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -132,17 +132,17 @@ namespace gtsam { if(H1) { gtsam::Matrix H0; PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); *H1 = *H1 * H0; return reprojectionError.vector(); } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); return reprojectionError.vector(); } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); return reprojectionError.vector(); } } catch( CheiralityException& e) { diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4eb7992a2..30f17fb7a 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -104,7 +104,7 @@ public: } else { gtsam::Matrix J2; - gtsam::Point2 uv= camera.project(landmark,H1, J2); + gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); if (H1) { *H1 = (*H1) * gtsam::eye(6); }