From 894605be3f5ff7365c3f3a5330bdb25b509a05ff Mon Sep 17 00:00:00 2001 From: yotams Date: Tue, 5 Apr 2022 08:51:03 +0300 Subject: [PATCH 1/3] added wrapper for ProjectionFactorRollingShutter --- gtsam_unstable/gtsam_unstable.i | 26 ++++++++++++++++++++++++++ python/README.md | 1 + 2 files changed, 27 insertions(+) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index dd66e7a730..08cd45e186 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 54436df93b..278d620948 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), From 0a062387850746478c2efe84943ab1b710e1c5bb Mon Sep 17 00:00:00 2001 From: yotams Date: Wed, 6 Apr 2022 08:26:50 +0300 Subject: [PATCH 2/3] added wrapper for ProjectionFactorRollingShutter --- .gitignore | 1 + .../test_ProjectionFactorRollingShutter.py | 59 +++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py diff --git a/.gitignore b/.gitignore index e6e38132f5..0e34eed347 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,4 @@ # for QtCreator: CMakeLists.txt.user* xcode/ +/Dockerfile diff --git a/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py b/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py new file mode 100644 index 0000000000..23a9d08317 --- /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 = 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.assertEqual(factor.error(values), 0) + + +if __name__ == '__main__': + unittest.main() From 936a7ac073cfa3c74e89c73eb3612591dc6a564f Mon Sep 17 00:00:00 2001 From: yotams Date: Wed, 6 Apr 2022 09:27:57 +0300 Subject: [PATCH 3/3] added test for the wrapper --- .../tests/test_ProjectionFactorRollingShutter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py b/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py index 23a9d08317..0e4db3faff 100644 --- a/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py +++ b/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py @@ -27,7 +27,7 @@ cal = gtsam.Cal3_S2() body_p_sensor = gtsam.Pose3() alpha = 0.1 -measured = array([0.13257015, 0.0004157]) +measured = np.array([0.13257015, 0.0004157]) class TestProjectionFactorRollingShutter(GtsamTestCase): @@ -52,7 +52,7 @@ def test_error(self): values.insert(1, pose2) values.insert(2, point) factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal) - self.assertEqual(factor.error(values), 0) + self.gtsamAssertEquals(factor.error(values), np.array(0), tol=1e-9) if __name__ == '__main__':