Name change from k3,k4 to p1,p2 as in OpenCV
dellaert
parent
0a7fd40b6c
commit
ea0dbf05ef
|
|
@ -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,20 +34,20 @@ 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;
|
||||
}
|
||||
|
|
@ -57,10 +57,6 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
|
|||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
|
||||
// parameters
|
||||
const double fx = fx_, fy = fy_, s = s_;
|
||||
const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
|
||||
|
||||
// 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)];
|
||||
|
|
@ -68,40 +64,44 @@ 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));
|
||||
*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;
|
||||
const double dg_dx = k1_ * dr_dx + k2_ * 2. * rr * dr_dx;
|
||||
const double dg_dy = k1_ * dr_dy + k2_ * 2. * rr * dr_dy;
|
||||
|
||||
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);
|
||||
const double dDx_dx = 2. * p1_ * y + p2_ * (dr_dx + 4. * x);
|
||||
const double dDx_dy = 2. * p1_ * x + p2_ * dr_dy;
|
||||
const double dDy_dx = 2. * p2_ * y + p1_ * dr_dx;
|
||||
const double dDy_dy = 2. * p2_ * x + p1_ * (dr_dy + 4. * y);
|
||||
|
||||
Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy);
|
||||
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);
|
||||
|
||||
*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 +123,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,24 +136,21 @@ 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;
|
||||
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);
|
||||
// Dx = 2*p1_*xy + p2_*(rr+2*xx);
|
||||
// Dy = 2*p2_*xy + p1_*(rr+2*yy);
|
||||
const double dDx_dx = 2*p1_*y + p2_*(dr_dx + 4*x);
|
||||
const double dDx_dy = 2*p1_*x + p2_*dr_dy;
|
||||
const double dDy_dx = 2*p2_*y + p1_*dr_dx;
|
||||
const double dDy_dy = 2*p2_*x + p1_*(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,
|
||||
|
|
@ -167,16 +164,15 @@ 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) );
|
||||
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) );
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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_;}
|
||||
|
||||
/// image center in x
|
||||
inline double k1() const { return k1_;}
|
||||
|
||||
/// image center in y
|
||||
inline double k2() const { return k2_;}
|
||||
|
||||
/// image center in x
|
||||
inline double p1() const { return p1_;}
|
||||
|
||||
/// image center in y
|
||||
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue