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

change: reduce some temporary computation

release/4.3a0
lvzhaoyang 2015-06-23 10:01:02 -04:00
parent b758eafad2
commit 93e1311e0d
2 changed files with 8 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/geometry/Cal3_S2.cpp
View File

@ -83,26 +83,20 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
}
/* ************************************************************************* */
Point2 Cal3_S2::calibrate(const Point2& p) const {
const double u = p.x(), v = p.y();
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
(1 / fy_) * (v - v0_));
}
/* ************************************************************************* */
Point2 Cal3_S2::calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal,
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
OptionalJacobian<2,2> Dp) 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_;
Point2 point(inv_fx * (delta_u - s_ * inv_fy * delta_v),
inv_fy * delta_v);
double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
inv_fy_delta_v);
if(Dcal)
*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,
*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 * point.y(), 0, 0, -inv_fy;
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 point;
}

9
gtsam/geometry/Cal3_S2.h
View File

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