added calibrate with jacobians for Cal3Bundler
parent
a4c3623701
commit
921dc848a0
|
@ -121,6 +121,39 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
|
|||
return pn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
Point2 pn = calibrate(p, 1e-5);
|
||||
|
||||
// Approximate the jacobians via a single iteration of g.
|
||||
if (Dcal) {
|
||||
const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_;
|
||||
const double xx = x * x, yy = y * y;
|
||||
const double rr = xx + yy;
|
||||
const double g = (1 + k1_ * rr + k2_ * rr * rr);
|
||||
const double inv_f = 1 / f_, inv_g = 1 / g;
|
||||
|
||||
*Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y,
|
||||
-y * inv_g * rr, -y * inv_g * rr * rr;
|
||||
}
|
||||
if (Dp) {
|
||||
const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_;
|
||||
const double xx = x * x, yy = y * y;
|
||||
const double rr = xx + yy;
|
||||
const double g = (1 + k1_ * rr + k2_ * rr * rr);
|
||||
const double inv_f = 1 / f_, inv_g = 1 / g;
|
||||
|
||||
const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f);
|
||||
const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f);
|
||||
|
||||
*Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv,
|
||||
-pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv);
|
||||
}
|
||||
|
||||
return pn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
|
||||
Matrix2 Dp;
|
||||
|
|
|
@ -120,6 +120,16 @@ public:
|
|||
/// Convert a pixel coordinate to ideal coordinate
|
||||
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
|
||||
|
||||
/**
|
||||
* Convert image coordinates uv to intrinsic coordinates xy
|
||||
* @param p point in image coordinates
|
||||
* @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// @deprecated might be removed in next release, use uncalibrate
|
||||
Matrix2 D2d_intrinsic(const Point2& p) const;
|
||||
|
||||
|
|
|
@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
|
|||
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( PinholeCamera, Cal3Bundler) {
|
||||
Cal3Bundler calibration;
|
||||
Pose3 wTc;
|
||||
PinholeCamera<Cal3Bundler> camera(wTc, calibration);
|
||||
Point2 p(50, 100);
|
||||
camera.backproject(p, 10);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
Loading…
Reference in New Issue