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();
double delta_u = u - u0_, delta_v = v - v0_;
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)
*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,
*Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy * delta_v, -inv_fx * point.y(),
-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)
*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);
return point;
}
/* ************************************************************************* */