209 lines
8.1 KiB
Python
209 lines
8.1 KiB
Python
"""
|
|
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
|
|
from gtsam.symbol_shorthand import K, L, P
|
|
from gtsam.utils.test_case import GtsamTestCase
|
|
|
|
import gtsam
|
|
|
|
|
|
def ulp(ftype=np.float64):
|
|
"""
|
|
Unit in the last place of floating point datatypes
|
|
"""
|
|
f = np.finfo(ftype)
|
|
return f.tiny / ftype(1 << f.nmant)
|
|
|
|
|
|
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)
|
|
|
|
def test_jacobian_on_axis(self):
|
|
"""Check of jacobian at optical axis"""
|
|
obj_point_on_axis = np.array([0, 0, 1])
|
|
img_point = np.array([0, 0])
|
|
f, z, H = self.evaluate_jacobian(obj_point_on_axis, img_point)
|
|
self.assertAlmostEqual(f, 0)
|
|
self.gtsamAssertEquals(z, np.zeros(2))
|
|
self.gtsamAssertEquals(H @ H.T, 3*np.eye(2))
|
|
|
|
def test_jacobian_convergence(self):
|
|
"""Test stability of jacobian close to optical axis"""
|
|
t = ulp(np.float64)
|
|
obj_point_close_to_axis = np.array([t, 0, 1])
|
|
img_point = np.array([np.sqrt(t), 0])
|
|
f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point)
|
|
self.assertAlmostEqual(f, 0)
|
|
self.gtsamAssertEquals(z, np.zeros(2))
|
|
self.gtsamAssertEquals(H @ H.T, 3*np.eye(2))
|
|
|
|
# With a height of sqrt(ulp), this may cause an overflow
|
|
t = ulp(np.float64)
|
|
obj_point_close_to_axis = np.array([np.sqrt(t), 0, 1])
|
|
img_point = np.array([np.sqrt(t), 0])
|
|
f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point)
|
|
self.assertAlmostEqual(f, 0)
|
|
self.gtsamAssertEquals(z, np.zeros(2))
|
|
self.gtsamAssertEquals(H @ H.T, 3*np.eye(2))
|
|
|
|
def test_scaling_factor(self):
|
|
"""Check convergence of atan2(r, z)/r ~ 1/z for small r"""
|
|
r = ulp(np.float64)
|
|
s = np.arctan(r) / r
|
|
self.assertEqual(s, 1.0)
|
|
z = 1
|
|
s = self.scaling_factor(r, z)
|
|
self.assertEqual(s, 1.0/z)
|
|
z = 2
|
|
s = self.scaling_factor(r, z)
|
|
self.assertEqual(s, 1.0/z)
|
|
s = self.scaling_factor(2*r, z)
|
|
self.assertEqual(s, 1.0/z)
|
|
|
|
@staticmethod
|
|
def scaling_factor(r, z):
|
|
"""Projection factor theta/r for equidistant fisheye lens model"""
|
|
return np.arctan2(r, z) / r if r/z != 0 else 1.0/z
|
|
|
|
@staticmethod
|
|
def evaluate_jacobian(obj_point, img_point):
|
|
"""Evaluate jacobian at given object point"""
|
|
pose = gtsam.Pose3()
|
|
camera = gtsam.Cal3Fisheye()
|
|
state = gtsam.Values()
|
|
pose_key, landmark_key = P(0), L(0)
|
|
state.insert_point3(landmark_key, obj_point)
|
|
state.insert_pose3(pose_key, pose)
|
|
g = gtsam.NonlinearFactorGraph()
|
|
noise_model = gtsam.noiseModel.Unit.Create(2)
|
|
factor = gtsam.GenericProjectionFactorCal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera)
|
|
g.add(factor)
|
|
f = g.error(state)
|
|
gaussian_factor_graph = g.linearize(state)
|
|
H, z = gaussian_factor_graph.jacobian()
|
|
return f, z, H
|
|
|
|
@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 = [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()
|