From c13569df4c659e5c27f9b819e5a93f4790f7c9ea Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Thu, 5 Jun 2014 09:25:06 -0400 Subject: [PATCH] Converted stereo factors to use stereo points and cameras; added operator<< to StereoPoint2 (STILL CAUSES LNK ERROR) --- gtsam/geometry/Cal3_S2Stereo.h | 5 +++++ gtsam/geometry/StereoPoint2.cpp | 5 +++++ gtsam/geometry/StereoPoint2.h | 2 ++ gtsam/slam/SmartFactorBase.h | 1 + gtsam/slam/SmartStereoProjectionFactor.h | 14 +++++++------- gtsam/slam/SmartStereoProjectionPoseFactor.h | 8 ++++---- .../testSmartStereoProjectionPoseFactor.cpp | 19 ++++++++++--------- 7 files changed, 34 insertions(+), 20 deletions(-) diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 811264967..b47153547 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -52,6 +52,11 @@ namespace gtsam { /// constructor from vector Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){} + /// easy constructor; field-of-view in degrees, assumes zero skew + Cal3_S2Stereo(double fov, int w, int h, double b) : + K_(fov, w, h), b_(b) { + } + /// @} /// @name Testable /// @{ diff --git a/gtsam/geometry/StereoPoint2.cpp b/gtsam/geometry/StereoPoint2.cpp index f599a2dea..4aaa513f5 100644 --- a/gtsam/geometry/StereoPoint2.cpp +++ b/gtsam/geometry/StereoPoint2.cpp @@ -26,3 +26,8 @@ void StereoPoint2::print(const string& s) const { cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" << endl; } /* ************************************************************************* */ + +ostream &operator<<(ostream &os, const StereoPoint2& p) { + os << '(' << p.uL() << ", " << p.uR() << p.v() << ')'; + return os; +} diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 8ce2e49bf..8e420fc16 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -88,6 +88,8 @@ namespace gtsam { StereoPoint2 operator-(const StereoPoint2& b) const { return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); } + + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); /// @} /// @name Manifold diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 69f92e402..eab7810db 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -26,6 +26,7 @@ #include #include // for Cheirality exception +#include #include #include diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 9bfe72eae..9fa1a2b27 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -63,7 +63,7 @@ enum LinearizationMode { * TODO: why LANDMARK parameter? */ template -class SmartStereoProjectionFactor: public SmartFactorBase, D> { +class SmartStereoProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -93,7 +93,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase, D> Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -112,7 +112,7 @@ public: /// shorthand for a pinhole camera // TODO: Switch to stereoCamera: - typedef PinholeCamera Camera; + typedef StereoCamera Camera; // typedef StereoCamera Camera; typedef std::vector Cameras; @@ -264,9 +264,9 @@ public: degenerate_ = true; break; } - const Point2& zi = this->measured_.at(i); + const StereoPoint2& zi = this->measured_.at(i); try { - Point2 reprojectionError(camera.project(point_) - zi); + StereoPoint2 reprojectionError(camera.project(point_) - zi); totalReprojError += reprojectionError.vector().norm(); } catch (CheiralityException) { cheiralityException_ = true; @@ -652,10 +652,10 @@ public: // size_t i = 0; // double overallError = 0; // BOOST_FOREACH(const Camera& camera, cameras) { -// const Point2& zi = this->measured_.at(i); +// const StereoPoint2& zi = this->measured_.at(i); // if (i == 0) // first pose // this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity -// Point2 reprojectionError( +// StereoPoint2 reprojectionError( // camera.projectPointAtInfinity(this->point_) - zi); // overallError += 0.5 // * this->noise_.at(i)->distance(reprojectionError.vector()); diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index 4077071d9..b6fad38fa 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -83,7 +83,7 @@ public: * @param noise_i is the measurement noise * @param K_i is the (known) camera calibration */ - void add(const Point2 measured_i, const Key poseKey_i, + void add(const StereoPoint2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i, const boost::shared_ptr K_i) { Base::add(measured_i, poseKey_i, noise_i); @@ -97,7 +97,7 @@ public: * @param noises vector of measurement noises * @param Ks vector of calibration objects */ - void add(std::vector measurements, std::vector poseKeys, + void add(std::vector measurements, std::vector poseKeys, std::vector noises, std::vector > Ks) { Base::add(measurements, poseKeys, noises); @@ -113,7 +113,7 @@ public: * @param noise measurement noise (same for all measurements) * @param K the (known) camera calibration (same for all measurements) */ - void add(std::vector measurements, std::vector poseKeys, + void add(std::vector measurements, std::vector poseKeys, const SharedNoiseModel noise, const boost::shared_ptr K) { for (size_t i = 0; i < measurements.size(); i++) { Base::add(measurements.at(i), poseKeys.at(i), noise); @@ -157,7 +157,7 @@ public: size_t i=0; BOOST_FOREACH(const Key& k, this->keys_) { Pose3 pose = values.at(k); - typename Base::Camera camera(pose, *K_all_[i++]); + typename Base::Camera camera(pose, K_all_[i++]); cameras.push_back(camera); } return cameras; diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index e31693f46..5952dd9bb 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -36,9 +36,10 @@ static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees static size_t w=640,h=480; +static double b = 1; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); -static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b)); +static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b)); static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); static double rankTol = 1.0; @@ -57,19 +58,19 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Key poseKey1(x1); -static Point2 measurement1(323.0, 240.0); +static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -typedef SmartStereoProjectionPoseFactor SmartFactor; +typedef SmartStereoProjectionPoseFactor SmartFactor; typedef SmartStereoProjectionPoseFactor SmartFactorBundler; void stereo_projectToMultipleCameras( - SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, - vector& measurements_cam){ + StereoCamera cam1, StereoCamera cam2, StereoCamera cam3, Point3 landmark, + vector& measurements_cam){ - Point2 cam1_uv1 = cam1.project(landmark); - Point2 cam2_uv1 = cam2.project(landmark); - Point2 cam3_uv1 = cam3.project(landmark); + StereoPoint2 cam1_uv1 = cam1.project(landmark); + StereoPoint2 cam2_uv1 = cam2.project(landmark); + StereoPoint2 cam3_uv1 = cam3.project(landmark); measurements_cam.push_back(cam1_uv1); measurements_cam.push_back(cam2_uv1); measurements_cam.push_back(cam3_uv1);