Finished some re-factoring in Cal3DS2
dellaert
commit
7c9328414c
|
|
@ -25,7 +25,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Cal3DS2::Cal3DS2(const Vector &v):
|
||||
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
|
||||
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]){}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Cal3DS2::K() const {
|
||||
|
|
@ -34,32 +34,64 @@ Matrix Cal3DS2::K() const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Vector Cal3DS2::vector() const {
|
||||
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_);
|
||||
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3DS2::print(const std::string& s) const {
|
||||
gtsam::print(K(), s + ".K");
|
||||
gtsam::print(Vector(k()), s + ".k");
|
||||
void Cal3DS2::print(const std::string& s_) const {
|
||||
gtsam::print(K(), s_ + ".K");
|
||||
gtsam::print(Vector(k()), s_ + ".k");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
|
||||
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
|
||||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
|
||||
fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol)
|
||||
fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2::uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
|
||||
double yy, double xy, double rr, double r4, double pnx, double pny,
|
||||
const Eigen::Matrix<double, 2, 2>& DK) {
|
||||
Eigen::Matrix<double, 2, 5> DR1;
|
||||
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
|
||||
Eigen::Matrix<double, 2, 4> DR2;
|
||||
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
|
||||
y * rr, y * r4, rr + 2 * yy, 2 * xy;
|
||||
Eigen::Matrix<double, 2, 9> D;
|
||||
D << DR1, DK * DR2;
|
||||
return D;
|
||||
}
|
||||
|
||||
// parameters
|
||||
const double fx = fx_, fy = fy_, s = s_;
|
||||
const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
|
||||
/* ************************************************************************* */
|
||||
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
|
||||
double g, double k1, double k2, double p1, double p2,
|
||||
const Eigen::Matrix<double, 2, 2>& DK) {
|
||||
const double drdx = 2. * x;
|
||||
const double drdy = 2. * y;
|
||||
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
|
||||
const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
|
||||
|
||||
// Dx = 2*p1*xy + p2*(rr+2*xx);
|
||||
// Dy = 2*p2*xy + p1*(rr+2*yy);
|
||||
const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
|
||||
const double dDxdy = 2. * p1 * x + p2 * drdy;
|
||||
const double dDydx = 2. * p2 * y + p1 * drdx;
|
||||
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
|
||||
|
||||
Eigen::Matrix<double, 2, 2> DR;
|
||||
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
|
||||
y * dgdx + dDydx, g + y * dgdy + dDydy;
|
||||
|
||||
return DK * DR;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
|
||||
// rr = x^2 + y^2;
|
||||
// g = (1 + k(1)*rr + k(2)*rr^2);
|
||||
|
|
@ -68,40 +100,29 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
|
|||
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
|
||||
const double rr = xx + yy;
|
||||
const double r4 = rr * rr;
|
||||
const double g = 1. + k1 * rr + k2 * r4;
|
||||
const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx);
|
||||
const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy);
|
||||
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
|
||||
|
||||
// tangential component
|
||||
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
|
||||
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
|
||||
|
||||
// Radial and tangential distortion applied
|
||||
const double pnx = g * x + dx;
|
||||
const double pny = g * y + dy;
|
||||
|
||||
// Inlined derivative for calibration
|
||||
if (H1) {
|
||||
*H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
|
||||
fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
|
||||
fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
|
||||
fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
|
||||
}
|
||||
// Inlined derivative for points
|
||||
if (H2) {
|
||||
const double dr_dx = 2. * x;
|
||||
const double dr_dy = 2. * y;
|
||||
const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx;
|
||||
const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy;
|
||||
Eigen::Matrix<double, 2, 2> DK;
|
||||
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x);
|
||||
const double dDx_dy = 2. * k3 * x + k4 * dr_dy;
|
||||
const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
|
||||
const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
|
||||
// Derivative for calibration
|
||||
if (H1)
|
||||
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
||||
|
||||
Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy);
|
||||
Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
|
||||
y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
|
||||
// Derivative for points
|
||||
if (H2)
|
||||
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
|
||||
|
||||
*H2 = DK * DR;
|
||||
}
|
||||
|
||||
return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_);
|
||||
// Regular uncalibrate after distortion
|
||||
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -123,8 +144,8 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
|
|||
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
|
||||
const double rr = xx + yy;
|
||||
const double g = (1 + k1_ * rr + k2_ * rr * rr);
|
||||
const double dx = 2*k3_*xy + k4_*(rr+2*xx);
|
||||
const double dy = 2*k4_*xy + k3_*(rr+2*yy);
|
||||
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
|
||||
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
|
||||
pn = (invKPi - Point2(dx, dy)) / g;
|
||||
}
|
||||
|
||||
|
|
@ -136,30 +157,13 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
|
||||
//const double fx = fx_, fy = fy_, s = s_;
|
||||
const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
|
||||
//const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
|
||||
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
|
||||
const double rr = xx + yy;
|
||||
const double dr_dx = 2*x;
|
||||
const double dr_dy = 2*y;
|
||||
const double r4 = rr * rr;
|
||||
const double g = 1 + k1*rr + k2*r4;
|
||||
const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx;
|
||||
const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy;
|
||||
|
||||
// Dx = 2*k3*xy + k4*(rr+2*xx);
|
||||
// Dy = 2*k4*xy + k3*(rr+2*yy);
|
||||
const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x);
|
||||
const double dDx_dy = 2*k3*x + k4*dr_dy;
|
||||
const double dDy_dx = 2*k4*y + k3*dr_dx;
|
||||
const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y);
|
||||
|
||||
Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
|
||||
Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
|
||||
y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
|
||||
|
||||
return DK * DR;
|
||||
const double g = (1 + k1_ * rr + k2_ * r4);
|
||||
Eigen::Matrix<double, 2, 2> DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -167,16 +171,14 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
|
|||
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
|
||||
const double rr = xx + yy;
|
||||
const double r4 = rr * rr;
|
||||
const double fx = fx_, fy = fy_, s = s_;
|
||||
const double g = (1 + k1_ * rr + k2_ * r4);
|
||||
const double dx = 2*k3_*xy + k4_*(rr+2*xx);
|
||||
const double dy = 2*k4_*xy + k3_*(rr+2*yy);
|
||||
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
|
||||
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
|
||||
const double pnx = g * x + dx;
|
||||
const double pny = g * y + dy;
|
||||
|
||||
return (Matrix(2, 9) <<
|
||||
pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
|
||||
0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) );
|
||||
Eigen::Matrix<double, 2, 2> DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -27,6 +27,15 @@ namespace gtsam {
|
|||
* @brief Calibration of a camera with radial distortion
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*
|
||||
* Uses same distortionmodel as OpenCV, with
|
||||
* http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
|
||||
* but using only k1,k2,p1, and p2 coefficients.
|
||||
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
|
||||
* rr = Pn.x^2 + Pn.y^2
|
||||
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
|
||||
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||
* pi = K*pn
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
|
||||
|
||||
|
|
@ -34,28 +43,22 @@ protected:
|
|||
|
||||
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
|
||||
double k1_, k2_ ; // radial 2nd-order and 4th-order
|
||||
double k3_, k4_ ; // tangential distortion
|
||||
|
||||
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
|
||||
// rr = Pn.x^2 + Pn.y^2
|
||||
// \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
|
||||
// k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||
// pi = K*pn
|
||||
double p1_, p2_ ; // tangential distortion
|
||||
|
||||
public:
|
||||
Matrix K() const ;
|
||||
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); }
|
||||
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
|
||||
Vector vector() const ;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
|
||||
Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
|
||||
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double k3 = 0.0, double k4 = 0.0) :
|
||||
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {}
|
||||
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) {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
|
|
@ -92,18 +95,30 @@ public:
|
|||
/// image center in y
|
||||
inline double py() const { return v0_;}
|
||||
|
||||
/// First distortion coefficient
|
||||
inline double k1() const { return k1_;}
|
||||
|
||||
/// Second distortion coefficient
|
||||
inline double k2() const { return k2_;}
|
||||
|
||||
/// First tangential distortion coefficient
|
||||
inline double p1() const { return p1_;}
|
||||
|
||||
/// Second tangential distortion coefficient
|
||||
inline double p2() const { return p2_;}
|
||||
|
||||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv
|
||||
* convert intrinsic coordinates xy to (distorted) image coordinates uv
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
* @return point in (distorted) image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> Dcal = boost::none,
|
||||
boost::optional<Matrix&> Dp = boost::none) const ;
|
||||
|
||||
/// Conver a pixel coordinate to ideal coordinate
|
||||
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt intrinsic coordinates
|
||||
|
|
@ -148,8 +163,8 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(v0_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k3_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k4_);
|
||||
ar & BOOST_SERIALIZATION_NVP(p1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(p2_);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ void Cal3Unified::print(const std::string& s) const {
|
|||
bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
|
||||
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
|
||||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
|
||||
fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol ||
|
||||
fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol ||
|
||||
fabs(xi_ - K.xi_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -31,6 +31,14 @@ namespace gtsam {
|
|||
* @brief Calibration of a omni-directional camera with mirror + lens radial distortion
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*
|
||||
* Similar to Cal3DS2, does distortion but has additional mirror parameter xi
|
||||
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
|
||||
* Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
|
||||
* rr = Pn.x^2 + Pn.y^2
|
||||
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
|
||||
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||
* pi = K*pn
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
|
||||
|
||||
|
|
@ -41,13 +49,6 @@ private:
|
|||
|
||||
double xi_; // mirror parameter
|
||||
|
||||
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
|
||||
// Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
|
||||
// rr = Pn.x^2 + Pn.y^2
|
||||
// \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
|
||||
// k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||
// pi = K*pn
|
||||
|
||||
public:
|
||||
//Matrix K() const ;
|
||||
//Eigen::Vector4d k() const { return Base::k(); }
|
||||
|
|
@ -60,8 +61,8 @@ public:
|
|||
Cal3Unified() : Base(), xi_(0) {}
|
||||
|
||||
Cal3Unified(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) :
|
||||
Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {}
|
||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
|
||||
Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
|
|
|
|||
|
|
@ -60,6 +60,8 @@ TEST( Cal3DS2, Duncalibrate1)
|
|||
K.uncalibrate(p, computed, boost::none);
|
||||
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
|
||||
CHECK(assert_equal(numerical,computed,1e-5));
|
||||
Matrix separate = K.D2d_calibration(p);
|
||||
CHECK(assert_equal(numerical,separate,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -68,6 +70,8 @@ TEST( Cal3DS2, Duncalibrate2)
|
|||
Matrix computed; K.uncalibrate(p, boost::none, computed);
|
||||
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
|
||||
CHECK(assert_equal(numerical,computed,1e-5));
|
||||
Matrix separate = K.D2d_intrinsic(p);
|
||||
CHECK(assert_equal(numerical,separate,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue