diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index eb83fd10f..9e5b88b31 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -29,16 +29,15 @@ namespace gtsam { } /* ************************************************************************* */ - StereoPoint2 StereoCamera::project(const Point3& point, - OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, - OptionalJacobian<3,0> H3) const { + StereoPoint2 StereoCamera::project(const Point3& point) const { + return project2(point); + } + + /* ************************************************************************* */ + StereoPoint2 StereoCamera::project2(const Point3& point, + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { -#ifdef STEREOCAMERA_CHAIN_RULE - const Point3 q = leftCamPose_.transform_to(point, H1, H2); -#else - // omit derivatives const Point3 q = leftCamPose_.transform_to(point); -#endif if ( q.z() <= 0 ) throw StereoCheiralityException(); @@ -56,12 +55,6 @@ namespace gtsam { // check if derivatives need to be computed if (H1 || H2) { -#ifdef STEREOCAMERA_CHAIN_RULE - // just implement chain rule - Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian - if (H1) *H1 = D_project_point*(*H1); - if (H2) *H2 = D_project_point*(*H2); -#else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; @@ -76,9 +69,6 @@ namespace gtsam { fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v; *H2 << d * (*H2); } -#endif - if (H3) - throw std::runtime_error("StereoCamera::project does not support third derivative yet"); } // finally translate @@ -86,15 +76,23 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { - double d = 1.0 / P.z(), d2 = d*d; - const Cal3_S2Stereo& K = *K_; - double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - Matrix3 m; - m << f_x*d, 0.0, -d2*f_x* P.x(), - f_x*d, 0.0, -d2*f_x*(P.x() - b), - 0.0, f_y*d, -d2*f_y* P.y(); - return m; + StereoPoint2 StereoCamera::project(const Point3& point, + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, + OptionalJacobian<3,0> H3) const { + if (H3) + throw std::runtime_error( + "StereoCamera::project does not support third derivative - BTW use project2"); + return project2(point,H1,H2); + } + + /* ************************************************************************* */ + Point3 StereoCamera::backproject(const StereoPoint2& z) const { + Vector measured = z.vector(); + double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); + double X = Z * (measured[0] - K_->px()) / K_->fx(); + double Y = Z * (measured[2] - K_->py()) / K_->fy(); + Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); + return world_point; } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 88ffbcdbd..ea28ecfc7 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -17,9 +17,6 @@ #pragma once -#include - -#include #include #include #include @@ -127,33 +124,37 @@ public: return K_->baseline(); } - /* - * project 3D point and compute optional derivatives + /// Project 3D point to StereoPoint2 (uL,uR,v) + StereoPoint2 project(const Point3& point) const; + + /** Project 3D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; + + /// back-project a measurement + Point3 backproject(const StereoPoint2& z) const; + + /// @} + /// @name Deprecated + /// @{ + + /** Project 3D point and compute optional derivatives + * @deprecated, use project2 - this class has fixed calibration * @param H1 derivative with respect to pose * @param H2 derivative with respect to point * @param H3 IGNORED (for calibration) */ - StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 = - boost::none, OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 0> H3 = boost::none) const; - - /// back-project a measurement - Point3 backproject(const StereoPoint2& z) const { - Vector measured = z.vector(); - double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); - double X = Z * (measured[0] - K_->px()) / K_->fx(); - double Y = Z * (measured[2] - K_->py()) / K_->fy(); - Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); - return world_point; - } + StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = + boost::none) const; /// @} private: - /// utility function - Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; - friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index c77509b91..45f26c244 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -96,16 +96,12 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_ TEST( StereoCamera, Dproject) { Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); - Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none); + Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); CHECK(assert_equal(expected1,actual1,1e-7)); Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); - Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none); + Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); - - Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K); - Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3); -// CHECK(assert_equal(expected3,actual3,1e-8)); } /* ************************************************************************* */