Merge pull request #1159 from shteren1/rs_wrapper

added wrapper for ProjectionFactorRollingShutter

.gitignore

@@ -17,3 +17,4 @@
 # for QtCreator:
 CMakeLists.txt.user*
 xcode/
+/Dockerfile

gtsam_unstable/gtsam_unstable.i

@@ -797,4 +797,30 @@ virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor {
 typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCCal3_S2;
 typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCCal3DS2;
 
+#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
+virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor {
+  ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
+      size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K);
+
+  ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
+    size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, gtsam::Pose3& body_P_sensor);
+
+  ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
+        size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality,
+        bool verboseCheirality);
+
+  ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,
+      size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality,
+      bool verboseCheirality, gtsam::Pose3& body_P_sensor);
+
+  gtsam::Point2 measured() const;
+  double alpha() const;
+  gtsam::Cal3_S2* calibration() const;
+  bool verboseCheirality() const;
+  bool throwCheirality() const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 } //\namespace gtsam

python/README.md

@@ -8,6 +8,7 @@ For instructions on updating the version of the [wrap library](https://github.co
 
 ## Requirements
 
+- Cmake >= 3.15
 - If you want to build the GTSAM python library for a specific python version (eg 3.6),
   use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used.
 - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment),

python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py

@@ -0,0 +1,59 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+ProjectionFactorRollingShutter unit tests.
+Author: Yotam Stern
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+import gtsam_unstable
+from gtsam.utils.test_case import GtsamTestCase
+
+
+pose1 = gtsam.Pose3()
+pose2 = gtsam.Pose3(np.array([[ 0.9999375 ,  0.00502487,  0.00998725,  0.1       ],
+                              [-0.00497488,  0.999975  , -0.00502487,  0.02      ],
+                              [-0.01001225,  0.00497488,  0.9999375 ,  1.        ],
+                              [ 0.        ,  0.        ,  0.        ,  1.        ]]))
+point = np.array([2, 0, 15])
+point_noise = gtsam.noiseModel.Diagonal.Sigmas(np.ones(2))
+cal = gtsam.Cal3_S2()
+body_p_sensor = gtsam.Pose3()
+alpha = 0.1
+measured = np.array([0.13257015, 0.0004157])
+
+
+class TestProjectionFactorRollingShutter(GtsamTestCase):
+
+    def test_constructor(self):
+        '''
+        test constructor for the ProjectionFactorRollingShutter
+        '''
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal)
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal,
+                                                               body_p_sensor)
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, True, False)
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, True, False,
+                                                               body_p_sensor)
+
+    def test_error(self):
+        '''
+        test the factor error for a specific example
+        '''
+        values = gtsam.Values()
+        values.insert(0, pose1)
+        values.insert(1, pose2)
+        values.insert(2, point)
+        factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal)
+        self.gtsamAssertEquals(factor.error(values), np.array(0), tol=1e-9)
+
+
+if __name__ == '__main__':
+    unittest.main()