diff --git a/examples/FisheyeExample.cpp b/examples/FisheyeExample.cpp index 2f79eb667..b03c9a71d 100644 --- a/examples/FisheyeExample.cpp +++ b/examples/FisheyeExample.cpp @@ -10,11 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file VisualBatchExample.cpp + * @file FisheyeExample.cpp * @brief A visualSLAM example for the structure-from-motion problem on a * simulated dataset. This version uses a fisheye camera model and a GaussNewton * solver to solve the graph in one batch * @author ghaggin + * @Date Apr 9,2020 */ /** diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index df2978350..8fd22cedc 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -11,7 +11,7 @@ /** * @file Cal3Fisheye.h - * @brief Calibration of a fisheye, calculations in base class Cal3Fisheye_Base + * @brief Calibration of a fisheye camera, calculations in base class Cal3Fisheye_Base * @date Apr 8, 2020 * @author ghaggin */ diff --git a/gtsam/geometry/Cal3Fisheye_Base.cpp b/gtsam/geometry/Cal3Fisheye_Base.cpp index 19f7d010b..4b51276df 100644 --- a/gtsam/geometry/Cal3Fisheye_Base.cpp +++ b/gtsam/geometry/Cal3Fisheye_Base.cpp @@ -81,11 +81,12 @@ static Matrix2 D2dintrinsic(const double xi, const double yi, const double r, co const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; - const double rr = r*r; - const double dxd_dxi = dtd_dxi*xi*(1/r) + td*(1/r) + td*xi*(-1/rr)*dr_dxi; - const double dxd_dyi = dtd_dyi*xi*(1/r) + td*xi*(-1/rr)*dr_dyi; - const double dyd_dxi = dtd_dxi*yi*(1/r) + td*yi*(-1/rr)*dr_dxi; - const double dyd_dyi = dtd_dyi*yi*(1/r) + td*(1/r) + td*yi*(-1/rr)*dr_dyi; + const double rinv = 1/r; + const double rrinv = 1/(r*r); + const double dxd_dxi = dtd_dxi*xi*rinv + td*rinv + td*xi*(-rrinv)*dr_dxi; + const double dxd_dyi = dtd_dyi*xi*rinv - td*xi*rrinv*dr_dyi; + const double dyd_dxi = dtd_dxi*yi*rinv - td*yi*rrinv*dr_dxi; + const double dyd_dyi = dtd_dyi*yi*rinv + td*rinv + td*yi*(-rrinv)*dr_dyi; Matrix2 DR; DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;