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

Undo change in scaling function

release/4.3a0
roderick-koehle 2021-10-28 13:27:05 +02:00 committed by GitHub
parent 0d01e4844f
commit 8c2ea78b1a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -32,7 +32,7 @@ Vector9 Cal3Fisheye::vector() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Cal3Fisheye::Scaling(double r, double zi) { double Cal3Fisheye::Scaling(double r) {
static constexpr double threshold = 1e-8; static constexpr double threshold = 1e-8;
if (r > threshold || r < -threshold) { if (r > threshold || r < -threshold) {
return atan(r) / r; return atan(r) / r;
@ -53,7 +53,7 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
Vector5 K, T; Vector5 K, T;
K << 1, k1_, k2_, k3_, k4_; K << 1, k1_, k2_, k3_, k4_;
T << 1, t2, t4, t6, t8; T << 1, t2, t4, t6, t8;
const double scaling = Scaling(r, zi); const double scaling = Scaling(r);
const double s = scaling * K.dot(T); const double s = scaling * K.dot(T);
const double xd = s * xi, yd = s * yi; const double xd = s * xi, yd = s * yi;
Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);