diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h new file mode 100644 index 000000000..8a314e71b --- /dev/null +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3_S2Stereo.h + * @brief The most common 5DOF 3D->2D calibration + Stereo baseline + * @author Chris Beall + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + /** The most common 5DOF 3D->2D calibration */ + class Cal3_S2Stereo: public Cal3_S2 { + private: + double b_; + + public: + /** + * default calibration leaves coordinates unchanged + */ + Cal3_S2Stereo() : + Cal3_S2(1, 1, 0, 0, 0), b_(1.0) { + } + + /** + * constructor from doubles + */ + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) : + Cal3_S2(fx, fy, s, u0, v0), b_(b) { + } + + void print(const std::string& s = "") const { + gtsam::print(matrix(), s); + std::cout << "Baseline: " << b_ << std::endl; + } + +// /** +// * Check if equal up to specified tolerance +// */ +// bool equals(const Cal3_S2& K, double tol = 10e-9) const; + + inline double baseline() const { return b_; } + + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(b_); + } + }; + + typedef boost::shared_ptr shared_ptrKStereo; + +} diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index 4ed7f7b77..0fef71cee 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -15,7 +15,7 @@ sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3 tests/testPose3 # Cameras -headers += GeneralCameraT.h +headers += GeneralCameraT.h Cal3_S2Stereo.h sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler tests/testCalibratedCamera tests/testSimpleCamera diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index ce25dc74b..f636d22d5 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -23,14 +23,8 @@ using namespace gtsam; namespace gtsam { /* ************************************************************************* */ -StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, - double baseline) : - leftCamPose_(leftCamPose), K_(K), baseline_(baseline) { - Vector calibration = K_.vector(); - fx_ = calibration(0); - fy_ = calibration(1); - cx_ = calibration(3); - cy_ = calibration(4); +StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K) : + leftCamPose_(leftCamPose), K_(K) { } /* ************************************************************************* */ @@ -65,16 +59,16 @@ StereoPoint2 StereoCamera::project(const Point3& point, } double d = 1.0 / cameraPoint.z(); - double uL = cx_ + d * fx_ * cameraPoint.x(); - double uR = cx_ + d * fx_ * (cameraPoint.x() - baseline_); - double v = cy_ + d * 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() - 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); } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index c800992cd..9041f28d4 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -18,7 +18,7 @@ #pragma once #include "boost/tuple/tuple.hpp" -#include +#include #include #include #include @@ -32,17 +32,15 @@ namespace gtsam { private: Pose3 leftCamPose_; - Cal3_S2 K_; - double baseline_; - double fx_, fy_; - double cx_, cy_; + Cal3_S2Stereo K_; + public: StereoCamera() { } - StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double baseline); + StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K); - const Cal3_S2& K() const { + const Cal3_S2Stereo& K() const { return K_; } @@ -51,7 +49,7 @@ namespace gtsam { } const double baseline() const { - return baseline_; + return K_.baseline(); } StereoPoint2 project(const Point3& point, @@ -65,7 +63,7 @@ namespace gtsam { /** Exponential map around p0 */ inline StereoCamera expmap(const Vector& d) const { - return StereoCamera(pose().expmap(d),K(),baseline()); + return StereoCamera(pose().expmap(d),K()); } Vector logmap(const StereoCamera &camera) const { diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 0e57131e6..2aa76732e 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -36,8 +36,8 @@ 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_S2 K(1500, 1500, 0, 320, 240); - StereoCamera stereoCam(Pose3(), K, 0.5); + Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5); + StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters Point3 p(0, 0, 5); @@ -55,7 +55,7 @@ TEST( StereoCamera, project) Rot3 unit = Rot3(); Point3 one_meter_z(0, 0, 1); Pose3 camPose3(unit, one_meter_z); - StereoCamera stereoCam3(camPose3, K, 0.5); + StereoCamera stereoCam3(camPose3, K); StereoPoint2 result3 = stereoCam3.project(p3); CHECK(assert_equal(StereoPoint2(320.0+300.0, 320.0+150.0, 240.0+300),result3)); @@ -63,7 +63,7 @@ TEST( StereoCamera, project) Point3 p4(5, 1, 0); Rot3 right = Rot3(0, 0, 1, 0, 1, 0, -1, 0, 0); Pose3 camPose4(right, one_meter_z); - StereoCamera stereoCam4(camPose4, K, 0.5); + StereoCamera stereoCam4(camPose4, K); StereoPoint2 result4 = stereoCam4.project(p4); CHECK(assert_equal(StereoPoint2(320.0+300.0,320.0+150.0,240.0+300),result4)); } @@ -77,8 +77,8 @@ Pose3 camera1(Matrix_(3,3, ), Point3(0,0,6.25)); -Cal3_S2 K(1500, 1500, 0, 320, 240); -StereoCamera stereoCam(Pose3(), K, 0.5); +Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5); +StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters Point3 p(0, 0, 5); diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 6006a45f8..15c4207e9 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include #include namespace gtsam { @@ -32,8 +32,7 @@ private: // Keep a copy of measurement and calibration for I/O StereoPoint2 z_; - boost::shared_ptr K_; - double baseline_; + boost::shared_ptr K_; public: @@ -48,7 +47,7 @@ public: /** * Default constructor */ - GenericStereoFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + GenericStereoFactor() : K_(new Cal3_S2Stereo(444, 555, 666, 777, 888, 1.0)) {} /** * Constructor @@ -59,8 +58,8 @@ public: * @param K the constant calibration */ GenericStereoFactor(const StereoPoint2& z, const SharedGaussian& model, KEY1 j_pose, - KEY2 j_landmark, const shared_ptrK& K, double baseline) : - Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) { + KEY2 j_landmark, const shared_ptrKStereo& K) : + Base(model, j_pose, j_landmark), z_(z), K_(K) { } ~GenericStereoFactor() {} @@ -94,7 +93,7 @@ public: /** h(x)-z */ Vector evaluateError(const CamPose& pose, const Point3& point, boost::optional H1, boost::optional H2) const { - StereoCamera stereoCam(pose, *K_, baseline_); + StereoCamera stereoCam(pose, *K_); return (stereoCam.project(point, H1, H2) - z_).vector(); } @@ -109,7 +108,6 @@ private: void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(K_); - ar & BOOST_SERIALIZATION_NVP(baseline_); } }; diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index e602d5f8b..4e7143ebd 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -40,8 +40,8 @@ Pose3 camera1(Matrix_(3,3, ), Point3(0,0,6.25)); -Cal3_S2 K(1500, 1500, 0, 320, 240); -StereoCamera stereoCam(Pose3(), K, 0.5); +Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5); +StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters Point3 p(0, 0, 5); @@ -50,15 +50,15 @@ static SharedGaussian sigma(noiseModel::Unit::Create(1)); /* ************************************************************************* */ TEST( StereoFactor, singlePoint) { - //Cal3_S2 K(625, 625, 0, 320, 240); - boost::shared_ptr K(new Cal3_S2(625, 625, 0, 320, 240)); + //Cal3_S2 K(625, 625, 0, 320, 240, 0.5); + boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); boost::shared_ptr graph(new Graph()); graph->add(PoseConstraint(1,camera1)); StereoPoint2 z14(320,320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph->add(StereoFactor(z14,sigma, 1, 1, K, 0.5)); + graph->add(StereoFactor(z14,sigma, 1, 1, K)); // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values());