diff --git a/.gitignore b/.gitignore index e6e38132f..0e34eed34 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,4 @@ # for QtCreator: CMakeLists.txt.user* xcode/ +/Dockerfile diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index dd66e7a73..08cd45e18 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -797,4 +797,30 @@ virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor { typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3_S2; typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3DS2; +#include +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 diff --git a/python/README.md b/python/README.md index 54436df93..278d62094 100644 --- a/python/README.md +++ b/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), diff --git a/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py b/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py new file mode 100644 index 000000000..0e4db3faf --- /dev/null +++ b/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()