diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index ca97f0461..c727c7ddb 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -30,6 +30,9 @@ namespace gtsam { double b_; public: + + typedef boost::shared_ptr shared_ptr; ///< shared pointer to stereo calibration object + /** * default calibration leaves coordinates unchanged */ diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 20c2fcefc..48b83a550 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -23,7 +23,7 @@ using namespace gtsam; namespace gtsam { /* ************************************************************************* */ -StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K) : +StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K) : leftCamPose_(leftCamPose), K_(K) { } @@ -32,49 +32,40 @@ StereoPoint2 StereoCamera::project(const Point3& point, boost::optional H1, boost::optional H2) const { - Point3 cameraPoint = leftCamPose_.transform_to(point); - - if (H1) { - Matrix D_cameraPoint_pose; - Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose, - boost::none); + Point3 cameraPoint = leftCamPose_.transform_to(point, H1, H2); + if(H1 || H2) { 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 - *H1 = D_projection_intrinsic * D_intrinsic_pose; - } + if (H1) { + //Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * *H1; + *H1 = D_projection_intrinsic * D_intrinsic_cameraPoint * *H1; + } - if (H2) { - Matrix D_cameraPoint_point; - Point3 cameraPoint = pose().transform_to(point, boost::none, - D_cameraPoint_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(calibration()); // 3x3 - *H2 = D_projection_intrinsic * D_intrinsic_point; + if (H2) { + //Matrix D_intrinsic_point = D_intrinsic_cameraPoint * *H2; + *H2 = D_projection_intrinsic * D_intrinsic_cameraPoint * *H2; + } } double d = 1.0 / cameraPoint.z(); - double uL = K_.px() + d * K_.fx() * cameraPoint.x(); - double uR = K_.px() + d * K_.fx() * (cameraPoint.x() - K_.baseline()); - double v = K_.py() + d * K_.fy() * cameraPoint.y(); + double uL = K_->px() + d * K_->fx() * cameraPoint.x(); + double uR = K_->px() + d * K_->fx() * (cameraPoint.x() - K_->baseline()); + double v = K_->py() + d * K_->fy() * cameraPoint.y(); return StereoPoint2(uL, uR, v); } /* ************************************************************************* */ Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { double d = 1.0 / P.z(), d2 = d * d; - return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - K_.baseline()) * d2, + return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - K_->baseline()) * d2, 0.0, d, -P.y() * d2); } /* ************************************************************************* */ -Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) { - Vector calibration = K.vector(); +Matrix StereoCamera::Duncalibrate2(const Cal3_S2Stereo::shared_ptr K) { + Vector calibration = K->vector(); Vector calibration2(3); calibration2(0) = calibration(0); calibration2(1) = calibration(0); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 6427cefbd..c483155bf 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -32,15 +32,15 @@ namespace gtsam { private: Pose3 leftCamPose_; - Cal3_S2Stereo K_; + Cal3_S2Stereo::shared_ptr K_; public: StereoCamera() { } - StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K); + StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); - const Cal3_S2Stereo& calibration() const { + const Cal3_S2Stereo::shared_ptr calibration() const { return K_; } @@ -49,7 +49,7 @@ namespace gtsam { } const double baseline() const { - return K_.baseline(); + return K_->baseline(); } @@ -75,16 +75,16 @@ namespace gtsam { * i.e. does not rely on baseline */ Point3 backproject(const Point2& projection, const double scale) const { - Point2 intrinsic = K_.calibrate(projection); + Point2 intrinsic = K_->calibrate(projection); Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);; return pose().transform_from(cameraPoint); } 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(); + 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; } @@ -101,7 +101,7 @@ namespace gtsam { /** Exponential map around p0 */ inline StereoCamera expmap(const Vector& d) const { - return StereoCamera(pose().expmap(d), calibration()); + return StereoCamera(pose().expmap(d), K_); } Vector logmap(const StereoCamera &camera) const { @@ -110,8 +110,8 @@ namespace gtsam { } bool equals(const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && K_.equals( - camera.K_, tol); + return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( + *camera.K_, tol); } Pose3 between(const StereoCamera &camera, @@ -122,13 +122,13 @@ namespace gtsam { void print(const std::string& s = "") const { leftCamPose_.print(s + ".camera."); - K_.print(s + ".calibration."); + K_->print(s + ".calibration."); } private: /** utility functions */ Matrix Dproject_to_stereo_camera1(const Point3& P) const; - static Matrix Duncalibrate2(const Cal3_S2& K); + static Matrix Duncalibrate2(const Cal3_S2Stereo::shared_ptr K); friend class boost::serialization::access; template diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 3f44411d7..6ad95254c 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -36,7 +36,7 @@ TEST( StereoCamera, project) { // create a Stereo camera at the origin with focal length 1500, baseline 0.5m // and principal point 320, 240 (for a hypothetical 640x480 sensor) - Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5); + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters @@ -77,7 +77,7 @@ Pose3 camera1(Matrix_(3,3, ), Point3(0,0,6.25)); -Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5); +Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters @@ -115,7 +115,7 @@ TEST( StereoCamera, backproject2) -0.804435942, -0.592650676, -0.0405925523, 0.0732045588, -0.0310882277, -0.996832359); Point3 t(53.5239823, 23.7866016, -4.42379876); - Cal3_S2Stereo K(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); StereoCamera camera(Pose3(R,t), K); StereoPoint2 z(184.812, 129.068, 714.768); diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 616c012a1..729b9fff1 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -86,7 +86,7 @@ public: /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional H2) const { - StereoCamera stereoCam(pose, *K_); + StereoCamera stereoCam(pose, K_); return (stereoCam.project(point, H1, H2) - z_).vector(); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 5182f567b..d3e5d4f70 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -40,7 +40,7 @@ Pose3 camera1(Matrix_(3,3, ), Point3(0,0,6.25)); -Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5); +Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters