12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #811 from roderick-koehle/python-fisheye-interface 

Python fisheye interface
release/4.3a0
Fan Jiang 2021-07-14 17:48:44 -04:00 committed by GitHub
parent fa42d96360 a115788ea5
commit 740c9c6f39
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 356 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -106,11 +106,21 @@ 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_;
Point2 pi(xd, yd);
const double theta = sqrt(xd * xd + yd * yd);
// 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_,
// throw exception if max iterations are reached

2
gtsam/geometry/SimpleCamera.h
View File

@ -22,6 +22,7 @@
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
@ -33,6 +34,7 @@ namespace gtsam {
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**

4
gtsam/geometry/triangulation.h
View File

@ -20,6 +20,8 @@
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
@ -499,6 +501,8 @@ 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>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
} // \namespace gtsam

104
gtsam/gtsam.i
View File

@ -231,7 +231,7 @@ virtual class Value {
};
#include <gtsam/base/GenericValue.h>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
@ -911,6 +911,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;
@ -978,6 +984,52 @@ class Cal3Bundler {
void pickle() const;
};
#include <gtsam/geometry/Cal3Fisheye.h>
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 <gtsam/geometry/CalibratedCamera.h>
class CalibratedCamera {
// Standard Constructors and Named Constructors
@ -1086,6 +1138,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
template<T>
class CameraSet {
@ -1146,7 +1199,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
//*************************************************************************
@ -2119,8 +2178,11 @@ class NonlinearFactorGraph {
template <T = {double, Vector, gtsam::Point2, gtsam::StereoPoint2,
gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4,
gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,
gtsam::Cal3Fisheye, gtsam::Cal3Unified,
gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified,
gtsam::imuBias::ConstantBias}>
void addPrior(size_t key, const T& prior,
const gtsam::noiseModel::Base* noiseModel);
@ -2253,9 +2315,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<gtsam::Cal3Bundler>& 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);
@ -2273,9 +2339,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<gtsam::Cal3Bundler>& 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);
@ -2295,9 +2365,13 @@ class Values {
gtsam::Cal3_S2,
gtsam::Cal3DS2,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::EssentialMatrix,
gtsam::PinholeCameraCal3_S2,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified,
gtsam::imuBias::ConstantBias,
gtsam::NavState,
Vector,
@ -2604,7 +2678,9 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
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;
@ -2656,10 +2732,14 @@ template <T = {double,
gtsam::Cal3_S2,
gtsam::Cal3DS2,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::CalibratedCamera,
gtsam::PinholeCameraCal3_S2,
gtsam::imuBias::ConstantBias,
gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
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;
@ -2801,6 +2881,8 @@ virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Fisheye> GenericProjectionFactorCal3Fisheye;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Unified> GenericProjectionFactorCal3Unified;
#include <gtsam/slam/GeneralSFMFactor.h>
@ -2811,9 +2893,11 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Bundler, gtsam::Point3> GeneralSFMFactorCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Fisheye, gtsam::Point3> GeneralSFMFactorCal3Fisheye;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Unified, gtsam::Point3> GeneralSFMFactorCal3Unified;
template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
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;

2
python/CMakeLists.txt
View File

@ -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

2
python/gtsam/specializations.h
View File

@ -32,4 +32,6 @@ 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");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified> > >(m_, "CameraSetCal3Unified");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Fisheye> > >(m_, "CameraSetCal3Fisheye");
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

133
python/gtsam/tests/test_Cal3Fisheye.py Normal file
View File

@ -0,0 +1,133 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Cal3Fisheye unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
Refactored: Roderick Koehle
"""
import unittest
import numpy as np
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
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()
self.assertEqual(K.fx(), 1.)
self.assertEqual(K.fy(), 1.)
def test_distortion(self):
"""Fisheye distortion and rectification"""
equidistant = gtsam.Cal3Fisheye()
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"""
camera = gtsam.PinholeCameraCal3Fisheye()
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]))
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"""
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
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(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"""
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
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(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)
@unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
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(triangulated, self.origin)
def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification"""
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(triangulated, self.origin)
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()

107
python/gtsam/tests/test_Cal3Unified.py
View File

@ -14,15 +14,122 @@ import numpy as np
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
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)
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.)
self.assertEqual(K.fx(), 1.)
def test_distortion(self):
"""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*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 = self.obj_point
u, v = self.img_point
r = np.linalg.norm(self.obj_point)
pose = gtsam.Pose3()
camera = gtsam.PinholeCameraCal3Unified(pose, self.stereographic)
pt1 = camera.Project(self.obj_point)
self.gtsamAssertEquals(pt1, np.array([x/z, 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"""
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
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"""
r = np.linalg.norm(self.obj_point)
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
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)
@unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
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(triangulated, self.origin)
def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification"""
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(triangulated, self.origin)
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)