Frank Dellaert
commit
0a5690dfb3
|
|
@ -30,7 +30,7 @@ namespace gtsam {
|
|||
*/
|
||||
class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
|
||||
|
||||
private:
|
||||
protected:
|
||||
|
||||
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
|
||||
double k1_, k2_ ; // radial 2nd-order and 4th-order
|
||||
|
|
|
|||
|
|
@ -0,0 +1,141 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Cal3Unified.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Unified::Cal3Unified(const Vector &v):
|
||||
Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Cal3Unified::vector() const {
|
||||
return (Vector(10) << Base::vector(), xi_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3Unified::print(const std::string& s) const {
|
||||
Base::print(s);
|
||||
gtsam::print((Vector)(Vector(1) << xi_), s + ".xi");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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(xi_ - K.xi_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Unified::uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> 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_;
|
||||
|
||||
// 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.0 / (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: use Cal3DS2
|
||||
Point2 m(x,y);
|
||||
Matrix H1base, H2base; // jacobians from Base class
|
||||
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
|
||||
|
||||
// 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 = H2base * DU;
|
||||
|
||||
*H1 = collect(2, &H1base, &DDS2U);
|
||||
}
|
||||
// Inlined derivative for points
|
||||
if (H2) {
|
||||
// part1
|
||||
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
|
||||
const double mid = -(xi * xs*ys) * denom;
|
||||
Matrix DU = (Matrix(2, 2) <<
|
||||
(sqrt_nx + xi*(ys*ys + 1)) * denom, mid,
|
||||
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom);
|
||||
|
||||
*H2 = H2base * DU;
|
||||
}
|
||||
|
||||
return puncalib;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
|
||||
|
||||
// calibrate point to Nplane use base class::calibrate()
|
||||
Point2 pnplane = Base::calibrate(pi, tol);
|
||||
|
||||
// call nplane to space
|
||||
return this->nPlaneToSpace(pnplane);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Unified::nPlaneToSpace(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_)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Unified::spaceToNPlane(const Point2& p) const {
|
||||
|
||||
const double x = p.x(), y = p.y();
|
||||
const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);
|
||||
|
||||
return Point2((x / sq_xy), (y / sq_xy));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Unified Cal3Unified::retract(const Vector& d) const {
|
||||
return Cal3Unified(vector() + d);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,146 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief Calibration of a omni-directional camera with mirror + lens radial distortion
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
|
||||
|
||||
typedef Cal3Unified This;
|
||||
typedef Cal3DS2 Base;
|
||||
|
||||
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(); }
|
||||
Vector vector() const ;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default Constructor with only unit focal length
|
||||
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) {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
Cal3Unified(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 Cal3Unified& K, double tol = 10e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// mirror parameter
|
||||
inline double xi() const { return xi_;}
|
||||
|
||||
/**
|
||||
* 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<Matrix&> Dcal = boost::none,
|
||||
boost::optional<Matrix&> Dp = boost::none) const ;
|
||||
|
||||
/// Conver a pixel coordinate to ideal coordinate
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
|
||||
/// Convert a 3D point to normalized unit plane
|
||||
Point2 spaceToNPlane(const Point2& p) const;
|
||||
|
||||
/// Convert a normalized unit plane point to 3D space
|
||||
Point2 nPlaneToSpace(const Point2& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// Given delta vector, update calibration
|
||||
Cal3Unified retract(const Vector& d) const ;
|
||||
|
||||
/// Given a different calibration, calculate update to obtain it
|
||||
Vector localCoordinates(const Cal3Unified& 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<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version)
|
||||
{
|
||||
ar & boost::serialization::make_nvp("Cal3Unified",
|
||||
boost::serialization::base_object<Cal3DS2>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(xi_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testCal3Unify.cpp
|
||||
* @brief Unit tests for transform derivatives
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Cal3Unified.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unified)
|
||||
|
||||
/*
|
||||
ground truth from matlab, code :
|
||||
X = [0.5 0.7 1]';
|
||||
V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
|
||||
[P, J] = spaceToImgPlane(X, V);
|
||||
matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
|
||||
*/
|
||||
|
||||
static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1);
|
||||
static Point2 p(0.5, 0.7);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, uncalibrate)
|
||||
{
|
||||
Point2 p_i(364.7791831734982, 305.6677211952602) ;
|
||||
Point2 q = K.uncalibrate(p);
|
||||
CHECK(assert_equal(q,p_i));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, spaceNplane)
|
||||
{
|
||||
Point2 q = K.spaceToNPlane(p);
|
||||
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
|
||||
CHECK(assert_equal(p, K.nPlaneToSpace(q)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, calibrate)
|
||||
{
|
||||
Point2 pi = K.uncalibrate(p);
|
||||
Point2 pn_hat = K.calibrate(pi);
|
||||
CHECK( p.equals(pn_hat, 1e-8));
|
||||
}
|
||||
|
||||
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, Duncalibrate1)
|
||||
{
|
||||
Matrix computed;
|
||||
K.uncalibrate(p, computed, boost::none);
|
||||
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
|
||||
CHECK(assert_equal(numerical,computed,1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, Duncalibrate2)
|
||||
{
|
||||
Matrix computed;
|
||||
K.uncalibrate(p, boost::none, computed);
|
||||
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
|
||||
CHECK(assert_equal(numerical,computed,1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, assert_equal)
|
||||
{
|
||||
CHECK(assert_equal(K,K,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, retract)
|
||||
{
|
||||
Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
|
||||
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
|
||||
Vector d(10);
|
||||
d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
|
||||
Cal3Unified actual = K.retract(d);
|
||||
CHECK(assert_equal(expected,actual,1e-9));
|
||||
CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue