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

Avoid division by zero in jacobian calculation

release/4.3a0
roderick-koehle 2021-10-22 19:39:09 +02:00 committed by GitHub
parent 1640f172e6
commit 8df2c70866
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -76,6 +76,9 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
// Derivative for points in intrinsic coords (2 by 2) // Derivative for points in intrinsic coords (2 by 2)
if (H2) { if (H2) {
if (r2==0) {
*H2 = DK;
} else {
const double dtd_dt = const double dtd_dt =
1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
const double dt_dr = 1 / (1 + r2); const double dt_dr = 1 / (1 + r2);
@ -99,6 +102,7 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
*H2 = DK * DR; *H2 = DK * DR;
} }
}
return uv; return uv;
} }