Combine Cal3Fisheye and Cal3Fisheye_Base into one class
Glen Haggin
parent
3ee552c7c6
commit
5bb0bbdcc4
|
|
@ -15,24 +15,189 @@
|
|||
* @author ghaggin
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/geometry/Cal3Fisheye.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Cal3Fisheye.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Fisheye::Cal3Fisheye(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]) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector9 Cal3Fisheye::vector() const {
|
||||
Vector9 v;
|
||||
v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_;
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Cal3Fisheye::K() const {
|
||||
Matrix3 K;
|
||||
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
|
||||
return K;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix29 D2dcalibration(const double xd, const double yd,
|
||||
const double xi, const double yi,
|
||||
const double t3, const double t5,
|
||||
const double t7, const double t9, const double r,
|
||||
Matrix2& DK) {
|
||||
// order: fx, fy, s, u0, v0
|
||||
Matrix25 DR1;
|
||||
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
|
||||
|
||||
// order: k1, k2, k3, k4
|
||||
Matrix24 DR2;
|
||||
DR2 << t3 * xi, t5 * xi, t7 * xi, t9 * xi, t3 * yi, t5 * yi, t7 * yi, t9 * yi;
|
||||
DR2 /= r;
|
||||
Matrix29 D;
|
||||
D << DR1, DK * DR2;
|
||||
return D;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix2 D2dintrinsic(const double xi, const double yi, const double r,
|
||||
const double td, const double t, const double tt,
|
||||
const double t4, const double t6, const double t8,
|
||||
const double k1, const double k2, const double k3,
|
||||
const double k4, const Matrix2& DK) {
|
||||
const double dr_dxi = xi / sqrt(xi * xi + yi * yi);
|
||||
const double dr_dyi = yi / sqrt(xi * xi + yi * yi);
|
||||
const double dt_dr = 1 / (1 + r * r);
|
||||
const double dtd_dt =
|
||||
1 + 3 * k1 * tt + 5 * k2 * t4 + 7 * k3 * t6 + 9 * k4 * t8;
|
||||
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
|
||||
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
|
||||
|
||||
const double rinv = 1 / r;
|
||||
const double rrinv = 1 / (r * r);
|
||||
const double dxd_dxi =
|
||||
dtd_dxi * xi * rinv + td * rinv + td * xi * (-rrinv) * dr_dxi;
|
||||
const double dxd_dyi = dtd_dyi * xi * rinv - td * xi * rrinv * dr_dyi;
|
||||
const double dyd_dxi = dtd_dxi * yi * rinv - td * yi * rrinv * dr_dxi;
|
||||
const double dyd_dyi =
|
||||
dtd_dyi * yi * rinv + td * rinv + td * yi * (-rrinv) * dr_dyi;
|
||||
|
||||
Matrix2 DR;
|
||||
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
|
||||
|
||||
return DK * DR;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
|
||||
OptionalJacobian<2, 2> H2) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
|
||||
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||
const double xd = td / r * xi, yd = td / r * yi;
|
||||
Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
|
||||
|
||||
Matrix2 DK;
|
||||
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
// Derivative for calibration parameters (2 by 9)
|
||||
if (H1)
|
||||
*H1 = D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
|
||||
|
||||
// Derivative for points in intrinsic coords (2 by 2)
|
||||
if (H2)
|
||||
*H2 =
|
||||
D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||
|
||||
return uv;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
|
||||
// initial gues just inverts the pinhole model
|
||||
const double u = uv.x(), v = uv.y();
|
||||
const double yd = (v - v0_) / fy_;
|
||||
const double xd = (u - s_ * yd - u0_) / fx_;
|
||||
Point2 pi(xd, yd);
|
||||
|
||||
// Perform newtons method, break when solution converges past tol,
|
||||
// throw exception if max iterations are reached
|
||||
const int maxIterations = 10;
|
||||
int iteration;
|
||||
for (iteration = 0; iteration < maxIterations; ++iteration) {
|
||||
Matrix2 jac;
|
||||
|
||||
// Calculate the current estimate (uv_hat) and the jacobian
|
||||
const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
|
||||
|
||||
// Test convergence
|
||||
if ((uv_hat - uv).norm() < tol) break;
|
||||
|
||||
// Newton's method update step
|
||||
pi = pi - jac.inverse() * (uv_hat - uv);
|
||||
}
|
||||
|
||||
if (iteration >= maxIterations)
|
||||
throw std::runtime_error(
|
||||
"Cal3Fisheye::calibrate fails to converge. need a better "
|
||||
"initialization");
|
||||
|
||||
return pi;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix2 Cal3Fisheye::D2d_intrinsic(const Point2& p) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t * t, t4 = tt * tt, t6 = t4 * tt, t8 = t4 * t4;
|
||||
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||
|
||||
Matrix2 DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix29 Cal3Fisheye::D2d_calibration(const Point2& p) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
|
||||
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||
const double xd = td / r * xi, yd = td / r * yi;
|
||||
|
||||
Matrix2 DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
return D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3Fisheye::print(const std::string& s_) const {
|
||||
Base::print(s_);
|
||||
gtsam::print((Matrix)K(), s_ + ".K");
|
||||
gtsam::print(Vector(k()), s_ + ".k");
|
||||
;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
|
||||
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
|
||||
std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
|
||||
std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol || std::abs(k4_ - K.k4_) > tol)
|
||||
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
|
||||
std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
|
||||
std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
|
||||
std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
|
||||
std::abs(k4_ - K.k4_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
|
@ -47,7 +212,5 @@ Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
|
|||
return T2.vector() - vector();
|
||||
}
|
||||
|
||||
}
|
||||
} // namespace gtsam
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -11,41 +11,66 @@
|
|||
|
||||
/**
|
||||
* @file Cal3Fisheye.h
|
||||
* @brief Calibration of a fisheye camera, calculations in base class Cal3Fisheye_Base
|
||||
* @brief Calibration of a fisheye camera
|
||||
* @date Apr 8, 2020
|
||||
* @author ghaggin
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3Fisheye_Base.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief Calibration of a fisheye camera that also supports
|
||||
* Lie-group behaviors for optimization.
|
||||
* \sa Cal3Fisheye_Base
|
||||
* @brief Calibration of a fisheye camera
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*
|
||||
* Uses same distortionmodel as OpenCV, with
|
||||
* https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html
|
||||
* 3D point in camera frame
|
||||
* p = (x, y, z)
|
||||
* Intrinsic coordinates:
|
||||
* [x_i;y_i] = [x/z; y/z]
|
||||
* Distorted coordinates:
|
||||
* r^2 = (x_i)^2 + (y_i)^2
|
||||
* th = atan(r)
|
||||
* th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
|
||||
* [x_d; y_d] = (th_d / r)*[x_i; y_i]
|
||||
* Pixel coordinates:
|
||||
* K = [fx s u0; 0 fy v0 ;0 0 1]
|
||||
* [u; v; 1] = K*[x_d; y_d; 1]
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Fisheye : public Cal3Fisheye_Base {
|
||||
|
||||
typedef Cal3Fisheye_Base Base;
|
||||
|
||||
public:
|
||||
class GTSAM_EXPORT Cal3Fisheye {
|
||||
protected:
|
||||
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
|
||||
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
|
||||
|
||||
public:
|
||||
enum { dimension = 9 };
|
||||
typedef boost::shared_ptr<Cal3Fisheye>
|
||||
shared_ptr; ///< shared pointer to fisheye calibration object
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3Fisheye() : Base() {}
|
||||
Cal3Fisheye()
|
||||
: fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
|
||||
|
||||
Cal3Fisheye(const double fx, const double fy, const double s, const double u0, const double v0,
|
||||
const double k1, const double k2, const double k3, const double k4) :
|
||||
Base(fx, fy, s, u0, v0, k1, k2, k3, k4) {}
|
||||
Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
|
||||
const double v0, const double k1, const double k2,
|
||||
const double k3, const double k4)
|
||||
: fx_(fx),
|
||||
fy_(fy),
|
||||
s_(s),
|
||||
u0_(u0),
|
||||
v0_(v0),
|
||||
k1_(k1),
|
||||
k2_(k2),
|
||||
k3_(k3),
|
||||
k4_(k4) {}
|
||||
|
||||
virtual ~Cal3Fisheye() {}
|
||||
|
||||
|
|
@ -53,14 +78,77 @@ public:
|
|||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
Cal3Fisheye(const Vector &v) : Base(v) {}
|
||||
Cal3Fisheye(const Vector& v);
|
||||
|
||||
/// @}
|
||||
/// @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 k3() const { return k3_; }
|
||||
|
||||
/// Second tangential distortion coefficient
|
||||
inline double k4() const { return k4_; }
|
||||
|
||||
/// return calibration matrix
|
||||
Matrix3 K() const;
|
||||
|
||||
/// return distortion parameter vector
|
||||
Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
|
||||
|
||||
/// Return all parameters as a vector
|
||||
Vector9 vector() const;
|
||||
|
||||
/**
|
||||
* @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
|
||||
* coordinates [u; v]
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
|
||||
* k4)
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
|
||||
* @return point in (distorted) image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
|
||||
/// y_i]
|
||||
Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i]
|
||||
Matrix2 D2d_intrinsic(const Point2& p) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt the calibration parameters
|
||||
/// [fx, fy, s, u0, v0, k1, k2, k3, k4]
|
||||
Matrix29 D2d_calibration(const Point2& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
virtual void print(const std::string& s = "") const ;
|
||||
virtual void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
|
||||
|
|
@ -70,52 +158,54 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Given delta vector, update calibration
|
||||
Cal3Fisheye retract(const Vector& d) const ;
|
||||
Cal3Fisheye retract(const Vector& d) const;
|
||||
|
||||
/// Given a different calibration, calculate update to obtain it
|
||||
Vector localCoordinates(const Cal3Fisheye& T2) const ;
|
||||
Vector localCoordinates(const Cal3Fisheye& T2) const;
|
||||
|
||||
/// Return dimensions of calibration manifold object
|
||||
virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
|
||||
virtual size_t dim() const { return 9; }
|
||||
|
||||
/// Return dimensions of calibration manifold object
|
||||
static size_t Dim() { return 9; } //TODO: make a final dimension variable
|
||||
static size_t Dim() { return 9; }
|
||||
|
||||
/// @}
|
||||
/// @name Clone
|
||||
/// @{
|
||||
|
||||
/// @return a deep copy of this object
|
||||
virtual boost::shared_ptr<Base> clone() const {
|
||||
return boost::shared_ptr<Base>(new Cal3Fisheye(*this));
|
||||
virtual boost::shared_ptr<Cal3Fisheye> clone() const {
|
||||
return boost::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
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::make_nvp("Cal3Fisheye",
|
||||
boost::serialization::base_object<Cal3Fisheye_Base>(*this));
|
||||
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(k3_);
|
||||
ar& BOOST_SERIALIZATION_NVP(k4_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
|
||||
|
||||
template<>
|
||||
template <>
|
||||
struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
|
||||
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -1,185 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Cal3Fisheye_Base.cpp
|
||||
* @date Apr 8, 2020
|
||||
* @author ghaggin
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Cal3Fisheye_Base.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Fisheye_Base::Cal3Fisheye_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]), k3_(v[7]), k4_(v[8]){}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Cal3Fisheye_Base::K() const {
|
||||
Matrix3 K;
|
||||
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
|
||||
return K;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector9 Cal3Fisheye_Base::vector() const {
|
||||
Vector9 v;
|
||||
v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_;
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3Fisheye_Base::print(const std::string& s_) const {
|
||||
gtsam::print((Matrix)K(), s_ + ".K");
|
||||
gtsam::print(Vector(k()), s_ + ".k");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3Fisheye_Base::equals(const Cal3Fisheye_Base& K, double tol) const {
|
||||
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
|
||||
std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
|
||||
std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol || std::abs(k4_ - K.k4_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix29 D2dcalibration(const double xd, const double yd, const double xi, const double yi, const double t3, const double t5, const double t7, const double t9, const double r, Matrix2& DK) {
|
||||
// order: fx, fy, s, u0, v0
|
||||
Matrix25 DR1;
|
||||
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
|
||||
|
||||
//order: k1, k2, k3, k4
|
||||
Matrix24 DR2;
|
||||
DR2 << t3*xi, t5*xi, t7*xi, t9*xi, t3*yi, t5*yi, t7*yi, t9*yi;
|
||||
DR2 /= r;
|
||||
Matrix29 D;
|
||||
D << DR1, DK * DR2;
|
||||
return D;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix2 D2dintrinsic(const double xi, const double yi, const double r, const double td, const double t, const double tt, const double t4, const double t6, const double t8, const double k1, const double k2, const double k3, const double k4, const Matrix2& DK) {
|
||||
|
||||
const double dr_dxi = xi/sqrt(xi*xi + yi*yi);
|
||||
const double dr_dyi = yi/sqrt(xi*xi + yi*yi);
|
||||
const double dt_dr = 1/(1 + r*r);
|
||||
const double dtd_dt = 1 + 3*k1*tt + 5*k2*t4 + 7*k3*t6 + 9*k4*t8;
|
||||
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
|
||||
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
|
||||
|
||||
const double rinv = 1/r;
|
||||
const double rrinv = 1/(r*r);
|
||||
const double dxd_dxi = dtd_dxi*xi*rinv + td*rinv + td*xi*(-rrinv)*dr_dxi;
|
||||
const double dxd_dyi = dtd_dyi*xi*rinv - td*xi*rrinv*dr_dyi;
|
||||
const double dyd_dxi = dtd_dxi*yi*rinv - td*yi*rrinv*dr_dxi;
|
||||
const double dyd_dyi = dtd_dyi*yi*rinv + td*rinv + td*yi*(-rrinv)*dr_dyi;
|
||||
|
||||
Matrix2 DR;
|
||||
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
|
||||
|
||||
return DK * DR;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Fisheye_Base::uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
|
||||
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi*xi + yi*yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t*t, t4 = tt*tt, t6 = tt*t4, t8 = t4*t4;
|
||||
const double td = t*(1 + k1_*tt + k2_*t4 + k3_*t6 + k4_*t8);
|
||||
const double xd = td/r*xi, yd = td/r*yi;
|
||||
Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
|
||||
|
||||
Matrix2 DK;
|
||||
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
// Derivative for calibration parameters (2 by 9)
|
||||
if (H1)
|
||||
*H1 = D2dcalibration(xd, yd, xi, yi, t*tt, t*t4, t*t6, t*t8, r, DK);
|
||||
|
||||
// Derivative for points in intrinsic coords (2 by 2)
|
||||
if (H2)
|
||||
*H2 = D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||
|
||||
return uv;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Fisheye_Base::calibrate(const Point2& uv, const double tol) const {
|
||||
// initial gues just inverts the pinhole model
|
||||
const double u = uv.x(), v = uv.y();
|
||||
const double yd = (v - v0_)/fy_;
|
||||
const double xd = (u - s_*yd - u0_)/fx_;
|
||||
Point2 pi(xd, yd);
|
||||
|
||||
// Perform newtons method, break when solution converges past tol,
|
||||
// throw exception if max iterations are reached
|
||||
const int maxIterations = 10;
|
||||
int iteration;
|
||||
for (iteration = 0; iteration < maxIterations; ++iteration) {
|
||||
Matrix2 jac;
|
||||
|
||||
// Calculate the current estimate (uv_hat) and the jacobian
|
||||
const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
|
||||
|
||||
// Test convergence
|
||||
if((uv_hat - uv).norm() < tol) break;
|
||||
|
||||
// Newton's method update step
|
||||
pi = pi - jac.inverse()*(uv_hat - uv);
|
||||
}
|
||||
|
||||
if ( iteration >= maxIterations )
|
||||
throw std::runtime_error("Cal3Fisheye::calibrate fails to converge. need a better initialization");
|
||||
|
||||
return pi;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix2 Cal3Fisheye_Base::D2d_intrinsic(const Point2& p) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi*xi + yi*yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t*t, t4 = tt*tt, t6 = t4*tt, t8 = t4*t4;
|
||||
const double td = t*(1 + k1_*tt + k2_*t4 + k3_*t6 + k4_*t8);
|
||||
|
||||
Matrix2 DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix29 Cal3Fisheye_Base::D2d_calibration(const Point2& p) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi*xi + yi*yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t*t, t4 = tt*tt, t6 = tt*t4, t8 = t4*t4;
|
||||
const double td = t*(1 + k1_*tt + k2_*t4 + k3_*t6 + k4_*t8);
|
||||
const double xd = td/r*xi, yd = td/r*yi;
|
||||
|
||||
Matrix2 DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
return D2dcalibration(xd, yd, xi, yi, t*tt, t*t4, t*t6, t*t8, r, DK);
|
||||
}
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
@ -1,180 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Cal3Fisheye_Base.h
|
||||
* @brief Calibration of a fisheye camera.
|
||||
* @date Apr 8, 2020
|
||||
* @author ghaggin
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief Calibration of a fisheye camera
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*
|
||||
* Uses same distortionmodel as OpenCV, with
|
||||
* https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html
|
||||
* 3D point in camera frame
|
||||
* p = (x, y, z)
|
||||
* Intrinsic coordinates:
|
||||
* [x_i;y_i] = [x/z; y/z]
|
||||
* Distorted coordinates:
|
||||
* r^2 = (x_i)^2 + (y_i)^2
|
||||
* th = atan(r)
|
||||
* th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
|
||||
* [x_d; y_d] = (th_d / r)*[x_i; y_i]
|
||||
* Pixel coordinates:
|
||||
* K = [fx s u0; 0 fy v0 ;0 0 1]
|
||||
* [u; v; 1] = K*[x_d; y_d; 1]
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Fisheye_Base {
|
||||
|
||||
protected:
|
||||
|
||||
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
|
||||
double k1_, k2_, k3_, k4_ ; // fisheye distortion coefficients
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3Fisheye_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
|
||||
|
||||
Cal3Fisheye_Base(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double k3, double k4) :
|
||||
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {}
|
||||
|
||||
virtual ~Cal3Fisheye_Base() {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
Cal3Fisheye_Base(const Vector &v) ;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
virtual void print(const std::string& s = "") const ;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3Fisheye_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 k3() const { return k3_;}
|
||||
|
||||
/// Second tangential distortion coefficient
|
||||
inline double k4() const { return k4_;}
|
||||
|
||||
/// return calibration matrix
|
||||
Matrix3 K() const;
|
||||
|
||||
/// return distortion parameter vector
|
||||
Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
|
||||
|
||||
/// Return all parameters as a vector
|
||||
Vector9 vector() const;
|
||||
|
||||
/**
|
||||
* convert intrinsic coordinates [x_i; y_i] to (distorted) image coordinates [u; v]
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ..., k4)
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
|
||||
* @return point in (distorted) image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,9> Dcal = boost::none,
|
||||
OptionalJacobian<2,2> Dp = boost::none) const ;
|
||||
|
||||
/// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, y_i]
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i]
|
||||
Matrix2 D2d_intrinsic(const Point2& p) const ;
|
||||
|
||||
/// Derivative of uncalibrate wrpt the calibration parameters
|
||||
/// [fx, fy, s, u0, v0, k1, k2, k3, k4]
|
||||
Matrix29 D2d_calibration(const Point2& p) const ;
|
||||
|
||||
/// @}
|
||||
/// @name Clone
|
||||
/// @{
|
||||
|
||||
/// @return a deep copy of this object
|
||||
virtual boost::shared_ptr<Cal3Fisheye_Base> clone() const {
|
||||
return boost::shared_ptr<Cal3Fisheye_Base>(new Cal3Fisheye_Base(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
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(k3_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k4_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue