Merge pull request #615 from borglab/feature/wrap_camVector

vector of cameras and triangulation function wrapped

gtsam/geometry/triangulation.h

@@ -18,8 +18,10 @@
 
 #pragma once
 
-#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/slam/TriangulationFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -494,5 +496,9 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
     }
 }
 
+// Vector of Cameras - used by the Python/MATLAB wrapper
+using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
+using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
+
 } // \namespace gtsam
 

gtsam/gtsam.i

@@ -1067,6 +1067,15 @@ typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
 typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
+template<T>
+class CameraSet {
+  CameraSet();
+
+  // structure specific methods
+  T at(size_t i) const;
+  void push_back(const T& cam);
+};
+
 #include <gtsam/geometry/StereoCamera.h>
 class StereoCamera {
   // Standard Constructors and Named Constructors
@@ -1098,7 +1107,7 @@ class StereoCamera {
 
 #include <gtsam/geometry/triangulation.h>
 
-// Templates appear not yet supported for free functions
+// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
     gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
     double rank_tol, bool optimize);
@@ -1108,6 +1117,12 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
     gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
     double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
+    const gtsam::Point2Vector& measurements, double rank_tol,
+    bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
+    const gtsam::Point2Vector& measurements, double rank_tol,
+    bool optimize);
     
 //*************************************************************************
 // Symbolic

python/CMakeLists.txt

@@ -82,6 +82,8 @@ set(ignore
         gtsam::KeyVector
         gtsam::FixedLagSmootherKeyTimestampMapValue
         gtsam::BinaryMeasurementsUnit3
+        gtsam::CameraSetCal3_S2
+        gtsam::CameraSetCal3Bundler
         gtsam::KeyPairDoubleMap)
         
 pybind_wrap(gtsam_unstable_py # target

python/gtsam/preamble.h

@@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
+PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >);
+PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >);

python/gtsam/specializations.h

@@ -13,3 +13,5 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "Bina
 py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
 py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");

python/gtsam/tests/test_Triangulation.py

@@ -12,69 +12,123 @@ import unittest
 
 import numpy as np
 
-import gtsam as g
+import gtsam
+from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
+                   CameraSetCal3Bundler, PinholeCameraCal3_S2,
+                   PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3,
+                   Pose3Vector, Rot3)
 from gtsam.utils.test_case import GtsamTestCase
-from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
+
-    PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3
 
 class TestVisualISAMExample(GtsamTestCase):
-    def test_TriangulationExample(self):
+    """ Tests for triangulation with shared and individual calibrations """
-        # Some common constants
-        sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
 
+    def setUp(self):
+        """ Set up two camera poses """
         # Looking along X-axis, 1 meter above ground plane (x-y)
         upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
         pose1 = Pose3(upright, Point3(0, 0, 1))
-        camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
 
         # create second camera 1 meter to the right of first camera
         pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
-        camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
+        # twoPoses
+        self.poses = Pose3Vector()
+        self.poses.append(pose1)
+        self.poses.append(pose2)
 
         # landmark ~5 meters infront of camera
-        landmark = Point3(5, 0.5, 1.2)
+        self.landmark = Point3(5, 0.5, 1.2)
 
-        # 1. Project two landmarks into two cameras and triangulate
+    def generate_measurements(self, calibration, camera_model, cal_params, camera_set=None):
-        z1 = camera1.project(landmark)
+        """
-        z2 = camera2.project(landmark)
+        Generate vector of measurements for given calibration and camera model.
 
-        # twoPoses
+        Args: 
-        poses = Pose3Vector()
+            calibration: Camera calibration e.g. Cal3_S2
+            camera_model: Camera model e.g. PinholeCameraCal3_S2
+            cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
+            camera_set: Cameraset object (for individual calibrations)
+        Returns:
+            list of measurements and list/CameraSet object for cameras
+        """
+        if camera_set is not None:
+            cameras = camera_set()
+        else:
+            cameras = []
         measurements = Point2Vector()
 
-        poses.append(pose1)
+        for k, pose in zip(cal_params, self.poses):
-        poses.append(pose2)
+            K = calibration(*k)
-        measurements.append(z1)
+            camera = camera_model(pose, K)
-        measurements.append(z2)
+            cameras.append(camera)
+            z = camera.project(self.landmark)
+            measurements.append(z)
 
-        optimize = True
+        return measurements, cameras
-        rank_tol = 1e-9
 
-        triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
+    def test_TriangulationExample(self):
-        self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
+        """ Tests triangulation with shared Cal3_S2 calibration"""
+        # Some common constants
+        sharedCal = (1500, 1200, 0, 640, 480)
 
-        # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+        measurements, _ = self.generate_measurements(Cal3_S2,
-        measurements = Point2Vector()
+                                                     PinholeCameraCal3_S2,
-        measurements.append(z1 - np.array([0.1, 0.5]))
+                                                     (sharedCal, sharedCal))
-        measurements.append(z2 - np.array([-0.2, 0.3]))
+
+        triangulated_landmark = gtsam.triangulatePoint3(self.poses,
+                                                        Cal3_S2(sharedCal),
+                                                        measurements,
+                                                        rank_tol=1e-9,
+                                                        optimize=True)
+        self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
+
+        # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+        measurements_noisy = Point2Vector()
+        measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
+        measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
+
+        triangulated_landmark = gtsam.triangulatePoint3(self.poses,
+                                                        Cal3_S2(sharedCal),
+                                                        measurements_noisy,
+                                                        rank_tol=1e-9,
+                                                        optimize=True)
+
+        self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
+
+    def test_distinct_Ks(self):
+        """ Tests triangulation with individual Cal3_S2 calibrations """
+        # two camera parameters
+        K1 = (1500, 1200, 0, 640, 480)
+        K2 = (1600, 1300, 0, 650, 440)
+
+        measurements, cameras = self.generate_measurements(Cal3_S2,
+                                                           PinholeCameraCal3_S2,
+                                                           (K1, K2),
+                                                           camera_set=CameraSetCal3_S2)
+
+        triangulated_landmark = gtsam.triangulatePoint3(cameras,
+                                                        measurements,
+                                                        rank_tol=1e-9,
+                                                        optimize=True)
+        self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
+
+    def test_distinct_Ks_Bundler(self):
+        """ Tests triangulation with individual Cal3Bundler calibrations"""
+        # two camera parameters
+        K1 = (1500, 0, 0, 640, 480)
+        K2 = (1600, 0, 0, 650, 440)
+
+        measurements, cameras = self.generate_measurements(Cal3Bundler,
+                                                           PinholeCameraCal3Bundler,
+                                                           (K1, K2),
+                                                           camera_set=CameraSetCal3Bundler)
+
+        triangulated_landmark = gtsam.triangulatePoint3(cameras,
+                                                        measurements,
+                                                        rank_tol=1e-9,
+                                                        optimize=True)
+        self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
 
-        triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
-        self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2)
-        #
-        # # two Poses with Bundler Calibration
-        # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480)
-        # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal)
-        # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal)
-        #
-        # z1 = camera1.project(landmark)
-        # z2 = camera2.project(landmark)
-        #
-        # measurements = Point2Vector()
-        # measurements.append(z1)
-        # measurements.append(z2)
-        #
-        # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize)
-        # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
 
 if __name__ == "__main__":
     unittest.main()