Converted stereo factors to use stereo points and cameras; added operator<< to StereoPoint2 (STILL CAUSES LNK ERROR)

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
     /// @{

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;
+}

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

gtsam/slam/SmartFactorBase.h

@@ -26,6 +26,7 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/PinholeCamera.h>  // for Cheirality exception
+#include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
 

gtsam/slam/SmartStereoProjectionFactor.h

@@ -63,7 +63,7 @@ enum LinearizationMode {
  * TODO: why LANDMARK parameter?
  */
 template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
-class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> {
+class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> {
 protected:
 
   // Some triangulation parameters
@@ -93,7 +93,7 @@ protected:
   typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
 
   /// shorthand for base class type
-  typedef SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> Base;
+  typedef SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> 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<CALIBRATION> Camera;
+  typedef StereoCamera Camera;
 //  typedef StereoCamera Camera;
 
   typedef std::vector<Camera> 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());

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<CALIBRATION> 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<Point2> measurements, std::vector<Key> poseKeys,
+  void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
       std::vector<SharedNoiseModel> noises,
       std::vector<boost::shared_ptr<CALIBRATION> > 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<Point2> measurements, std::vector<Key> poseKeys,
+  void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
       const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> 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<Pose3>(k);
-      typename Base::Camera camera(pose, *K_all_[i++]);
+      typename Base::Camera camera(pose, K_all_[i++]);
       cameras.push_back(camera);
     }
     return cameras;

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_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b));
-static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480));
+static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b));
 static boost::shared_ptr<Cal3Bundler> 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<Pose3,Point3,Cal3_S2> SmartFactor;
+typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3_S2Stereo> SmartFactor;
 typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;
 
 void stereo_projectToMultipleCameras(
-    SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark,
+    StereoCamera cam1, StereoCamera cam2, StereoCamera cam3, Point3 landmark,
-    vector<Point2>& measurements_cam){
+    vector<StereoPoint2>& measurements_cam){
 
-  Point2 cam1_uv1 = cam1.project(landmark);
+  StereoPoint2 cam1_uv1 = cam1.project(landmark);
-  Point2 cam2_uv1 = cam2.project(landmark);
+  StereoPoint2 cam2_uv1 = cam2.project(landmark);
-  Point2 cam3_uv1 = cam3.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);