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

feature: add jacobian to Cal3_D2 calibrate()

release/4.3a0
lvzhaoyang 2015-06-23 01:46:50 -04:00
parent b7b7182148
commit f7bf418c45
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/geometry/Cal3_S2.cpp
View File

@ -89,6 +89,24 @@ Point2 Cal3_S2::calibrate(const Point2& p) const {
(1 / fy_) * (v - v0_));
}
/* ************************************************************************* */
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const {
const double u = p.x(), v = p.y();
double delta_u = u - u0_, delta_v = v - v0_;
double inv_fx = 1/ fx_, inv_fy = 1/fy_;
if(Dcal)
*Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v)
<< inv_fx * (s_ * inv_fy * inv_fy) * delta_v << -inv_fx * inv_fy * delta_v
<< -inv_fx << inv_fx * s_ * inv_fy
<< 0 << -inv_fy * inv_fy * delta_v << 0 << 0 << -inv_fy;
if(Dp)
*Dp << inv_fx << -inv_fx * s_ * inv_fy << 0 << inv_fy;
return Point2(inv_fx * (delta_u - s_ * inv_fy * delta_v),
inv_fy * delta_v);
}
/* ************************************************************************* */
Vector3 Cal3_S2::calibrate(const Vector3& p) const {
return matrix_inverse() * p;

10
gtsam/geometry/Cal3_S2.h
View File

@ -160,6 +160,16 @@ public:
*/
Point2 calibrate(const Point2& p) const;
/**
* convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert homogeneous image coordinates to intrinsic coordinates
* @param p point in image coordinates