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

Fixed-size versions (copy/paste!)

release/4.3a0
dellaert 2014-10-22 12:48:15 +02:00
parent f44e6f0187
commit 19f0e3fc46
2 changed files with 58 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam/geometry/Cal3Bundler.cpp
View File

@ -64,6 +64,45 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
return true;
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
boost::optional<Matrix23&> Dcal, boost::optional<Matrix2&> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
// Derivatives make use of intermediate variables above
if (Dcal) {
double rx = r * x, ry = r * y;
*Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
}
if (Dp) {
const double a = 2. * (k1_ + 2. * k2_ * r);
const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
*Dp << g + axx, axy, axy, g + ayy;
*Dp *= f_;
}
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
boost::optional<Matrix&> Dcal, boost::optional<Matrix&> Dp) const {

21
gtsam/geometry/Cal3Bundler.h
View File

@ -106,12 +106,29 @@ public:
/**
* convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p) const;
/**
* Version of uncalibrate with fixed derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
boost::none, boost::optional<Matrix&> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, boost::optional<Matrix23&> Dcal,
boost::optional<Matrix2&> Dp) const;
/**
* Version of uncalibrate with dynamic derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;