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

change: simplifies the equation

release/4.3a0
lvzhaoyang 2015-06-23 03:16:32 -04:00
parent 89dc6399e2
commit b758eafad2
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Cal3_S2.cpp
View File

@ -95,15 +95,15 @@ Point2 Cal3_S2::calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal,
const double u = p.x(), v = p.y(); const double u = p.x(), v = p.y();
double delta_u = u - u0_, delta_v = v - v0_; double delta_u = u - u0_, delta_v = v - v0_;
double inv_fx = 1/ fx_, inv_fy = 1/fy_; double inv_fx = 1/ fx_, inv_fy = 1/fy_;
Point2 point(inv_fx * (delta_u - s_ * inv_fy * delta_v),
inv_fy * delta_v);
if(Dcal) if(Dcal)
*Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) , *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy * delta_v, -inv_fx * point.y(),
inv_fx * (s_ * inv_fy * inv_fy) * delta_v, -inv_fx * inv_fy * delta_v,
-inv_fx, inv_fx * s_ * inv_fy, -inv_fx, inv_fx * s_ * inv_fy,
0, -inv_fy * inv_fy * delta_v, 0, 0, -inv_fy; 0, -inv_fy * point.y(), 0, 0, -inv_fy;
if(Dp) if(Dp)
*Dp << inv_fx, -inv_fx * s_ * inv_fy, 0, inv_fy; *Dp << inv_fx, -inv_fx * s_ * inv_fy, 0, inv_fy;
return Point2(inv_fx * (delta_u - s_ * inv_fy * delta_v), return point;
inv_fy * delta_v);
} }
/* ************************************************************************* */ /* ************************************************************************* */