move calculation of Cal3DS2 to base class, all unit tests passed and hopefully fix issue of DerivedValue
Jing Dong
parent
799beec7e2
commit
b4c62969d1
|
|
@ -23,24 +23,9 @@
|
||||||
|
|
||||||
namespace gtsam {
|
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]), p1_(v[7]), p2_(v[8]){}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Matrix Cal3DS2::K() const {
|
|
||||||
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Vector Cal3DS2::vector() const {
|
|
||||||
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void Cal3DS2::print(const std::string& s_) const {
|
void Cal3DS2::print(const std::string& s_) const {
|
||||||
gtsam::print(K(), s_ + ".K");
|
Base::print(s_);
|
||||||
gtsam::print(Vector(k()), s_ + ".k");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
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);
|
|
||||||
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
|
|
||||||
// pi(:,i) = g * pn(:,i) + dp;
|
|
||||||
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; // 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;
|
|
||||||
|
|
||||||
Eigen::Matrix<double, 2, 2> DK;
|
|
||||||
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
|
||||||
|
|
||||||
// Derivative for calibration
|
|
||||||
if (H1)
|
|
||||||
*H1 = 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);
|
|
||||||
|
|
||||||
// Regular uncalibrate after distortion
|
|
||||||
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) 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_)),
|
|
||||||
(1 / fy_) * (pi.y() - v0_));
|
|
||||||
|
|
||||||
// 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 (uncalibrate(pn).distance(pi) <= tol) break;
|
|
||||||
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 * p1_ * xy + p2_ * (rr + 2 * xx);
|
|
||||||
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
|
|
||||||
pn = (invKPi - Point2(dx, dy)) / g;
|
|
||||||
}
|
|
||||||
|
|
||||||
if ( iteration >= maxIterations )
|
|
||||||
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
|
|
||||||
|
|
||||||
return pn;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
|
|
||||||
const double x = p.x(), y = p.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);
|
|
||||||
Eigen::Matrix<double, 2, 2> DK;
|
|
||||||
DK << fx_, s_, 0.0, fy_;
|
|
||||||
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
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 g = (1 + k1_ * rr + k2_ * r4);
|
|
||||||
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;
|
|
||||||
Eigen::Matrix<double, 2, 2> DK;
|
|
||||||
DK << fx_, s_, 0.0, fy_;
|
|
||||||
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Cal3DS2 Cal3DS2::retract(const Vector& d) const {
|
Cal3DS2 Cal3DS2::retract(const Vector& d) const {
|
||||||
return Cal3DS2(vector() + d);
|
return Cal3DS2(vector() + d);
|
||||||
|
|
|
||||||
|
|
@ -19,7 +19,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/DerivedValue.h>
|
#include <gtsam/base/DerivedValue.h>
|
||||||
#include <gtsam/geometry/Point2.h>
|
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
|
@ -37,34 +37,27 @@ namespace gtsam {
|
||||||
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||||
* pi = K*pn
|
* pi = K*pn
|
||||||
*/
|
*/
|
||||||
class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
|
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> {
|
||||||
|
|
||||||
protected:
|
typedef Cal3DS2_Base Base;
|
||||||
|
|
||||||
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
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Matrix K() const ;
|
|
||||||
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
|
|
||||||
Vector vector() const ;
|
|
||||||
|
|
||||||
/// @name Standard Constructors
|
/// @name Standard Constructors
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// Default Constructor with only unit focal length
|
/// Default Constructor with only unit focal length
|
||||||
Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
|
Cal3DS2() : Base() {}
|
||||||
|
|
||||||
Cal3DS2(double fx, double fy, double s, double u0, double v0,
|
Cal3DS2(double fx, double fy, double s, double u0, double v0,
|
||||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
|
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) {}
|
Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Advanced Constructors
|
/// @name Advanced Constructors
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
Cal3DS2(const Vector &v) ;
|
Cal3DS2(const Vector &v) : Base(v) {}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Testable
|
/// @name Testable
|
||||||
|
|
@ -76,57 +69,6 @@ public:
|
||||||
/// assert equality up to a tolerance
|
/// assert equality up to a tolerance
|
||||||
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
|
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
|
||||||
|
|
||||||
/// @}
|
|
||||||
/// @name Standard Interface
|
|
||||||
/// @{
|
|
||||||
|
|
||||||
/// focal length x
|
|
||||||
inline double fx() const { return fx_;}
|
|
||||||
|
|
||||||
/// focal length x
|
|
||||||
inline double fy() const { return fy_;}
|
|
||||||
|
|
||||||
/// skew
|
|
||||||
inline double skew() const { return s_;}
|
|
||||||
|
|
||||||
/// image center in x
|
|
||||||
inline double px() const { return u0_;}
|
|
||||||
|
|
||||||
/// 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 (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 (distorted) image coordinates
|
|
||||||
*/
|
|
||||||
Point2 uncalibrate(const Point2& p,
|
|
||||||
boost::optional<Matrix&> Dcal = boost::none,
|
|
||||||
boost::optional<Matrix&> Dp = boost::none) const ;
|
|
||||||
|
|
||||||
/// 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
|
|
||||||
Matrix D2d_intrinsic(const Point2& p) const ;
|
|
||||||
|
|
||||||
/// Derivative of uncalibrate wrpt the calibration parameters
|
|
||||||
Matrix D2d_calibration(const Point2& p) const ;
|
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Manifold
|
/// @name Manifold
|
||||||
/// @{
|
/// @{
|
||||||
|
|
@ -156,18 +98,10 @@ private:
|
||||||
{
|
{
|
||||||
ar & boost::serialization::make_nvp("Cal3DS2",
|
ar & boost::serialization::make_nvp("Cal3DS2",
|
||||||
boost::serialization::base_object<Value>(*this));
|
boost::serialization::base_object<Value>(*this));
|
||||||
ar & BOOST_SERIALIZATION_NVP(fx_);
|
ar & boost::serialization::make_nvp("Cal3DS2",
|
||||||
ar & BOOST_SERIALIZATION_NVP(fy_);
|
boost::serialization::base_object<Cal3DS2_Base>(*this));
|
||||||
ar & BOOST_SERIALIZATION_NVP(s_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(u0_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(v0_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(k1_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(p1_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(p2_);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,187 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Cal3DS2_Base.cpp
|
||||||
|
* @date Feb 28, 2010
|
||||||
|
* @author ydjian
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/base/Vector.h>
|
||||||
|
#include <gtsam/base/Matrix.h>
|
||||||
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
#include <gtsam/geometry/Point3.h>
|
||||||
|
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||||
|
|
||||||
|
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]){}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix Cal3DS2_Base::K() const {
|
||||||
|
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Vector Cal3DS2_Base::vector() const {
|
||||||
|
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void Cal3DS2_Base::print(const std::string& s_) const {
|
||||||
|
gtsam::print(K(), s_ + ".K");
|
||||||
|
gtsam::print(Vector(k()), s_ + ".k");
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
bool Cal3DS2_Base::equals(const Cal3DS2_Base& 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(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
|
||||||
|
return false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
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_Base::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);
|
||||||
|
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
|
||||||
|
// pi(:,i) = g * pn(:,i) + dp;
|
||||||
|
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; // 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;
|
||||||
|
|
||||||
|
Eigen::Matrix<double, 2, 2> DK;
|
||||||
|
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||||
|
|
||||||
|
// Derivative for calibration
|
||||||
|
if (H1)
|
||||||
|
*H1 = 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);
|
||||||
|
|
||||||
|
// 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 {
|
||||||
|
// 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_)),
|
||||||
|
(1 / fy_) * (pi.y() - v0_));
|
||||||
|
|
||||||
|
// 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 (uncalibrate(pn).distance(pi) <= tol) break;
|
||||||
|
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 * p1_ * xy + p2_ * (rr + 2 * xx);
|
||||||
|
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
|
||||||
|
pn = (invKPi - Point2(dx, dy)) / g;
|
||||||
|
}
|
||||||
|
|
||||||
|
if ( iteration >= maxIterations )
|
||||||
|
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
|
||||||
|
|
||||||
|
return pn;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
|
||||||
|
const double x = p.x(), y = p.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);
|
||||||
|
Eigen::Matrix<double, 2, 2> DK;
|
||||||
|
DK << fx_, s_, 0.0, fy_;
|
||||||
|
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix Cal3DS2_Base::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 g = (1 + k1_ * rr + k2_ * r4);
|
||||||
|
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;
|
||||||
|
Eigen::Matrix<double, 2, 2> DK;
|
||||||
|
DK << fx_, s_, 0.0, fy_;
|
||||||
|
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,158 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Cal3DS2.h
|
||||||
|
* @brief Calibration of a camera with radial distortion
|
||||||
|
* @date Feb 28, 2010
|
||||||
|
* @author ydjian
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/base/DerivedValue.h>
|
||||||
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
|
||||||
|
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_Base {
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
public:
|
||||||
|
Matrix K() const ;
|
||||||
|
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_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
|
||||||
|
|
||||||
|
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) {}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Advanced Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
Cal3DS2_Base(const Vector &v) ;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// print with optional string
|
||||||
|
void print(const std::string& s = "") const ;
|
||||||
|
|
||||||
|
/// assert equality up to a tolerance
|
||||||
|
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Standard Interface
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// focal length x
|
||||||
|
inline double fx() const { return fx_;}
|
||||||
|
|
||||||
|
/// focal length x
|
||||||
|
inline double fy() const { return fy_;}
|
||||||
|
|
||||||
|
/// skew
|
||||||
|
inline double skew() const { return s_;}
|
||||||
|
|
||||||
|
/// image center in x
|
||||||
|
inline double px() const { return u0_;}
|
||||||
|
|
||||||
|
/// 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 (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 (distorted) image coordinates
|
||||||
|
*/
|
||||||
|
Point2 uncalibrate(const Point2& p,
|
||||||
|
boost::optional<Matrix&> Dcal = boost::none,
|
||||||
|
boost::optional<Matrix&> Dp = boost::none) const ;
|
||||||
|
|
||||||
|
/// 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
|
||||||
|
Matrix D2d_intrinsic(const Point2& p) const ;
|
||||||
|
|
||||||
|
/// Derivative of uncalibrate wrpt the calibration parameters
|
||||||
|
Matrix D2d_calibration(const Point2& p) const ;
|
||||||
|
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Advanced Interface
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/** Serialization function */
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template<class Archive>
|
||||||
|
void serialize(Archive & ar, const unsigned int version)
|
||||||
|
{
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(fx_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(fy_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(u0_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(v0_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(k1_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(p1_);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(p2_);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -22,8 +22,8 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/geometry/Cal3DS2.h>
|
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||||
#include <gtsam/geometry/Point2.h>
|
#include <gtsam/base/DerivedValue.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
|
@ -40,10 +40,10 @@ namespace gtsam {
|
||||||
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||||
* pi = K*pn
|
* pi = K*pn
|
||||||
*/
|
*/
|
||||||
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
|
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> {
|
||||||
|
|
||||||
typedef Cal3Unified This;
|
typedef Cal3Unified This;
|
||||||
typedef Cal3DS2 Base;
|
typedef Cal3DS2_Base Base;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
@ -135,7 +135,9 @@ private:
|
||||||
void serialize(Archive & ar, const unsigned int version)
|
void serialize(Archive & ar, const unsigned int version)
|
||||||
{
|
{
|
||||||
ar & boost::serialization::make_nvp("Cal3Unified",
|
ar & boost::serialization::make_nvp("Cal3Unified",
|
||||||
boost::serialization::base_object<Cal3DS2>(*this));
|
boost::serialization::base_object<Value>(*this));
|
||||||
|
ar & boost::serialization::make_nvp("Cal3Unified",
|
||||||
|
boost::serialization::base_object<Cal3DS2_Base>(*this));
|
||||||
ar & BOOST_SERIALIZATION_NVP(xi_);
|
ar & BOOST_SERIALIZATION_NVP(xi_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue