add more calibrations and a general camera type
Kai Ni
parent
3a978d6930
commit
e4673e2cd5
|
|
@ -0,0 +1,140 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* Cal3Bundler.cpp
|
||||
*
|
||||
* Created on: Sep 25, 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/Cal3Bundler.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {}
|
||||
Cal3Bundler::Cal3Bundler(const Vector &v) : f_(v(0)), k1_(v(1)), k2_(v(2)) {}
|
||||
Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : f_(f), k1_(k1), k2_(k2) {}
|
||||
|
||||
Matrix Cal3Bundler::K() const { return Matrix_(3,3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); }
|
||||
Vector Cal3Bundler::k() const { return Vector_(4, k1_, k2_, 0, 0) ; }
|
||||
Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; }
|
||||
void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; }
|
||||
|
||||
bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
|
||||
if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol)
|
||||
return false;
|
||||
return true ;
|
||||
}
|
||||
|
||||
Point2 Cal3Bundler::uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
// r = x^2 + y^2 ;
|
||||
// g = (1 + k(1)*r + k(2)*r^2) ;
|
||||
// pi(:,i) = g * pn(:,i)
|
||||
const double x = p.x(), y = p.y() ;
|
||||
const double r = x*x + y*y ;
|
||||
const double r2 = r*r ;
|
||||
const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply
|
||||
const double fg = f_*g ;
|
||||
|
||||
// semantic meaningful version
|
||||
//if (H1) *H1 = D2d_calibration(p) ;
|
||||
//if (H2) *H2 = D2d_intrinsic(p) ;
|
||||
|
||||
// unrolled version, much faster
|
||||
if ( H1 || H2 ) {
|
||||
|
||||
const double fx = f_*x, fy = f_*y ;
|
||||
if ( H1 ) {
|
||||
*H1 = Matrix_(2, 3,
|
||||
g*x, fx*r , fx*r2,
|
||||
g*y, fy*r , fy*r2) ;
|
||||
}
|
||||
|
||||
if ( H2 ) {
|
||||
const double dr_dx = 2*x ;
|
||||
const double dr_dy = 2*y ;
|
||||
const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ;
|
||||
const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ;
|
||||
*H2 = Matrix_(2, 2,
|
||||
fg + fx*dg_dx, fx*dg_dy,
|
||||
fy*dg_dx , fg + fy*dg_dy) ;
|
||||
}
|
||||
}
|
||||
|
||||
return Point2(fg * x, fg * y) ;
|
||||
}
|
||||
|
||||
Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
|
||||
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
|
||||
const double r = xx + yy ;
|
||||
const double dr_dx = 2*x ;
|
||||
const double dr_dy = 2*y ;
|
||||
const double g = 1 + (k1_+k2_*r) * r ; // save one multiply
|
||||
//const double g = 1 + k1_*r + k2_*r*r ;
|
||||
const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ;
|
||||
const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ;
|
||||
|
||||
Matrix DK = Matrix_(2, 2, f_, 0.0, 0.0, f_);
|
||||
Matrix DR = Matrix_(2, 2,
|
||||
g + x*dg_dx, x*dg_dy,
|
||||
y*dg_dx , g + y*dg_dy) ;
|
||||
return prod(DK,DR) ;
|
||||
}
|
||||
|
||||
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
|
||||
|
||||
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ;
|
||||
const double r = xx + yy ;
|
||||
const double r2 = r*r ;
|
||||
const double f = f_ ;
|
||||
const double g = 1 + (k1_+k2_*r) * r ; // save one multiply
|
||||
//const double g = (1+k1_*r+k2_*r2) ;
|
||||
|
||||
return Matrix_(2, 3,
|
||||
g*x, f*x*r , f*x*r2,
|
||||
g*y, f*y*r , f*y*r2);
|
||||
}
|
||||
|
||||
Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
|
||||
|
||||
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
|
||||
const double r = xx + yy ;
|
||||
const double r2 = r*r;
|
||||
const double dr_dx = 2*x ;
|
||||
const double dr_dy = 2*y ;
|
||||
const double g = 1 + (k1_*r) + (k2_*r2) ;
|
||||
const double f = f_ ;
|
||||
const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ;
|
||||
const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ;
|
||||
|
||||
Matrix H = Matrix_(2,5,
|
||||
f*(g + x*dg_dx), f*(x*dg_dy), g*x, f*x*r , f*x*r2,
|
||||
f*(y*dg_dx) , f*(g + y*dg_dy), g*y, f*y*r , f*y*r2);
|
||||
|
||||
return H ;
|
||||
}
|
||||
|
||||
|
||||
Cal3Bundler Cal3Bundler::expmap(const Vector& d) const { return Cal3Bundler(vector() + d) ; }
|
||||
Vector Cal3Bundler::logmap(const Cal3Bundler& T2) const { return vector() - T2.vector(); }
|
||||
Cal3Bundler Cal3Bundler::Expmap(const Vector& v) { return Cal3Bundler(v) ; }
|
||||
Vector Cal3Bundler::Logmap(const Cal3Bundler& p) { return p.vector(); }
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* Cal3Bundler.h
|
||||
*
|
||||
* Created on: Sep 25, 2010
|
||||
* Author: ydjian
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Cal3Bundler: public Testable<Cal3Bundler> {
|
||||
|
||||
private:
|
||||
double f_, k1_, k2_ ;
|
||||
|
||||
public:
|
||||
|
||||
Cal3Bundler() ;
|
||||
Cal3Bundler(const Vector &v) ;
|
||||
Cal3Bundler(const double f, const double k1, const double k2) ;
|
||||
Matrix K() const ;
|
||||
Vector k() const ;
|
||||
|
||||
Vector vector() const;
|
||||
void print(const std::string& s = "") const;
|
||||
bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
|
||||
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const ;
|
||||
|
||||
Matrix D2d_intrinsic(const Point2& p) const ;
|
||||
Matrix D2d_calibration(const Point2& p) const ;
|
||||
Matrix D2d_intrinsic_calibration(const Point2& p) const ;
|
||||
|
||||
Cal3Bundler expmap(const Vector& d) const ;
|
||||
Vector logmap(const Cal3Bundler& T2) const ;
|
||||
|
||||
static Cal3Bundler Expmap(const Vector& v) ;
|
||||
static Vector Logmap(const Cal3Bundler& p) ;
|
||||
|
||||
int dim() const { return 3 ; }
|
||||
static size_t Dim() { return 3; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version)
|
||||
{
|
||||
ar & BOOST_SERIALIZATION_NVP(f_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* Cal3DS2.cpp
|
||||
*
|
||||
* Created on: 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.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
Cal3DS2::Cal3DS2():fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0){}
|
||||
|
||||
// Construction
|
||||
Cal3DS2::Cal3DS2(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) {}
|
||||
|
||||
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]){}
|
||||
|
||||
Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
|
||||
Vector Cal3DS2::k() const { return Vector_(4, k1_, k2_, k3_, k4_); }
|
||||
Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_) ; }
|
||||
void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K") ; gtsam::print(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)
|
||||
return false ;
|
||||
return true ;
|
||||
}
|
||||
|
||||
Point2 Cal3DS2::uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
// r = x^2 + y^2 ;
|
||||
// g = (1 + k(1)*r + k(2)*r^2) ;
|
||||
// dp = [2*k(3)*x*y + k(4)*(r + 2*x^2) ; 2*k(4)*x*y + k(3)*(r + 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 r = xx + yy ;
|
||||
const double g = (1+k1_*r+k2_*r*r) ;
|
||||
const double dx = 2*k3_*xy + k4_*(r+2*xx) ;
|
||||
const double dy = 2*k4_*xy + k3_*(r+2*yy) ;
|
||||
|
||||
const double x2 = g*x + dx ;
|
||||
const double y2 = g*y + dy ;
|
||||
|
||||
if (H1) *H1 = D2d_calibration(p) ;
|
||||
if (H2) *H2 = D2d_intrinsic(p) ;
|
||||
|
||||
return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ;
|
||||
}
|
||||
|
||||
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 r = xx + yy ;
|
||||
const double dr_dx = 2*x ;
|
||||
const double dr_dy = 2*y ;
|
||||
const double r2 = r*r ;
|
||||
const double g = 1 + k1*r + k2*r2 ;
|
||||
const double dg_dx = k1*dr_dx + k2*2*r*dr_dx ;
|
||||
const double dg_dy = k1*dr_dy + k2*2*r*dr_dy ;
|
||||
|
||||
// Dx = 2*k3*xy + k4*(r+2*xx) ;
|
||||
// Dy = 2*k4*xy + k3*(r+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) ;
|
||||
|
||||
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) ;
|
||||
|
||||
return prod(DK,DR) ;
|
||||
}
|
||||
|
||||
|
||||
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 r = xx + yy ;
|
||||
const double r2 = r*r ;
|
||||
const double fx = fx_, fy = fy_, s = s_ ;
|
||||
const double g = (1+k1_*r+k2_*r2) ;
|
||||
|
||||
return Matrix_(2, 9,
|
||||
g*x, 0.0, g*y, 1.0, 0.0, fx*x*r + s*y*r, fx*x*r2 + s*y*r2, fx*2*xy + s*(r+2*yy), fx*(r+2*xx) + s*(2*xy),
|
||||
0.0, g*y, 0.0, 0.0, 1.0, fy*y*r , fy*y*r2 , fy*(r+2*yy) , fy*(2*xy) );
|
||||
}
|
||||
|
||||
Cal3DS2 Cal3DS2::expmap(const Vector& d) const { return Cal3DS2(vector() + d) ; }
|
||||
Vector Cal3DS2::logmap(const Cal3DS2& T2) const { return vector() - T2.vector(); }
|
||||
Cal3DS2 Cal3DS2::Expmap(const Vector& v) { return Cal3DS2(v) ; }
|
||||
Vector Cal3DS2::Logmap(const Cal3DS2& p) { return p.vector(); }
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* Cal3DS2.h
|
||||
*
|
||||
* Created on: Feb 28, 2010
|
||||
* Author: ydjian
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
#ifndef CAL3DS2_H_
|
||||
#define CAL3DS2_H_
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** This class a camera with radial distortion */
|
||||
class Cal3DS2 : Testable<Cal3DS2> {
|
||||
private:
|
||||
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
|
||||
double k1_, k2_ ; // radial 2nd-order and 4th-order
|
||||
double k3_, k4_ ; // tagential distortion
|
||||
|
||||
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
|
||||
// r = Pn.x^2 + Pn.y^2
|
||||
// \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ;
|
||||
// k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||
|
||||
public:
|
||||
// Default Constructor with only unit focal length
|
||||
Cal3DS2();
|
||||
|
||||
// Construction
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double k3, double k4) ;
|
||||
|
||||
Cal3DS2(const Vector &v) ;
|
||||
|
||||
Matrix K() const ;
|
||||
Vector k() const ;
|
||||
Vector vector() const ;
|
||||
void print(const std::string& s = "") const ;
|
||||
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
|
||||
|
||||
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const ;
|
||||
|
||||
Matrix D2d_intrinsic(const Point2& p) const ;
|
||||
Matrix D2d_calibration(const Point2& p) const ;
|
||||
|
||||
Cal3DS2 expmap(const Vector& d) const ;
|
||||
Vector logmap(const Cal3DS2& T2) const ;
|
||||
|
||||
static Cal3DS2 Expmap(const Vector& v) ;
|
||||
static Vector Logmap(const Cal3DS2& p) ;
|
||||
|
||||
int dim() const { return 9 ; }
|
||||
static size_t Dim() { return 9; }
|
||||
|
||||
private:
|
||||
/** 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_);
|
||||
}
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
#endif /* CAL3DS2_H_ */
|
||||
|
|
@ -0,0 +1,210 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* DistortedCameraT.h
|
||||
*
|
||||
* Created on: Mar 1, 2010
|
||||
* Author: ydjian
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef GENERALCAMERAT_H_
|
||||
#define GENERALCAMERAT_H_
|
||||
|
||||
#include <boost/numeric/ublas/vector_proxy.hpp>
|
||||
#include <boost/numeric/ublas/matrix_proxy.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
|
||||
|
||||
#include <gtsam/sfm/Cal3Dummy.h>
|
||||
#include <gtsam/sfm/Cal3Bundler.h>
|
||||
#include <gtsam/sfm/Cal3DS2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template <typename Camera, typename Calibration>
|
||||
class GeneralCameraT {
|
||||
|
||||
private:
|
||||
Camera calibrated_; // Calibrated camera
|
||||
Calibration calibration_; // Calibration
|
||||
|
||||
public:
|
||||
GeneralCameraT(){}
|
||||
GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {}
|
||||
GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {}
|
||||
GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {}
|
||||
GeneralCameraT(const Pose3& pose) : calibrated_(pose) {}
|
||||
GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {}
|
||||
|
||||
// Vector Initialization
|
||||
GeneralCameraT(const Vector &v) :
|
||||
calibrated_(subrange(v, 0, Camera::Dim())),
|
||||
calibration_(subrange(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {}
|
||||
|
||||
inline const Pose3& pose() const { return calibrated_.pose(); }
|
||||
inline const Camera& calibrated() const { return calibrated_; }
|
||||
inline const Calibration& calibration() const { return calibration_; }
|
||||
|
||||
std::pair<Point2,bool> projectSafe(
|
||||
const Point3& P,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
|
||||
Point3 cameraPoint = calibrated_.pose().transform_to(P);
|
||||
return std::pair<Point2, bool>(
|
||||
project(P,H1,H2),
|
||||
cameraPoint.z() > 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* project a 3d point to the camera
|
||||
* P is point in camera coordinate
|
||||
* H1 is respect to pose + calibration
|
||||
* H2 is respect to landmark
|
||||
*/
|
||||
Point2 project(const Point3& P,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const {
|
||||
|
||||
if (!H1 && !H2) {
|
||||
Point2 intrinsic = calibrated_.project(P);
|
||||
return calibration_.uncalibrate(intrinsic) ;
|
||||
}
|
||||
|
||||
Matrix I1, I2, J1, J2 ;
|
||||
Point2 intrinsic = calibrated_.project(P, I1, I2);
|
||||
Point2 projection = calibration_.uncalibrate(intrinsic, J1, J2);
|
||||
|
||||
if ( H1 ) { Matrix T = prod(J2,I1); *H1 = collect(2, &T, &J1) ; }
|
||||
if ( H2 ) *H2 = prod(J2, I2) ;
|
||||
|
||||
return projection;
|
||||
}
|
||||
|
||||
// dump functions for compilation for now
|
||||
bool equals (const GeneralCameraT &camera, double tol = 1e-9) const {
|
||||
return calibrated_.equals(camera.calibrated_, tol) &&
|
||||
calibration_.equals(camera.calibration_, tol) ;
|
||||
}
|
||||
|
||||
void print(const std::string& s = "") const {
|
||||
calibrated_.pose().print(s + ".camera.") ;
|
||||
calibration_.print(s + ".calibration.") ;
|
||||
}
|
||||
|
||||
GeneralCameraT expmap(const Vector &v) const {
|
||||
return GeneralCameraT(
|
||||
calibrated_.expmap(subrange(v,0,Camera::Dim())),
|
||||
calibration_.expmap(subrange(v,Camera::Dim(),Camera::Dim()+Calibration::Dim())));
|
||||
}
|
||||
|
||||
Vector logmap(const GeneralCameraT &C) const {
|
||||
//std::cout << "logmap" << std::endl;
|
||||
const Vector v1(calibrated().logmap(C.calibrated())),
|
||||
v2(calibration().logmap(C.calibration()));
|
||||
return concatVectors(2,&v1,&v2) ;
|
||||
}
|
||||
|
||||
static GeneralCameraT Expmap(const Vector& v) {
|
||||
//std::cout << "Expmap" << std::endl;
|
||||
return GeneralCameraT(
|
||||
Camera::Expmap(subrange(v,0,Camera::Dim())),
|
||||
Calibration::Expmap(subrange(v,Camera::Dim(), Camera::Dim()+Calibration::Dim()))
|
||||
);
|
||||
}
|
||||
|
||||
static Vector Logmap(const GeneralCameraT& p) {
|
||||
//std::cout << "Logmap" << std::endl;
|
||||
const Vector v1(Camera::Logmap(p.calibrated())),
|
||||
v2(Calibration::Logmap(p.calibration()));
|
||||
return concatVectors(2,&v1,&v2);
|
||||
|
||||
}
|
||||
|
||||
inline GeneralCameraT compose(const Pose3 &p) const {
|
||||
return GeneralCameraT( pose().compose(p), calibration_ ) ;
|
||||
}
|
||||
|
||||
Matrix D2d_camera(const Point3& point) const {
|
||||
Point2 intrinsic = calibrated_.project(point);
|
||||
Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
|
||||
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
|
||||
Matrix D_2d_pose = prod(D_2d_intrinsic,D_intrinsic_pose);
|
||||
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
|
||||
|
||||
const int n1 = calibrated_.dim() ;
|
||||
const int n2 = calibration_.dim() ;
|
||||
Matrix D(2,n1+n2) ;
|
||||
|
||||
subrange(D,0,2,0,n1) = D_2d_pose ;
|
||||
subrange(D,0,2,n1,n1+n2) = D_2d_calibration ;
|
||||
return D;
|
||||
}
|
||||
|
||||
Matrix D2d_3d(const Point3& point) const {
|
||||
Point2 intrinsic = calibrated_.project(point);
|
||||
Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
|
||||
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
|
||||
return prod(D_2d_intrinsic,D_intrinsic_3d);
|
||||
}
|
||||
|
||||
Matrix D2d_camera_3d(const Point3& point) const {
|
||||
Point2 intrinsic = calibrated_.project(point);
|
||||
Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
|
||||
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
|
||||
Matrix D_2d_pose = prod(D_2d_intrinsic,D_intrinsic_pose);
|
||||
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
|
||||
|
||||
Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
|
||||
Matrix D_2d_3d = prod(D_2d_intrinsic,D_intrinsic_3d);
|
||||
|
||||
const int n1 = calibrated_.dim() ;
|
||||
const int n2 = calibration_.dim() ;
|
||||
|
||||
Matrix D(2,n1+n2+3) ;
|
||||
|
||||
subrange(D,0,2,0,n1) = D_2d_pose ;
|
||||
subrange(D,0,2,n1,n1+n2) = D_2d_calibration ;
|
||||
subrange(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ;
|
||||
return D;
|
||||
}
|
||||
|
||||
//inline size_t dim() { return Camera::dim() + Calibration::dim() ; }
|
||||
inline size_t dim() const { return calibrated().dim() + calibration().dim() ; }
|
||||
static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; }
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version)
|
||||
{
|
||||
ar & BOOST_SERIALIZATION_NVP(calibrated_);
|
||||
ar & BOOST_SERIALIZATION_NVP(calibration_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
typedef GeneralCameraT<CalibratedCamera,Cal3Bundler> Cal3BundlerCamera;
|
||||
typedef GeneralCameraT<CalibratedCamera,Cal3DS2> Cal3DS2Camera;
|
||||
typedef GeneralCameraT<CalibratedCamera,Cal3_S2> Cal3_S2Camera;
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -15,7 +15,8 @@ sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp
|
|||
check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3 tests/testPose3
|
||||
|
||||
# Cameras
|
||||
sources += Cal3_S2.cpp CalibratedCamera.cpp SimpleCamera.cpp
|
||||
headers += GeneralCameraT.h
|
||||
sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp
|
||||
check_PROGRAMS += tests/testCal3_S2 tests/testCalibratedCamera tests/testSimpleCamera
|
||||
|
||||
# Stereo
|
||||
|
|
|
|||
Loading…
Reference in New Issue