diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 84ce0cfb5..cdd62a675 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -100,6 +100,12 @@ namespace gtsam { camera.K_, tol); } + Pose3 between(const StereoCamera &camera, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + return leftCamPose_.between(camera.pose(), H1, H2); + } + void print(const std::string& s = "") const { leftCamPose_.print(s + ".camera."); K_.print(s + ".calibration.");