diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index f636d22d5..0ac08cee2 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -42,7 +42,7 @@ StereoPoint2 StereoCamera::project(const Point3& point, Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose; - Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3 + Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3 *Dproject_stereo_pose = D_projection_intrinsic * D_intrinsic_pose; } @@ -54,7 +54,7 @@ StereoPoint2 StereoCamera::project(const Point3& point, Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point; - Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3 + Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3 *Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point; } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 9041f28d4..9aebb5ebd 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -40,7 +40,7 @@ namespace gtsam { StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K); - const Cal3_S2Stereo& K() const { + const Cal3_S2Stereo& calibration() const { return K_; } @@ -63,7 +63,7 @@ namespace gtsam { /** Exponential map around p0 */ inline StereoCamera expmap(const Vector& d) const { - return StereoCamera(pose().expmap(d),K()); + return StereoCamera(pose().expmap(d),calibration()); } Vector logmap(const StereoCamera &camera) const {