Jacobians for Camera models
- Add jacobians for calibrate function using implicit function theorem - Consistent naming of jacobian parameters - Added tests for jacobians - Some simple formatting - Fixed docs for implicit function theorem - Added parentheses to conform with Google stylerelease/4.3a0
Varun Agrawal
parent
e90bfdbf93
commit
e1c3314e48
|
|
@ -124,8 +124,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
|
|||
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
|
||||
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
|
||||
// df/pi = -I (pn and pi are independent args)
|
||||
// Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
|
||||
// Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
|
||||
// Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
|
||||
// Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
|
||||
Matrix23 H_uncal_K;
|
||||
Matrix22 H_uncal_pn, H_uncal_pn_inv;
|
||||
|
||||
|
|
|
|||
|
|
@ -44,8 +44,8 @@ public:
|
|||
Cal3DS2() : Base() {}
|
||||
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
|
||||
Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
|
||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) :
|
||||
Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
|
||||
|
||||
virtual ~Cal3DS2() {}
|
||||
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
* @file Cal3DS2_Base.cpp
|
||||
* @date Feb 28, 2010
|
||||
* @author ydjian
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
|
@ -24,8 +25,17 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
|
||||
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
|
||||
Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
|
||||
: fx_(v(0)),
|
||||
fy_(v(1)),
|
||||
s_(v(2)),
|
||||
u0_(v(3)),
|
||||
v0_(v(4)),
|
||||
k1_(v(5)),
|
||||
k2_(v(6)),
|
||||
p1_(v(7)),
|
||||
p2_(v(8)),
|
||||
tol_(1e-5) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Cal3DS2_Base::K() const {
|
||||
|
|
@ -94,9 +104,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
|
||||
|
||||
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// rr = x^2 + y^2;
|
||||
// g = (1 + k(1)*rr + k(2)*rr^2);
|
||||
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
|
||||
|
|
@ -115,37 +124,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
|
|||
const double pny = g * y + dy;
|
||||
|
||||
Matrix2 DK;
|
||||
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||
if (Dcal || Dp) {
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
}
|
||||
|
||||
// Derivative for calibration
|
||||
if (H1)
|
||||
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
||||
if (Dcal) {
|
||||
*Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
||||
}
|
||||
|
||||
// Derivative for points
|
||||
if (H2)
|
||||
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
|
||||
if (Dp) {
|
||||
*Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
|
||||
}
|
||||
|
||||
// Regular uncalibrate after distortion
|
||||
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
|
||||
Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// Use the following fixed point iteration to invert the radial distortion.
|
||||
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
|
||||
|
||||
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
|
||||
const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
|
||||
(1 / fy_) * (pi.y() - v0_));
|
||||
|
||||
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
|
||||
// initialize by ignoring the distortion at all, might be problematic for
|
||||
// pixels around boundary
|
||||
Point2 pn = invKPi;
|
||||
|
||||
// iterate until the uncalibrate is close to the actual pixel coordinate
|
||||
const int maxIterations = 10;
|
||||
int iteration;
|
||||
for (iteration = 0; iteration < maxIterations; ++iteration) {
|
||||
if (distance2(uncalibrate(pn), pi) <= tol) break;
|
||||
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
|
||||
if (distance2(uncalibrate(pn), pi) <= tol_) break;
|
||||
const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px,
|
||||
yy = py * py;
|
||||
const double rr = xx + yy;
|
||||
const double g = (1 + k1_ * rr + k2_ * rr * rr);
|
||||
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
|
||||
|
|
@ -153,8 +169,28 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
|
|||
pn = (invKPi - Point2(dx, dy)) / g;
|
||||
}
|
||||
|
||||
if ( iteration >= maxIterations )
|
||||
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
|
||||
if (iteration >= maxIterations)
|
||||
throw std::runtime_error(
|
||||
"Cal3DS2::calibrate fails to converge. need a better initialization");
|
||||
|
||||
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
|
||||
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
|
||||
// df/pi = -I (pn and pi are independent args)
|
||||
// Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
|
||||
// Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
|
||||
Matrix29 H_uncal_K;
|
||||
Matrix22 H_uncal_pn, H_uncal_pn_inv;
|
||||
|
||||
if (Dcal || Dp) {
|
||||
// Compute uncalibrate Jacobians
|
||||
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
|
||||
|
||||
H_uncal_pn_inv = H_uncal_pn.inverse();
|
||||
|
||||
if (Dp) *Dp = H_uncal_pn_inv;
|
||||
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
|
||||
|
||||
}
|
||||
|
||||
return pn;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
* @brief Calibration of a camera with radial distortion
|
||||
* @date Feb 28, 2010
|
||||
* @author ydjian
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -43,6 +44,7 @@ protected:
|
|||
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
|
||||
double k1_, k2_ ; // radial 2nd-order and 4th-order
|
||||
double p1_, p2_ ; // tangential distortion
|
||||
double tol_; // tolerance value when calibrating
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -50,11 +52,30 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
|
||||
Cal3DS2_Base()
|
||||
: fx_(1),
|
||||
fy_(1),
|
||||
s_(0),
|
||||
u0_(0),
|
||||
v0_(0),
|
||||
k1_(0),
|
||||
k2_(0),
|
||||
p1_(0),
|
||||
p2_(0),
|
||||
tol_(1e-5) {}
|
||||
|
||||
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
|
||||
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
|
||||
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
|
||||
double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
|
||||
: fx_(fx),
|
||||
fy_(fy),
|
||||
s_(s),
|
||||
u0_(u0),
|
||||
v0_(v0),
|
||||
k1_(k1),
|
||||
k2_(k2),
|
||||
p1_(p1),
|
||||
p2_(p2),
|
||||
tol_(tol) {}
|
||||
|
||||
virtual ~Cal3DS2_Base() {}
|
||||
|
||||
|
|
@ -72,7 +93,7 @@ public:
|
|||
virtual void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
|
||||
bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -121,12 +142,12 @@ public:
|
|||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in (distorted) image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,9> Dcal = boost::none,
|
||||
OptionalJacobian<2,2> Dp = boost::none) const ;
|
||||
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt intrinsic coordinates
|
||||
Matrix2 D2d_intrinsic(const Point2& p) const ;
|
||||
|
|
@ -164,6 +185,7 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(p1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(p2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(tol_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
* @file Cal3Unified.cpp
|
||||
* @date Mar 8, 2014
|
||||
* @author Jing Dong
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
|
@ -54,8 +55,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
|
|||
/* ************************************************************************* */
|
||||
// todo: make a fixed sized jacobian version of this
|
||||
Point2 Cal3Unified::uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,10> H1,
|
||||
OptionalJacobian<2,2> H2) const {
|
||||
OptionalJacobian<2,10> Dcal,
|
||||
OptionalJacobian<2,2> Dp) const {
|
||||
|
||||
// this part of code is modified from Cal3DS2,
|
||||
// since the second part of this model (after project to normalized plane)
|
||||
|
|
@ -78,16 +79,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
|
|||
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
|
||||
|
||||
// Inlined derivative for calibration
|
||||
if (H1) {
|
||||
if (Dcal) {
|
||||
// part1
|
||||
Vector2 DU;
|
||||
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
|
||||
-ys * sqrt_nx * xi_sqrt_nx2;
|
||||
*H1 << H1base, H2base * DU;
|
||||
*Dcal << H1base, H2base * DU;
|
||||
}
|
||||
|
||||
// Inlined derivative for points
|
||||
if (H2) {
|
||||
if (Dp) {
|
||||
// part1
|
||||
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
|
||||
const double mid = -(xi * xs*ys) * denom;
|
||||
|
|
@ -95,20 +96,41 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
|
|||
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
|
||||
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
|
||||
|
||||
*H2 << H2base * DU;
|
||||
*Dp << H2base * DU;
|
||||
}
|
||||
|
||||
return puncalib;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
|
||||
|
||||
Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// calibrate point to Nplane use base class::calibrate()
|
||||
Point2 pnplane = Base::calibrate(pi, tol);
|
||||
Point2 pnplane = Base::calibrate(pi);
|
||||
|
||||
// call nplane to space
|
||||
return this->nPlaneToSpace(pnplane);
|
||||
Point2 pn = this->nPlaneToSpace(pnplane);
|
||||
|
||||
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
|
||||
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
|
||||
// df/pi = -I (pn and pi are independent args)
|
||||
// Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
|
||||
// Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
|
||||
Eigen::Matrix<double, 2, 10> H_uncal_K;
|
||||
Matrix22 H_uncal_pn, H_uncal_pn_inv;
|
||||
|
||||
if (Dcal || Dp) {
|
||||
// Compute uncalibrate Jacobians
|
||||
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
|
||||
|
||||
H_uncal_pn_inv = H_uncal_pn.inverse();
|
||||
|
||||
if (Dp) *Dp = H_uncal_pn_inv;
|
||||
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
|
||||
|
||||
}
|
||||
|
||||
return pn;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
* @brief Unified Calibration Model, see Mei07icra for details
|
||||
* @date Mar 8, 2014
|
||||
* @author Jing Dong
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
/**
|
||||
|
|
@ -99,7 +100,8 @@ public:
|
|||
OptionalJacobian<2,2> Dp = boost::none) const ;
|
||||
|
||||
/// Conver a pixel coordinate to ideal coordinate
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Convert a 3D point to normalized unit plane
|
||||
Point2 spaceToNPlane(const Point2& p) const;
|
||||
|
|
|
|||
|
|
@ -74,12 +74,26 @@ TEST( Cal3DS2, Duncalibrate2)
|
|||
CHECK(assert_equal(numerical,separate,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3DS2, assert_equal)
|
||||
{
|
||||
CHECK(assert_equal(K,K,1e-5));
|
||||
Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
|
||||
return k.calibrate(pt);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3DS2, Dcalibrate)
|
||||
{
|
||||
Point2 pn(0.5, 0.5);
|
||||
Point2 pi = K.uncalibrate(pn);
|
||||
Matrix Dcal, Dp;
|
||||
K.calibrate(pi, Dcal, Dp);
|
||||
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi, 1e-7);
|
||||
CHECK(assert_equal(numerical1, Dcal, 1e-5));
|
||||
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi, 1e-7);
|
||||
CHECK(assert_equal(numerical2, Dp, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3DS2, retract)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -82,6 +82,22 @@ TEST( Cal3Unified, Duncalibrate2)
|
|||
CHECK(assert_equal(numerical,computed,1e-6));
|
||||
}
|
||||
|
||||
Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
|
||||
return k.calibrate(pt);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, Dcalibrate)
|
||||
{
|
||||
Point2 pi = K.uncalibrate(p);
|
||||
Matrix Dcal, Dp;
|
||||
K.calibrate(pi, Dcal, Dp);
|
||||
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
|
||||
CHECK(assert_equal(numerical1,Dcal,1e-5));
|
||||
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
|
||||
CHECK(assert_equal(numerical2,Dp,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, assert_equal)
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue