Comments and formatting, plus some added tests on Cal3Bundler
Frank Dellaert
parent
7d0674fe4b
commit
a423849afe
|
|
@ -24,131 +24,144 @@
|
|||
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 ;
|
||||
Cal3Bundler::Cal3Bundler() :
|
||||
f_(1), k1_(0), k2_(0) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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) ;
|
||||
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&> Dcal, boost::optional<Matrix&> Dp) 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 ;
|
||||
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_ + k2_ * r) * r;
|
||||
const double fg = f_ * g;
|
||||
|
||||
// semantic meaningful version
|
||||
//if (H1) *H1 = D2d_calibration(p) ;
|
||||
//if (H2) *H2 = D2d_intrinsic(p) ;
|
||||
//if (H1) *H1 = D2d_calibration(p);
|
||||
//if (H2) *H2 = D2d_intrinsic(p);
|
||||
|
||||
// unrolled version, much faster
|
||||
if ( H1 || H2 ) {
|
||||
if (Dcal || Dp) {
|
||||
|
||||
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) ;
|
||||
const double fx = f_ * x, fy = f_ * y;
|
||||
if (Dcal) {
|
||||
*Dcal = 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) ;
|
||||
if (Dp) {
|
||||
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;
|
||||
*Dp = Matrix_(2, 2, //
|
||||
fg + fx * dg_dx, fx * dg_dy, //
|
||||
fy * dg_dx, fg + fy * dg_dy);
|
||||
}
|
||||
}
|
||||
|
||||
return Point2(fg * x, fg * y) ;
|
||||
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 ;
|
||||
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;
|
||||
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) ;
|
||||
Matrix DR = Matrix_(2, 2, g + x * dg_dx, x * dg_dy, y * dg_dx, g + y * dg_dy);
|
||||
return 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) ;
|
||||
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;
|
||||
|
||||
return Matrix_(2, 3,
|
||||
g*x, f*x*r , f*x*r2,
|
||||
g*y, f*y*r , f*y*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 ;
|
||||
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_ + k2_ * r) * r;
|
||||
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);
|
||||
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 ;
|
||||
return H;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Bundler Cal3Bundler::retract(const Vector& d) const { return Cal3Bundler(vector() + d) ; }
|
||||
Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
|
||||
return Cal3Bundler(vector() + d);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { return vector() - T2.vector(); }
|
||||
Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
|
||||
return vector() - T2.vector();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@
|
|||
* @file Cal3Bundler.h
|
||||
* @brief Calibration used by Bundler
|
||||
* @date Sep 25, 2010
|
||||
* @author ydjian
|
||||
* @author Yong Dian Jian
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -31,30 +31,31 @@ namespace gtsam {
|
|||
class GTSAM_EXPORT Cal3Bundler : public DerivedValue<Cal3Bundler> {
|
||||
|
||||
private:
|
||||
double f_, k1_, k2_ ;
|
||||
double f_; ///< focal length
|
||||
double k1_, k2_;///< radial distortion
|
||||
|
||||
public:
|
||||
|
||||
Matrix K() const ;
|
||||
Vector k() const ;
|
||||
Matrix K() const;///< Standard 3*3 calibration matrix
|
||||
Vector k() const;///< Radial distortion parameters
|
||||
|
||||
Vector vector() const;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Cal3Bundler() ;
|
||||
/// Default constructor
|
||||
Cal3Bundler();
|
||||
|
||||
///TODO: comment
|
||||
Cal3Bundler(const double f, const double k1, const double k2) ;
|
||||
/// Constructor
|
||||
Cal3Bundler(const double f, const double k1, const double k2);
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Cal3Bundler(const Vector &v) ;
|
||||
/// Construct from vector
|
||||
Cal3Bundler(const Vector &v);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -70,35 +71,41 @@ public:
|
|||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const ;
|
||||
boost::optional<Matrix&> Dcal = boost::none,
|
||||
boost::optional<Matrix&> Dp = boost::none) const;
|
||||
|
||||
///TODO: comment
|
||||
Matrix D2d_intrinsic(const Point2& p) const ;
|
||||
/// 2*2 Jacobian of uncalibrate with respect to intrinsic coordinates
|
||||
Matrix D2d_intrinsic(const Point2& p) const;
|
||||
|
||||
///TODO: comment
|
||||
Matrix D2d_calibration(const Point2& p) const ;
|
||||
/// 2*3 Jacobian of uncalibrate wrpt CalBundler parameters
|
||||
Matrix D2d_calibration(const Point2& p) const;
|
||||
|
||||
///TODO: comment
|
||||
Matrix D2d_intrinsic_calibration(const Point2& p) const ;
|
||||
/// 2*5 Jacobian of uncalibrate wrpt both intrinsic and calibration
|
||||
Matrix D2d_intrinsic_calibration(const Point2& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Cal3Bundler retract(const Vector& d) const ;
|
||||
/// Update calibration with tangent space delta
|
||||
Cal3Bundler retract(const Vector& d) const;
|
||||
|
||||
///TODO: comment
|
||||
Vector localCoordinates(const Cal3Bundler& T2) const ;
|
||||
/// Calculate local coordinates to another calibration
|
||||
Vector localCoordinates(const Cal3Bundler& T2) const;
|
||||
|
||||
///TODO: comment
|
||||
virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
|
||||
/// dimensionality
|
||||
virtual size_t dim() const {return 3;}
|
||||
|
||||
///TODO: comment
|
||||
static size_t Dim() { return 3; } //TODO: make a final dimension variable
|
||||
/// dimensionality
|
||||
static size_t Dim() {return 3;}
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -118,7 +125,6 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||
}
|
||||
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -92,10 +92,16 @@ public:
|
|||
/// image center in y
|
||||
inline double py() const { return v0_;}
|
||||
|
||||
/// Convert ideal point p (in intrinsic coordinates) into pixel coordinates
|
||||
/**
|
||||
* 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&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const ;
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -39,24 +39,22 @@ TEST( Cal3Bundler, calibrate)
|
|||
CHECK(assert_equal(q,q_hat));
|
||||
}
|
||||
|
||||
|
||||
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Bundler, Duncalibrate1)
|
||||
TEST( Cal3Bundler, Duncalibrate)
|
||||
{
|
||||
Matrix computed;
|
||||
K.uncalibrate(p, computed, boost::none);
|
||||
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
|
||||
CHECK(assert_equal(numerical,computed,1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Bundler, Duncalibrate2)
|
||||
{
|
||||
Matrix computed; K.uncalibrate(p, boost::none, computed);
|
||||
Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
|
||||
CHECK(assert_equal(numerical,computed,1e-7));
|
||||
Matrix Dcal, Dp;
|
||||
K.uncalibrate(p, Dcal, Dp);
|
||||
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
|
||||
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
|
||||
CHECK(assert_equal(numerical1,Dcal,1e-7));
|
||||
CHECK(assert_equal(numerical2,Dp,1e-7));
|
||||
CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7));
|
||||
CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7));
|
||||
Matrix Dcombined(2,5);
|
||||
Dcombined << Dp, Dcal;
|
||||
CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue