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

Changed cacheing in jac func and fixed comments per PR discussion

release/4.3a0
Glen Haggin 2020-04-09 13:24:32 -04:00
parent a488888d40
commit 3ee552c7c6
3 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/FisheyeExample.cpp
View File

@ -10,11 +10,12 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file VisualBatchExample.cpp * @file FisheyeExample.cpp
* @brief A visualSLAM example for the structure-from-motion problem on a * @brief A visualSLAM example for the structure-from-motion problem on a
* simulated dataset. This version uses a fisheye camera model and a GaussNewton * simulated dataset. This version uses a fisheye camera model and a GaussNewton
* solver to solve the graph in one batch * solver to solve the graph in one batch
* @author ghaggin * @author ghaggin
* @Date Apr 9,2020
*/ */
/** /**

2
gtsam/geometry/Cal3Fisheye.h
View File

@ -11,7 +11,7 @@
/** /**
* @file Cal3Fisheye.h * @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 * @date Apr 8, 2020
* @author ghaggin * @author ghaggin
*/ */

11
gtsam/geometry/Cal3Fisheye_Base.cpp
View File

@ -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_dxi = dtd_dt * dt_dr * dr_dxi;
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
const double rr = r*r; const double rinv = 1/r;
const double dxd_dxi = dtd_dxi*xi*(1/r) + td*(1/r) + td*xi*(-1/rr)*dr_dxi; const double rrinv = 1/(r*r);
const double dxd_dyi = dtd_dyi*xi*(1/r) + td*xi*(-1/rr)*dr_dyi; const double dxd_dxi = dtd_dxi*xi*rinv + td*rinv + td*xi*(-rrinv)*dr_dxi;
const double dyd_dxi = dtd_dxi*yi*(1/r) + td*yi*(-1/rr)*dr_dxi; const double dxd_dyi = dtd_dyi*xi*rinv - td*xi*rrinv*dr_dyi;
const double dyd_dyi = dtd_dyi*yi*(1/r) + td*(1/r) + td*yi*(-1/rr)*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; Matrix2 DR;
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;