12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Comments and formatting, plus some added tests on Cal3Bundler

release/4.3a0
Frank Dellaert 2013-10-12 14:31:18 +00:00
parent 7d0674fe4b
commit a423849afe
4 changed files with 155 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

187
gtsam/geometry/Cal3Bundler.cpp
View File

@ -24,131 +24,144 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {} 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, Cal3Bundler::Cal3Bundler(const Vector &v) :
boost::optional<Matrix&> H1, f_(v(0)), k1_(v(1)), k2_(v(2)) {
boost::optional<Matrix&> H2) const { }
// r = x^2 + y^2 ;
// g = (1 + k(1)*r + k(2)*r^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) // pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y() ; const double x = p.x(), y = p.y();
const double r = x*x + y*y ; const double r = x * x + y * y;
const double r2 = r*r ; const double r2 = r * r;
const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply const double g = 1 + (k1_ + k2_ * r) * r;
const double fg = f_*g ; const double fg = f_ * g;
// semantic meaningful version // semantic meaningful version
//if (H1) *H1 = D2d_calibration(p) ; //if (H1) *H1 = D2d_calibration(p);
//if (H2) *H2 = D2d_intrinsic(p) ; //if (H2) *H2 = D2d_intrinsic(p);
// unrolled version, much faster // unrolled version, much faster
if ( H1 || H2 ) { if (Dcal || Dp) {
const double fx = f_*x, fy = f_*y ; const double fx = f_ * x, fy = f_ * y;
if ( H1 ) { if (Dcal) {
*H1 = Matrix_(2, 3, *Dcal = Matrix_(2, 3, //
g*x, fx*r , fx*r2, g * x, fx * r, fx * r2, //
g*y, fy*r , fy*r2) ; g * y, fy * r, fy * r2);
} }
if ( H2 ) { if (Dp) {
const double dr_dx = 2*x ; const double dr_dx = 2 * x;
const double dr_dy = 2*y ; const double dr_dy = 2 * y;
const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; 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 dg_dy = k1_ * dr_dy + k2_ * 2 * r * dr_dy;
*H2 = Matrix_(2, 2, *Dp = Matrix_(2, 2, //
fg + fx*dg_dx, fx*dg_dy, fg + fx * dg_dx, fx * dg_dy, //
fy*dg_dx , fg + fy*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 { Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double r = xx + yy ; const double r = xx + yy;
const double dr_dx = 2*x ; const double dr_dx = 2 * x;
const double dr_dy = 2*y ; const double dr_dy = 2 * y;
const double g = 1 + (k1_+k2_*r) * r ; // save one multiply const double g = 1 + (k1_ + k2_ * r) * r;
//const double g = 1 + k1_*r + k2_*r*r ; const double dg_dx = k1_ * dr_dx + k2_ * 2 * r * dr_dx;
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 dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ;
Matrix DK = Matrix_(2, 2, f_, 0.0, 0.0, f_); Matrix DK = Matrix_(2, 2, f_, 0.0, 0.0, f_);
Matrix DR = Matrix_(2, 2, Matrix DR = Matrix_(2, 2, g + x * dg_dx, x * dg_dy, y * dg_dx, g + y * dg_dy);
g + x*dg_dx, x*dg_dy,
y*dg_dx , g + y*dg_dy) ;
return DK * DR; return DK * DR;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ; const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double r = xx + yy ; const double r = xx + yy;
const double r2 = r*r ; const double r2 = r * r;
const double f = f_ ; const double f = f_;
const double g = 1 + (k1_+k2_*r) * r ; // save one multiply const double g = 1 + (k1_ + k2_ * r) * r;
//const double g = (1+k1_*r+k2_*r2) ;
return Matrix_(2, 3, return Matrix_(2, 3, g * x, f * x * r, f * x * r2, g * y, f * y * r,
g*x, f*x*r , f*x*r2, f * y * r2);
g*y, f*y*r , f*y*r2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { 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 x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double r = xx + yy ; const double r = xx + yy;
const double r2 = r*r; const double r2 = r * r;
const double dr_dx = 2*x ; const double dr_dx = 2 * x;
const double dr_dy = 2*y ; const double dr_dy = 2 * y;
const double g = 1 + (k1_*r) + (k2_*r2) ; const double g = 1 + (k1_ + k2_ * r) * r;
const double f = f_ ; const double f = f_;
const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; 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 dg_dy = k1_ * dr_dy + k2_ * 2 * r * dr_dy;
Matrix H = Matrix_(2,5, Matrix H = Matrix_(2, 5, f * (g + x * dg_dx), f * (x * dg_dy), g * x,
f*(g + x*dg_dx), f*(x*dg_dy), g*x, f*x*r , f*x*r2, f * x * r, f * x * r2, f * (y * dg_dx), f * (g + y * dg_dy), g * y,
f*(y*dg_dx) , f*(g + y*dg_dy), g*y, f*y*r , f*y*r2); 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();
}
} }

62
gtsam/geometry/Cal3Bundler.h
View File

@ -13,7 +13,7 @@
* @file Cal3Bundler.h * @file Cal3Bundler.h
* @brief Calibration used by Bundler * @brief Calibration used by Bundler
* @date Sep 25, 2010 * @date Sep 25, 2010
* @author ydjian * @author Yong Dian Jian
*/ */
#pragma once #pragma once
@ -31,30 +31,31 @@ namespace gtsam {
class GTSAM_EXPORT Cal3Bundler : public DerivedValue<Cal3Bundler> { class GTSAM_EXPORT Cal3Bundler : public DerivedValue<Cal3Bundler> {
private: private:
double f_, k1_, k2_ ; double f_; ///< focal length
double k1_, k2_;///< radial distortion
public: public:
Matrix K() const ; Matrix K() const;///< Standard 3*3 calibration matrix
Vector k() const ; Vector k() const;///< Radial distortion parameters
Vector vector() const; Vector vector() const;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
///TODO: comment /// Default constructor
Cal3Bundler() ; Cal3Bundler();
///TODO: comment /// Constructor
Cal3Bundler(const double f, const double k1, const double k2) ; Cal3Bundler(const double f, const double k1, const double k2);
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors
/// @{ /// @{
///TODO: comment /// Construct from vector
Cal3Bundler(const Vector &v) ; Cal3Bundler(const Vector &v);
/// @} /// @}
/// @name Testable /// @name Testable
@ -70,35 +71,41 @@ public:
/// @name Standard Interface /// @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, Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> H2 = boost::none) const ; boost::optional<Matrix&> Dp = boost::none) const;
///TODO: comment /// 2*2 Jacobian of uncalibrate with respect to intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ; Matrix D2d_intrinsic(const Point2& p) const;
///TODO: comment /// 2*3 Jacobian of uncalibrate wrpt CalBundler parameters
Matrix D2d_calibration(const Point2& p) const ; Matrix D2d_calibration(const Point2& p) const;
///TODO: comment /// 2*5 Jacobian of uncalibrate wrpt both intrinsic and calibration
Matrix D2d_intrinsic_calibration(const Point2& p) const ; Matrix D2d_intrinsic_calibration(const Point2& p) const;
/// @} /// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{
///TODO: comment /// Update calibration with tangent space delta
Cal3Bundler retract(const Vector& d) const ; Cal3Bundler retract(const Vector& d) const;
///TODO: comment /// Calculate local coordinates to another calibration
Vector localCoordinates(const Cal3Bundler& T2) const ; Vector localCoordinates(const Cal3Bundler& T2) const;
///TODO: comment /// dimensionality
virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) virtual size_t dim() const {return 3;}
///TODO: comment /// dimensionality
static size_t Dim() { return 3; } //TODO: make a final dimension variable static size_t Dim() {return 3;}
private: private:
@ -118,7 +125,6 @@ private:
ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(k2_);
} }
/// @} /// @}
}; };

12
gtsam/geometry/Cal3DS2.h
View File

@ -92,10 +92,16 @@ public:
/// image center in y /// image center in y
inline double py() const { return v0_;} 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, Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> H2 = boost::none) const ; boost::optional<Matrix&> Dp = boost::none) const ;
/// Conver a pixel coordinate to ideal coordinate /// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, const double tol=1e-5) const; Point2 calibrate(const Point2& p, const double tol=1e-5) const;

26
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -39,24 +39,22 @@ TEST( Cal3Bundler, calibrate)
CHECK(assert_equal(q,q_hat)); CHECK(assert_equal(q,q_hat));
} }
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3Bundler, Duncalibrate1) TEST( Cal3Bundler, Duncalibrate)
{ {
Matrix computed; Matrix Dcal, Dp;
K.uncalibrate(p, computed, boost::none); K.uncalibrate(p, Dcal, Dp);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p); Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-7)); 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));
TEST( Cal3Bundler, Duncalibrate2) CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7));
{ Matrix Dcombined(2,5);
Matrix computed; K.uncalibrate(p, boost::none, computed); Dcombined << Dp, Dcal;
Matrix numerical = numericalDerivative22(uncalibrate_, K, p); CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7));
CHECK(assert_equal(numerical,computed,1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */