From 9967c59ed0c9a1112bbffa7d84908154439e22cf Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 14:30:16 +0200 Subject: [PATCH 01/22] Forward declaration for Set of Fisheye Cameras Forward declaration of camera vector for PinholeCamera and PinholeCamera. --- gtsam/geometry/triangulation.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22e..b18c55818d 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include #include @@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3Fisheye = CameraSet>; +using CameraSetCal3Unified = CameraSet>; } // \namespace gtsam From dfd50f98c26791e68ab670c1768605fba9f20209 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 15:13:29 +0200 Subject: [PATCH 02/22] Extend python wrapper to include fisheye models. Extend python wrapper to include fisheye camera models Cal3Fisheye and Cal3Unified. --- gtsam/gtsam.i | 102 ++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 90 insertions(+), 12 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953bb..327574bf89 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -231,7 +231,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; @@ -977,6 +977,52 @@ class Cal3Bundler { void pickle() const; }; +#include +class Cal3Fisheye { + // Standard Constructors + Cal3Fisheye(); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4, double tol); + Cal3Fisheye(Vector v); + + // Testable + void print(string s = "Cal3Fisheye") const; + bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Fisheye retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Fisheye& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double k1() const; + double k2() const; + double k3() const; + double k4() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + Matrix inverse() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + + #include class CalibratedCamera { // Standard Constructors and Named Constructors @@ -1085,6 +1131,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; template class CameraSet { @@ -1145,7 +1192,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); - +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); + //************************************************************************* // Symbolic //************************************************************************* @@ -2118,8 +2171,11 @@ class NonlinearFactorGraph { template , + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -2252,9 +2308,13 @@ class Values { void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void insert(size_t j, const gtsam::Cal3Unified& cal3unified); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Unified& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); @@ -2272,9 +2332,13 @@ class Values { void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void update(size_t j, const gtsam::Cal3Unified& cal3unified); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Unified& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); @@ -2294,10 +2358,14 @@ class Values { gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, - gtsam::EssentialMatrix, + gtsam::Cal3Fisheye, + gtsam::Cal3Unified, + gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, - gtsam::PinholeCamera, - gtsam::imuBias::ConstantBias, + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, + gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix, @@ -2603,7 +2671,9 @@ class ISAM2 { template , + gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler, + gtsam::Cal3Fisheye, gtsam::Cal3Unified, + gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; @@ -2655,10 +2725,14 @@ template }> + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2800,6 +2874,8 @@ virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Fisheye; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; #include @@ -2810,9 +2886,11 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { }; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Fisheye; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Unified; -template +template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); gtsam::Point2 measured() const; From 55c12743fc29191c0e3d63c12b7c52f8284c402f Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 15:45:11 +0200 Subject: [PATCH 03/22] Forward declaration of fisheye camera. Forward declaration of PinholeCal3Fisheye needed by Python wrapper. --- gtsam/geometry/SimpleCamera.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c77..2c34bdfa73 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -33,7 +34,8 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; - + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x From c8fc3cd216020487be3850d58b56eca9ff74ec99 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 19:53:41 +0200 Subject: [PATCH 04/22] Unit test for equidistant fisheye --- python/gtsam/tests/test_Cal3Fisheye.py | 105 +++++++++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 python/gtsam/tests/test_Cal3Fisheye.py diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py new file mode 100644 index 0000000000..18c8ecd80e --- /dev/null +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -0,0 +1,105 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestCal3Fisheye(GtsamTestCase): + + def test_Cal3Fisheye(self): + K = gtsam.Cal3Fisheye() + self.assertEqual(K.fx(), 1.) + self.assertEqual(K.fy(), 1.) + + def test_distortion(self): + "Equidistant fisheye model of focal length f, defined as r/f = tan(theta)" + equidistant = gtsam.Cal3Fisheye() + x, y, z = 1, 0, 1 + u, v = equidistant.uncalibrate([x, y]) + x2, y2 = equidistant.calibrate([u, v]) + self.assertAlmostEqual(u, np.arctan2(x, z)) + self.assertAlmostEqual(v, 0) + self.assertAlmostEqual(x2, x) + self.assertAlmostEqual(y2, 0) + + def test_pinhole(self): + "Spatial equidistant camera projection" + x, y, z = 1.0, 0.0, 1.0 + u, v = np.arctan2(x, z), 0.0 + camera = gtsam.PinholeCameraCal3Fisheye() + + pt1 = camera.Project([x, y, z]) + self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) + + pt2 = camera.project([x, y, z]) + self.gtsamAssertEquals(pt2, np.array([u, v])) + + obj1 = camera.backproject([u, v], z) + self.gtsamAssertEquals(obj1, np.array([x, y, z])) + + r1 = camera.range(np.array([x, y, z])) + self.assertEqual(r1, np.linalg.norm([x, y, z])) + + def test_generic_factor(self): + "Evaluate residual using pose and point as state variables" + objPoint = np.array([1, 0, 1]) + imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + poseKey = gtsam.symbol_shorthand.P(0) + pointKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Fisheye() + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(pointKey, gtsam.Point3(objPoint)) + factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_sfm_factor2(self): + "Evaluate residual with camera, pose and point as state variables" + objPoint = np.array([1, 0, 1]) + imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + cameraKey = gtsam.symbol_shorthand.K(0) + poseKey = gtsam.symbol_shorthand.P(0) + landmarkKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Fisheye() + state.insert_cal3fisheye(cameraKey, k) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_retract(self): + expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) + K = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3) + d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F') + actual = K.retract(d) + self.gtsamAssertEquals(actual, expected) + np.testing.assert_allclose(d, K.localCoordinates(actual)) + + +if __name__ == "__main__": + unittest.main() From 19e8cde733c40e2e6dc0beb3c313c55b34e354fa Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 19:59:56 +0200 Subject: [PATCH 05/22] Extend unit testing of omnidirectional projection Test projection function and factors using a stereoscopic (xi=1) reference model, i.e the image height is given by y = 2 f tan(theta/2). --- python/gtsam/tests/test_Cal3Unified.py | 83 ++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index fbf5f3565c..ff9d659607 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -23,6 +23,89 @@ def test_Cal3Unified(self): self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) + def test_distortion(self): + "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + x, y, z = 1, 0, 1 + u, v = stereographic.uncalibrate([x, y]) + r = np.linalg.norm([x, y, z]) + # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2)) + self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r)) + self.assertAlmostEqual(u, 2/(1+r)) + x2, y2 = stereographic.calibrate([u, v]) + self.assertAlmostEqual(x2, x) + + def test_pinhole(self): + "Spatial stereographic camera projection" + x, y, z = 1.0, 0.0, 1.0 + r = np.linalg.norm([x, y, z]) + u, v = 2/(1+r), 0.0 + objPoint = np.array([x, y, z]) + imgPoint = np.array([u, v]) + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + pose = gtsam.Pose3() + camera = gtsam.PinholeCameraCal3Unified(pose, stereographic) + pt1 = camera.Project(objPoint) + self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) + pt2 = camera.project(objPoint) + self.gtsamAssertEquals(pt2, np.array([u, v])) + obj1 = camera.backproject([u, v], z) + self.gtsamAssertEquals(obj1, np.array([x, y, z])) + r1 = camera.range(np.array([x, y, z])) + self.assertEqual(r1, r) + + def test_generic_factor(self): + "Evaluate residual using pose and point as state variables" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + objPoint = np.array([1, 0, 1]) + r = np.linalg.norm(objPoint) + imgPoint = np.array([2/(1+r), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + poseKey = gtsam.symbol_shorthand.P(0) + pointKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(pointKey, gtsam.Point3(objPoint)) + factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_sfm_factor2(self): + "Evaluate residual with camera, pose and point as state variables" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + objPoint = np.array([1, 0, 1]) + r = np.linalg.norm(objPoint) + imgPoint = np.array([2/(1+r), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + cameraKey = gtsam.symbol_shorthand.K(0) + poseKey = gtsam.symbol_shorthand.P(0) + landmarkKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + state.insert_cal3unified(cameraKey, k) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) + factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) From bdeb60679b7ea8f9b1a7a53166da65590b007bcb Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:14:10 +0200 Subject: [PATCH 06/22] Introduce setUpClass, python snake_case variables Test case fails if object depth z is not equal 1. --- python/gtsam/tests/test_Cal3Fisheye.py | 103 +++++++++++++------------ 1 file changed, 53 insertions(+), 50 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 18c8ecd80e..23f7a9b8c3 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -5,8 +5,9 @@ See LICENSE for the license information -Cal3Unified unit tests. +Cal3Fisheye unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) +Refactored: Roderick Koehle """ import unittest @@ -14,78 +15,80 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase - +from gtsam.symbol_shorthand import K, L, P class TestCal3Fisheye(GtsamTestCase): - + + @classmethod + def setUpClass(cls): + """ + Equidistant fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = tan(theta), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + # x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails! + u, v = np.arctan2(x, z), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fy(), 1.) def test_distortion(self): - "Equidistant fisheye model of focal length f, defined as r/f = tan(theta)" + """Fisheye distortion and rectification""" equidistant = gtsam.Cal3Fisheye() - x, y, z = 1, 0, 1 - u, v = equidistant.uncalibrate([x, y]) - x2, y2 = equidistant.calibrate([u, v]) - self.assertAlmostEqual(u, np.arctan2(x, z)) - self.assertAlmostEqual(v, 0) - self.assertAlmostEqual(x2, x) - self.assertAlmostEqual(y2, 0) + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = equidistant.uncalibrate(perspective_pt) + rectified_pt = equidistant.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) def test_pinhole(self): - "Spatial equidistant camera projection" - x, y, z = 1.0, 0.0, 1.0 - u, v = np.arctan2(x, z), 0.0 + """Spatial equidistant camera projection""" camera = gtsam.PinholeCameraCal3Fisheye() - - pt1 = camera.Project([x, y, z]) + pt1 = camera.Project(self.obj_point) # Perspective projection + pt2 = camera.project(self.obj_point) # Equidistant projection + x, y, z = self.obj_point + obj1 = camera.backproject(self.img_point, z) + r1 = camera.range(self.obj_point) + r = np.linalg.norm(self.obj_point) self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) - - pt2 = camera.project([x, y, z]) - self.gtsamAssertEquals(pt2, np.array([u, v])) - - obj1 = camera.backproject([u, v], z) - self.gtsamAssertEquals(obj1, np.array([x, y, z])) - - r1 = camera.range(np.array([x, y, z])) - self.assertEqual(r1, np.linalg.norm([x, y, z])) + self.gtsamAssertEquals(pt2, self.img_point) + self.gtsamAssertEquals(obj1, self.obj_point) + self.assertEqual(r1, r) def test_generic_factor(self): - "Evaluate residual using pose and point as state variables" - objPoint = np.array([1, 0, 1]) - imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + """Evaluate residual using pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - poseKey = gtsam.symbol_shorthand.P(0) - pointKey = gtsam.symbol_shorthand.L(0) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) k = gtsam.Cal3Fisheye() - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(pointKey, gtsam.Point3(objPoint)) - factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) def test_sfm_factor2(self): - "Evaluate residual with camera, pose and point as state variables" - objPoint = np.array([1, 0, 1]) - imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + """Evaluate residual with camera, pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - cameraKey = gtsam.symbol_shorthand.K(0) - poseKey = gtsam.symbol_shorthand.P(0) - landmarkKey = gtsam.symbol_shorthand.L(0) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) k = gtsam.Cal3Fisheye() - state.insert_cal3fisheye(cameraKey, k) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) - factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey) + state.insert_cal3fisheye(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, gtsam.Point3(self.obj_point)) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) @@ -93,12 +96,12 @@ def test_sfm_factor2(self): def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) - K = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, + k = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3) d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F') - actual = K.retract(d) + actual = k.retract(d) self.gtsamAssertEquals(actual, expected) - np.testing.assert_allclose(d, K.localCoordinates(actual)) + np.testing.assert_allclose(d, k.localCoordinates(actual)) if __name__ == "__main__": From 6205057ccbd63c3705ac109b47104d14e10b41e9 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:17:38 +0200 Subject: [PATCH 07/22] Use of common setUpClass method --- python/gtsam/tests/test_Cal3Unified.py | 121 ++++++++++++------------- 1 file changed, 60 insertions(+), 61 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index ff9d659607..f2c1ada488 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -14,94 +14,93 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import K, L, P class TestCal3Unified(GtsamTestCase): + @classmethod + def setUpClass(cls): + """ + Stereographic fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = 2*tan(theta/2), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + r = np.linalg.norm([x, y, z]) + u, v = 2*x/(z+r), 0.0 + #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) def test_distortion(self): - "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - x, y, z = 1, 0, 1 - u, v = stereographic.uncalibrate([x, y]) + """Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)""" + x, y, z = self.obj_point r = np.linalg.norm([x, y, z]) - # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2)) - self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r)) - self.assertAlmostEqual(u, 2/(1+r)) - x2, y2 = stereographic.calibrate([u, v]) - self.assertAlmostEqual(x2, x) + # Note: 2*tan(atan2(x, z)/2) = 2*x/(z+sqrt(x^2+z^2)) + self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2*x/(z+r)) + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = self.stereographic.uncalibrate(perspective_pt) + rectified_pt = self.stereographic.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) def test_pinhole(self): - "Spatial stereographic camera projection" - x, y, z = 1.0, 0.0, 1.0 - r = np.linalg.norm([x, y, z]) - u, v = 2/(1+r), 0.0 - objPoint = np.array([x, y, z]) - imgPoint = np.array([u, v]) - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + """Spatial stereographic camera projection""" + x, y, z = self.obj_point + u, v = self.img_point + r = np.linalg.norm(self.obj_point) pose = gtsam.Pose3() - camera = gtsam.PinholeCameraCal3Unified(pose, stereographic) - pt1 = camera.Project(objPoint) + camera = gtsam.PinholeCameraCal3Unified(pose, self.stereographic) + pt1 = camera.Project(self.obj_point) self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) - pt2 = camera.project(objPoint) - self.gtsamAssertEquals(pt2, np.array([u, v])) - obj1 = camera.backproject([u, v], z) - self.gtsamAssertEquals(obj1, np.array([x, y, z])) - r1 = camera.range(np.array([x, y, z])) + pt2 = camera.project(self.obj_point) + self.gtsamAssertEquals(pt2, self.img_point) + obj1 = camera.backproject(self.img_point, z) + self.gtsamAssertEquals(obj1, self.obj_point) + r1 = camera.range(self.obj_point) self.assertEqual(r1, r) def test_generic_factor(self): - "Evaluate residual using pose and point as state variables" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - objPoint = np.array([1, 0, 1]) - r = np.linalg.norm(objPoint) - imgPoint = np.array([2/(1+r), 0]) + """Evaluate residual using pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - poseKey = gtsam.symbol_shorthand.P(0) - pointKey = gtsam.symbol_shorthand.L(0) - k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(pointKey, gtsam.Point3(objPoint)) - factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) + k = self.stereographic + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) def test_sfm_factor2(self): - "Evaluate residual with camera, pose and point as state variables" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - objPoint = np.array([1, 0, 1]) - r = np.linalg.norm(objPoint) - imgPoint = np.array([2/(1+r), 0]) + """Evaluate residual with camera, pose and point as state variables""" + r = np.linalg.norm(self.obj_point) graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - cameraKey = gtsam.symbol_shorthand.K(0) - poseKey = gtsam.symbol_shorthand.P(0) - landmarkKey = gtsam.symbol_shorthand.L(0) - k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - state.insert_cal3unified(cameraKey, k) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) - factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + k = self.stereographic + state.insert_cal3unified(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, self.obj_point) + factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) From a411b664a198776f21aa6fd9a43732841e44add7 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:25:18 +0200 Subject: [PATCH 08/22] Correct tab to spaces to fix formatting --- gtsam/gtsam.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 327574bf89..ce251802ce 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2360,12 +2360,12 @@ class Values { gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, - gtsam::EssentialMatrix, + gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified, - gtsam::imuBias::ConstantBias, + gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix, From 66af0079baa5de84618fdc1927dc55e8e9fcff81 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 12:39:31 +0200 Subject: [PATCH 09/22] Improved accuracy for analytic undistortion --- gtsam/geometry/Cal3Fisheye.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index b9e60aceec..3ee036efff 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -110,7 +110,9 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, const double u = uv.x(), v = uv.y(); const double yd = (v - v0_) / fy_; const double xd = (u - s_ * yd - u0_) / fx_; - Point2 pi(xd, yd); + const double theta = sqrt(xd * xd + yd * yd); + const double scale = (theta > 0) ? tan(theta) / theta; + Point2 pi(scale * xd, scale * yd); // Perform newtons method, break when solution converges past tol_, // throw exception if max iterations are reached From 03049929a55d430bc67404bf8663d792ee9d62fb Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 15:21:22 +0200 Subject: [PATCH 10/22] Add comment about initial guess in undistortion For the equidistant fisheye model, r/f = tan(theta), the Gauss-Newton search to model radial distortion is expected to converge faster by mapping the angular coordinate space into the respective tangent space of the perspective plane. This is consistent to the nPlaneToSpace initial projection used in the calibrate() function of the omnidirectional model (Cal3Unified). --- gtsam/geometry/Cal3Fisheye.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 3ee036efff..52d475d5d1 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -106,12 +106,20 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, /* ************************************************************************* */ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, OptionalJacobian<2, 2> Dp) const { - // initial gues just inverts the pinhole model + // Apply inverse camera matrix to map the pixel coordinate (u, v) + // of the equidistant fisheye image to angular coordinate space (xd, yd) + // with radius theta given in radians. const double u = uv.x(), v = uv.y(); const double yd = (v - v0_) / fy_; const double xd = (u - s_ * yd - u0_) / fx_; const double theta = sqrt(xd * xd + yd * yd); - const double scale = (theta > 0) ? tan(theta) / theta; + + // Provide initial guess for the Gauss-Newton search. + // The angular coordinates given by (xd, yd) are mapped back to + // the focal plane of the perspective undistorted projection pi. + // See Cal3Unified.calibrate() using the same pattern for the + // undistortion of omnidirectional fisheye projection. + const double scale = (theta > 0) ? tan(theta) / theta : 1.0; Point2 pi(scale * xd, scale * yd); // Perform newtons method, break when solution converges past tol_, From d54e234f93128c3c612e0ef0a2c65cb9a1182fae Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 22:03:17 +0200 Subject: [PATCH 11/22] Add ambiguous calibrate/uncalibrate declarations. Without declaring calibrate / uncalibrated in the interface specification, the functions of the Base class Cal3DS2_Base is called. The layout of the optional Jacobian matrix is 2x10 in Cal3Unified and 2x9 in Cal3DS2_Base, so this are different function calls. --- gtsam/gtsam.i | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index ce251802ce..97012c2910 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -910,6 +910,12 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { gtsam::Cal3Unified retract(Vector v) const; Vector localCoordinates(const gtsam::Cal3Unified& c) const; + // Action on Point2 + // Note: the signature of this functions differ from the functions + // with equal name in the base class. + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + // enabling serialization functionality void serialize() const; From 3118fde6d3dce098ffbca641afb18488f97a9dbc Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 23:00:24 +0200 Subject: [PATCH 12/22] Missing CameraSet binding specialisations Add pybind specialisations for CameraSetCal3Unified and CameraSetCal3Fisheye. --- python/gtsam/specializations.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index be8eb8a6cf..2e8612fec6 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -32,4 +32,6 @@ py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector > >(m_, "CameraSetCal3_S2"); py::bind_vector > >(m_, "CameraSetCal3Bundler"); +py::bind_vector > >(m_, "CameraSetCal3Unified"); +py::bind_vector > >(m_, "CameraSetCal3Fisheye"); py::bind_vector >(m_, "JacobianVector"); From 0a73961f5a2762100d30147db25fde150434d1a6 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 23:05:53 +0200 Subject: [PATCH 13/22] Update ignore list in CMakeFile --- python/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 5f51368e6a..528732f4bd 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -102,6 +102,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler + gtsam::CameraSetCal3Unified + gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_unstable_py # target From 941594c94be1d1223d7fbcf9d1181f2c73411574 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sun, 11 Jul 2021 14:11:40 +0200 Subject: [PATCH 14/22] Testing CameraSet and triangulatePoint3 Currently triangulatePoint3 returns wrong results for fisheye models. The template for PinholePose may be implemented for a fixed size of variable dimensions. --- python/gtsam/tests/test_Cal3Fisheye.py | 40 +++++++++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 23f7a9b8c3..d731204ef8 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -29,7 +29,7 @@ def setUpClass(cls): image plane and theta the incident angle of the object point. """ x, y, z = 1.0, 0.0, 1.0 - # x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails! + # x, y, z = 0.5, 0.0, 2.0 u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) @@ -93,6 +93,44 @@ def test_sfm_factor2(self): score = graph.error(state) self.assertAlmostEqual(score, 0) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") + def test_triangulation_skipped(self): + """Estimate spatial point from image measurements""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + + def test_triangulation_rectify(self): + """Estimate spatial point from image measurements using rectification""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + shared_cal = gtsam.Cal3_S2() + poses = gtsam.Pose3Vector([pose1, pose2]) + triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) From c595767caeba831701298ab410a693048b2fe1db Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sun, 11 Jul 2021 14:14:08 +0200 Subject: [PATCH 15/22] Unittest, triangulation for Cal3Unified --- python/gtsam/tests/test_Cal3Unified.py | 38 ++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index f2c1ada488..0b09fc3ae7 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -105,6 +105,44 @@ def test_sfm_factor2(self): score = graph.error(state) self.assertAlmostEqual(score, 0) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") + def test_triangulation(self): + """Estimate spatial point from image measurements""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) + cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + + def test_triangulation_rectify(self): + """Estimate spatial point from image measurements using rectification""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) + cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + shared_cal = gtsam.Cal3_S2() + poses = gtsam.Pose3Vector([pose1, pose2]) + triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) From 3402e46ad19cc6e2bef1c71cd777b167e09fe136 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:00:21 +0200 Subject: [PATCH 16/22] Shared data for triangulation unit tests --- python/gtsam/tests/test_Cal3Fisheye.py | 46 ++++++++++---------------- 1 file changed, 18 insertions(+), 28 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index d731204ef8..756113b930 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -33,6 +33,19 @@ def setUpClass(cls): u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) + + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() @@ -96,40 +109,17 @@ def test_sfm_factor2(self): @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) - camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) - cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) - camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) - cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() - poses = gtsam.Pose3Vector([pose1, pose2]) - triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, From 17c37de7c4325bebdbfad1531ecd270f4ca80eb2 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:07:19 +0200 Subject: [PATCH 17/22] Shared setup triangulation unit test --- python/gtsam/tests/test_Cal3Unified.py | 46 ++++++++++---------------- 1 file changed, 18 insertions(+), 28 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index 0b09fc3ae7..ca0959e44c 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -40,6 +40,19 @@ def setUpClass(cls): xi = 1 cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) @@ -108,40 +121,17 @@ def test_sfm_factor2(self): @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) - camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) - cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) - camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) - cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() - poses = gtsam.Pose3Vector([pose1, pose2]) - triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, From 3e41ece75a0ba8fc923ea6f59d912fd52f3914d3 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:10:38 +0200 Subject: [PATCH 18/22] Minor fix test_Cal3Unified --- python/gtsam/tests/test_Cal3Unified.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index ca0959e44c..a402ae509f 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -122,7 +122,6 @@ def test_sfm_factor2(self): def test_triangulation(self): """Estimate spatial point from image measurements""" triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): @@ -130,7 +129,6 @@ def test_triangulation_rectify(self): rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): From d130387a7db0b52080905c15ddcebfa22484d0de Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:12:14 +0200 Subject: [PATCH 19/22] Minor fix test_Cal3Fisheye --- python/gtsam/tests/test_Cal3Fisheye.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 756113b930..646b48881c 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -110,7 +110,6 @@ def test_sfm_factor2(self): def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): @@ -118,7 +117,6 @@ def test_triangulation_rectify(self): rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): From 16cfc7fd381a9250bb573a4613037299be770d9f Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:15:10 +0200 Subject: [PATCH 20/22] Remove commented out line --- python/gtsam/tests/test_Cal3Fisheye.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 646b48881c..298c6e57b9 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -29,7 +29,6 @@ def setUpClass(cls): image plane and theta the incident angle of the object point. """ x, y, z = 1.0, 0.0, 1.0 - # x, y, z = 0.5, 0.0, 2.0 u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) From c2bbe78e867bf1797ef33be444cc400bfe946d71 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:16:00 +0200 Subject: [PATCH 21/22] Remove comment --- python/gtsam/tests/test_Cal3Unified.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index a402ae509f..dab1ae4466 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -31,7 +31,6 @@ def setUpClass(cls): x, y, z = 1.0, 0.0, 1.0 r = np.linalg.norm([x, y, z]) u, v = 2*x/(z+r), 0.0 - #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) From a115788ea5d56df6cfdbc127ab24b6ebdbb9d157 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:53:31 +0200 Subject: [PATCH 22/22] Remove spaces in empty line --- gtsam/geometry/SimpleCamera.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 2c34bdfa73..119e9d1a3c 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -35,7 +35,7 @@ namespace gtsam { using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; - + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x