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

Merge pull request #902 from roderick-koehle/Fix-Cal3Fisheye-Jacobian

release/4.3a0
Varun Agrawal 2021-11-28 09:50:07 -05:00 committed by GitHub
parent 58d4c05ff9 8846324b34
commit ad86b14b89
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 121 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -46,9 +46,9 @@ double Cal3Fisheye::Scaling(double r) {
/* ************************************************************************* */
Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
OptionalJacobian<2, 2> H2) const {
const double xi = p.x(), yi = p.y();
const double xi = p.x(), yi = p.y(), zi = 1;
const double r2 = xi * xi + yi * yi, r = sqrt(r2);
const double t = atan(r);
const double t = atan2(r, zi);
const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
Vector5 K, T;
K << 1, k1_, k2_, k3_, k4_;
@ -76,28 +76,32 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
// Derivative for points in intrinsic coords (2 by 2)
if (H2) {
const double dtd_dt =
1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
const double dt_dr = 1 / (1 + r2);
const double rinv = 1 / r;
const double dr_dxi = xi * rinv;
const double dr_dyi = yi * rinv;
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
if (r2==0) {
*H2 = DK;
} else {
const double dtd_dt =
1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
const double R2 = r2 + zi*zi;
const double dt_dr = zi / R2;
const double rinv = 1 / r;
const double dr_dxi = xi * rinv;
const double dr_dyi = yi * rinv;
const double dtd_dr = dtd_dt * dt_dr;
const double c2 = dr_dxi * dr_dxi;
const double s2 = dr_dyi * dr_dyi;
const double cs = dr_dxi * dr_dyi;
const double td = t * K.dot(T);
const double rrinv = 1 / r2;
const double dxd_dxi =
dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi;
const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
const double dyd_dyi =
dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
const double dxd_dxi = dtd_dr * c2 + s * (1 - c2);
const double dxd_dyi = (dtd_dr - s) * cs;
const double dyd_dxi = dxd_dyi;
const double dyd_dyi = dtd_dr * s2 + s * (1 - s2);
Matrix2 DR;
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
Matrix2 DR;
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
*H2 = DK * DR;
*H2 = DK * DR;
}
}
return uv;

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

@ -17,6 +17,15 @@ import gtsam
from gtsam.utils.test_case import GtsamTestCase
from gtsam.symbol_shorthand import K, L, P
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
@ -105,6 +114,71 @@ class TestCal3Fisheye(GtsamTestCase):
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()
camera_key, pose_key, landmark_key = K(0), 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"""

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

@ -117,6 +117,28 @@ class TestCal3Unified(GtsamTestCase):
score = graph.error(state)
self.assertAlmostEqual(score, 0)
def test_jacobian(self):
"""Evaluate jacobian at optical axis"""
obj_point_on_axis = np.array([0, 0, 1])
img_point = np.array([0.0, 0.0])
pose = gtsam.Pose3()
camera = gtsam.Cal3Unified()
state = gtsam.Values()
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
state.insert_cal3unified(camera_key, camera)
state.insert_point3(landmark_key, obj_point_on_axis)
state.insert_pose3(pose_key, pose)
g = gtsam.NonlinearFactorGraph()
noise_model = gtsam.noiseModel.Unit.Create(2)
factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key)
g.add(factor)
f = g.error(state)
gaussian_factor_graph = g.linearize(state)
H, z = gaussian_factor_graph.jacobian()
self.assertAlmostEqual(f, 0)
self.gtsamAssertEquals(z, np.zeros(2))
self.gtsamAssertEquals(H @ H.T, 4*np.eye(2))
@unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
def test_triangulation(self):
"""Estimate spatial point from image measurements"""