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

Introduce setUpClass, python snake_case variables 

Test case fails if object depth z is not equal 1.
release/4.3a0
roderick-koehle 2021-07-09 11:14:10 +02:00 committed by GitHub
parent 19e8cde733
commit bdeb60679b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 53 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
python/gtsam/tests/test_Cal3Fisheye.py
View File

@ -5,8 +5,9 @@ All Rights Reserved
See LICENSE for the license information See LICENSE for the license information
Cal3Unified unit tests. Cal3Fisheye unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python) Author: Frank Dellaert & Duy Nguyen Ta (Python)
Refactored: Roderick Koehle
""" """
import unittest import unittest
@ -14,78 +15,80 @@ import numpy as np
import gtsam import gtsam
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
from gtsam.symbol_shorthand import K, L, P
class TestCal3Fisheye(GtsamTestCase): 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): def test_Cal3Fisheye(self):
K = gtsam.Cal3Fisheye() K = gtsam.Cal3Fisheye()
self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.)
self.assertEqual(K.fy(), 1.) self.assertEqual(K.fy(), 1.)
def test_distortion(self): def test_distortion(self):
"Equidistant fisheye model of focal length f, defined as r/f = tan(theta)" """Fisheye distortion and rectification"""
equidistant = gtsam.Cal3Fisheye() equidistant = gtsam.Cal3Fisheye()
x, y, z = 1, 0, 1 perspective_pt = self.obj_point[0:2]/self.obj_point[2]
u, v = equidistant.uncalibrate([x, y]) distorted_pt = equidistant.uncalibrate(perspective_pt)
x2, y2 = equidistant.calibrate([u, v]) rectified_pt = equidistant.calibrate(distorted_pt)
self.assertAlmostEqual(u, np.arctan2(x, z)) self.gtsamAssertEquals(distorted_pt, self.img_point)
self.assertAlmostEqual(v, 0) self.gtsamAssertEquals(rectified_pt, perspective_pt)
self.assertAlmostEqual(x2, x)
self.assertAlmostEqual(y2, 0)
def test_pinhole(self): def test_pinhole(self):
"Spatial equidistant camera projection" """Spatial equidistant camera projection"""
x, y, z = 1.0, 0.0, 1.0
u, v = np.arctan2(x, z), 0.0
camera = gtsam.PinholeCameraCal3Fisheye() camera = gtsam.PinholeCameraCal3Fisheye()
pt1 = camera.Project(self.obj_point) # Perspective projection
pt1 = camera.Project([x, y, z]) 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(pt1, np.array([x/z, y/z]))
self.gtsamAssertEquals(pt2, self.img_point)
pt2 = camera.project([x, y, z]) self.gtsamAssertEquals(obj1, self.obj_point)
self.gtsamAssertEquals(pt2, np.array([u, v])) self.assertEqual(r1, r)
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): def test_generic_factor(self):
"Evaluate residual using pose and point as state variables" """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() graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values() state = gtsam.Values()
measured = imgPoint measured = self.img_point
noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
poseKey = gtsam.symbol_shorthand.P(0) pose_key, point_key = P(0), L(0)
pointKey = gtsam.symbol_shorthand.L(0)
k = gtsam.Cal3Fisheye() k = gtsam.Cal3Fisheye()
state.insert_pose3(poseKey, gtsam.Pose3()) state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(pointKey, gtsam.Point3(objPoint)) state.insert_point3(point_key, self.obj_point)
factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k) factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k)
graph.add(factor) graph.add(factor)
score = graph.error(state) score = graph.error(state)
self.assertAlmostEqual(score, 0) self.assertAlmostEqual(score, 0)
def test_sfm_factor2(self): def test_sfm_factor2(self):
"Evaluate residual with camera, pose and point as state variables" """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() graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values() state = gtsam.Values()
measured = imgPoint measured = self.img_point
noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
cameraKey = gtsam.symbol_shorthand.K(0) camera_key, pose_key, landmark_key = K(0), P(0), L(0)
poseKey = gtsam.symbol_shorthand.P(0)
landmarkKey = gtsam.symbol_shorthand.L(0)
k = gtsam.Cal3Fisheye() k = gtsam.Cal3Fisheye()
state.insert_cal3fisheye(cameraKey, k) state.insert_cal3fisheye(camera_key, k)
state.insert_pose3(poseKey, gtsam.Pose3()) state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) state.insert_point3(landmark_key, gtsam.Point3(self.obj_point))
factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey) factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key)
graph.add(factor) graph.add(factor)
score = graph.error(state) score = graph.error(state)
self.assertAlmostEqual(score, 0) self.assertAlmostEqual(score, 0)
@ -93,12 +96,12 @@ class TestCal3Fisheye(GtsamTestCase):
def test_retract(self): def test_retract(self):
expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 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) 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) 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') 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) self.gtsamAssertEquals(actual, expected)
np.testing.assert_allclose(d, K.localCoordinates(actual)) np.testing.assert_allclose(d, k.localCoordinates(actual))
if __name__ == "__main__": if __name__ == "__main__":