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
* 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
*/
/**

2
gtsam/geometry/Cal3Fisheye.h
View File

@ -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
*/

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_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;