From 1e76082888d316503174e12738b76c47ceb11f3e Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 9 Mar 2014 00:40:02 -0500 Subject: [PATCH] add unified projection model: a calibration class for omni-directional camera. Not test yet --- gtsam/geometry/Cal3Unify.cpp | 201 +++++++++++++++++++++++++++++++++++ gtsam/geometry/Cal3Unify.h | 168 +++++++++++++++++++++++++++++ 2 files changed, 369 insertions(+) create mode 100644 gtsam/geometry/Cal3Unify.cpp create mode 100644 gtsam/geometry/Cal3Unify.h diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp new file mode 100644 index 000000000..d49417492 --- /dev/null +++ b/gtsam/geometry/Cal3Unify.cpp @@ -0,0 +1,201 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unify.cpp + * @date Mar 8, 2014 + * @author Jing Dong + */ + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Unify::Cal3Unify(const Vector &v): + xi_(v[0]), fx_(v[1]), fy_(v[2]), s_(v[3]), u0_(v[4]), v0_(v[5]), k1_(v[6]), k2_(v[7]), k3_(v[8]), k4_(v[9]){} + +/* ************************************************************************* */ +Matrix Cal3Unify::K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); +} + +/* ************************************************************************* */ +Vector Cal3Unify::vector() const { + return (Vector(10) << xi_, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); +} + +/* ************************************************************************* */ +void Cal3Unify::print(const std::string& s) const { + gtsam::print(K(), s + ".K"); + gtsam::print(Vector(k()), s + ".k"); + gtsam::print(Vector(xi_), s + ".xi"); +} + +/* ************************************************************************* */ +bool Cal3Unify::equals(const Cal3Unify& 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(xi_ - K.xi_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3Unify::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + // this part of code is modified from Cal3DS2, + // since the second part of this model (after project to normalized plane) + // is same as Cal3DS2 + + // parameters + const double xi = xi_, fx = fx_, fy = fy_, s = s_; + const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; + + // Part1: project 3D space to NPlane + const double xs = p.x(), ys = p.y(); // normalized points in 3D space + const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); + const double xi_sqrt_nx = 1 + xi * sqrt_nx; + const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; + const double x = xs / xi_sqrt_nx, y = ys / xi_sqrt_nx; // points on NPlane + + // Part2: project NPlane point to pixel plane: same as Cal3DS2 + const double 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 pnx = g*x + dx; + const double pny = g*y + dy; + + // DDS2 will be used both in H1 and H2 + Matrix DDS2; + if (H1 || H2) { + // part2 + 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 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); + + DDS2 = DK * DR; + } + + // Inlined derivative for calibration + if (H1) { + // part1 + Matrix DU = (Matrix(2,1) << xs * sqrt_nx / xi_sqrt_nx2, + ys * sqrt_nx / xi_sqrt_nx2); + Matrix DDS2U = DDS2 * DU; + // part2 + Matrix DDS2V = (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 = collect(2, &DDS2U, &DDS2V); + } + // Inlined derivative for points + if (H2) { + // part1 + Matrix DU = (Matrix(2, 2) << (xi_sqrt_nx - xs * xs / sqrt_nx) / xi_sqrt_nx2, + -(ys * ys / (sqrt_nx * xi_sqrt_nx2)), + -(xs * xs / (sqrt_nx * xi_sqrt_nx2)), + (xi_sqrt_nx - ys * ys / sqrt_nx) / xi_sqrt_nx2); + + *H2 = DDS2 * DU; + } + + + return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3Unify::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}) + + // point on the normalized plane, input for DS2 + Point2 pnpl = this->imageToNPlane(pi); + double px = pnpl.x(); + double py = pnpl.y(); + const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)), + (1 / fy_) * (py - 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; + + // part1: image -> normalized plane + pnpl = this->imageToNPlane(pn); + // part2: normalized plane -> 3D space + px = pnpl.x(), py = pnpl.y(); + const double 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*k3_*xy + k4_*(rr+2*xx); + const double dy = 2*k4_*xy + k3_*(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; +} +/* ************************************************************************* */ +Point2 Cal3Unify::imageToNPlane(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double xy2 = x * x + y * y; + const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1); + + return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_))); +} + +/* ************************************************************************* */ +Cal3Unify Cal3Unify::retract(const Vector& d) const { + return Cal3Unify(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Unify::localCoordinates(const Cal3Unify& T2) const { + return T2.vector() - vector(); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unify.h new file mode 100644 index 000000000..58f19ece5 --- /dev/null +++ b/gtsam/geometry/Cal3Unify.h @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unify.h + * @brief Unified Calibration Model, see Mei07icra for details + * @date Mar 8, 2014 + * @author Jing Dong + */ + +/** + * @addtogroup geometry + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Calibration of a omni-directional camera with mirror + lens radial distortion + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3Unify : public DerivedValue { + +private: + + double xi_; // mirror parameter + 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 ] + // 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 Eigen::Vector4d(k1_, k2_, k3_, k4_); } + Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Unify() : xi_(0), fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + + Cal3Unify(double xi, double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3 = 0.0, double k4 = 0.0) : + xi_(xi), fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Unify(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 Cal3Unify& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// mirror parameter + inline double xi() const { return xi_;} + + /// focal length x + inline double fx() const { return fx_;} + + /// focal length y + 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_;} + + /** + * convert intrinsic coordinates xy to 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 + */ + Point2 uncalibrate(const Point2& p, + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + /// Conver a pixel coordinate to ideal coordinate + Point2 calibrate(const Point2& p, const double tol=1e-5) const; + + /// Convert a image point to normalized unit plane + Point2 imageToNPlane(const Point2& p) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Unify retract(const Vector& d) const ; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Unify& T2) const ; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + + /// Return dimensions of calibration manifold object + static size_t Dim() { return 10; } //TODO: make a final dimension variable + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & boost::serialization::make_nvp("Cal3Unify", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(xi_); + 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(k3_); + ar & BOOST_SERIALIZATION_NVP(k4_); + } + + + /// @} + +}; + +} +