Added optional image center to Cal3Bundler, used only as a convenience constant so one does not need to transform the image coordinates.
Frank Dellaert
parent
b51a8380c4
commit
8604bc7328
|
|
@ -29,18 +29,15 @@ Cal3Bundler::Cal3Bundler() :
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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) {
|
||||
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
|
||||
f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Cal3Bundler::K() const {
|
||||
return Matrix_(3, 3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0);
|
||||
Matrix3 K;
|
||||
K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
|
||||
return K;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -55,13 +52,14 @@ Vector Cal3Bundler::vector() const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Cal3Bundler::print(const std::string& s) const {
|
||||
gtsam::print(vector(), s + ".K");
|
||||
gtsam::print(Vector_(5, f_, k1_, k2_, u0_, v0_), 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)
|
||||
|| fabs(k2_ - K.k2_) > tol || fabs(u0_ - K.u0_) > tol
|
||||
|| fabs(v0_ - K.v0_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
|
@ -69,92 +67,59 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
|
|||
/* ************************************************************************* */
|
||||
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)
|
||||
// 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 g = 1. + (k1_ + k2_ * r) * r;
|
||||
const double u = g * x, v = g * y;
|
||||
const double f = f_;
|
||||
|
||||
// semantic meaningful version
|
||||
//if (H1) *H1 = D2d_calibration(p);
|
||||
//if (H2) *H2 = D2d_intrinsic(p);
|
||||
|
||||
// unrolled version, much faster
|
||||
if (Dcal || Dp) {
|
||||
// Derivatives make use of intermediate variables above
|
||||
if (Dcal) {
|
||||
double rx = r * x, ry = r * y;
|
||||
|
||||
if (Dcal) {
|
||||
Eigen::Matrix<double, 2, 3> D;
|
||||
D << u, f * rx, f * r * rx, v, f * ry, f * r * ry;
|
||||
*Dcal = D;
|
||||
}
|
||||
|
||||
if (Dp) {
|
||||
const double dg_dx = 2. * k1_ * x + 4. * k2_ * rx;
|
||||
const double dg_dy = 2. * k1_ * y + 4. * k2_ * ry;
|
||||
Eigen::Matrix<double, 2, 2> D;
|
||||
D << g + x * dg_dx, x * dg_dy, y * dg_dx, g + y * dg_dy;
|
||||
*Dp = f * D;
|
||||
}
|
||||
Eigen::Matrix<double, 2, 3> D;
|
||||
D << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
|
||||
*Dcal = D;
|
||||
}
|
||||
|
||||
return Point2(f * u, f * v);
|
||||
if (Dp) {
|
||||
const double a = 2. * (k1_ + 2. * k2_ * r);
|
||||
const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
|
||||
Eigen::Matrix<double, 2, 2> D;
|
||||
D << g + axx, axy, axy, g + ayy;
|
||||
*Dp = f_ * D;
|
||||
}
|
||||
|
||||
return Point2(u0_ + f_ * u, v0_ + f_ * v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
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 DK * DR;
|
||||
Matrix Dp;
|
||||
uncalibrate(p, boost::none, Dp);
|
||||
return Dp;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
|
||||
return Matrix_(2, 3, g * x, f * x * r, f * x * r2, g * y, f * y * r,
|
||||
f * y * r2);
|
||||
Matrix Dcal;
|
||||
uncalibrate(p, Dcal, boost::none);
|
||||
return Dcal;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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_ + 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 Dcal, Dp;
|
||||
uncalibrate(p, Dcal, Dp);
|
||||
Matrix H(2, 5);
|
||||
H << Dp, Dcal;
|
||||
return H;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
|
||||
return Cal3Bundler(vector() + d);
|
||||
return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -28,16 +28,17 @@ namespace gtsam {
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Bundler : public DerivedValue<Cal3Bundler> {
|
||||
class GTSAM_EXPORT Cal3Bundler: public DerivedValue<Cal3Bundler> {
|
||||
|
||||
private:
|
||||
double f_; ///< focal length
|
||||
double k1_, k2_;///< radial distortion
|
||||
double k1_, k2_; ///< radial distortion
|
||||
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
|
||||
|
||||
public:
|
||||
|
||||
Matrix K() const;///< Standard 3*3 calibration matrix
|
||||
Vector k() const;///< Radial distortion parameters
|
||||
Matrix K() const; ///< Standard 3*3 calibration matrix
|
||||
Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
|
||||
|
||||
Vector vector() const;
|
||||
|
||||
|
|
@ -47,15 +48,15 @@ public:
|
|||
/// Default constructor
|
||||
Cal3Bundler();
|
||||
|
||||
/// Constructor
|
||||
Cal3Bundler(const double f, const double k1, const double k2);
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/// Construct from vector
|
||||
Cal3Bundler(const Vector &v);
|
||||
/**
|
||||
* Constructor
|
||||
* @param f focal length
|
||||
* @param k1 first radial distortion coefficient (quadratic)
|
||||
* @param k2 second radial distortion coefficient (quartic)
|
||||
* @param u0 optional image center (default 0), considered a constant
|
||||
* @param v0 optional image center (default 0), considered a constant
|
||||
*/
|
||||
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -78,17 +79,16 @@ public:
|
|||
* @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;
|
||||
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
|
||||
boost::none, boost::optional<Matrix&> Dp = boost::none) const;
|
||||
|
||||
/// 2*2 Jacobian of uncalibrate with respect to intrinsic coordinates
|
||||
/// @deprecated might be removed in next release, use uncalibrate
|
||||
Matrix D2d_intrinsic(const Point2& p) const;
|
||||
|
||||
/// 2*3 Jacobian of uncalibrate wrpt CalBundler parameters
|
||||
/// @deprecated might be removed in next release, use uncalibrate
|
||||
Matrix D2d_calibration(const Point2& p) const;
|
||||
|
||||
/// 2*5 Jacobian of uncalibrate wrpt both intrinsic and calibration
|
||||
/// @deprecated might be removed in next release, use uncalibrate
|
||||
Matrix D2d_intrinsic_calibration(const Point2& p) const;
|
||||
|
||||
/// @}
|
||||
|
|
@ -102,10 +102,14 @@ public:
|
|||
Vector localCoordinates(const Cal3Bundler& T2) const;
|
||||
|
||||
/// dimensionality
|
||||
virtual size_t dim() const {return 3;}
|
||||
virtual size_t dim() const {
|
||||
return 3;
|
||||
}
|
||||
|
||||
/// dimensionality
|
||||
static size_t Dim() {return 3;}
|
||||
static size_t Dim() {
|
||||
return 3;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -116,17 +120,19 @@ private:
|
|||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version)
|
||||
{
|
||||
ar & boost::serialization::make_nvp("Cal3Bundler",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("Cal3Bundler",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(f_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(u0_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v0_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ using namespace gtsam;
|
|||
GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
|
||||
|
||||
static Cal3Bundler K(500, 1e-3, 1e-3);
|
||||
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
|
||||
static Point2 p(2,3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -34,11 +34,12 @@ TEST( Cal3Bundler, calibrate)
|
|||
Vector v = K.vector() ;
|
||||
double r = p.x()*p.x() + p.y()*p.y() ;
|
||||
double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
|
||||
Point2 q_hat (g*p.x(), g*p.y()) ;
|
||||
Point2 q = K.uncalibrate(p);
|
||||
CHECK(assert_equal(q,q_hat));
|
||||
Point2 expected (1000+g*p.x(), 2000+g*p.y()) ;
|
||||
Point2 actual = K.uncalibrate(p);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -46,7 +47,7 @@ TEST( Cal3Bundler, Duncalibrate)
|
|||
{
|
||||
Matrix Dcal, Dp;
|
||||
Point2 actual = K.uncalibrate(p, Dcal, Dp);
|
||||
Point2 expected(1182, 1773);
|
||||
Point2 expected(2182, 3773);
|
||||
CHECK(assert_equal(expected,actual,1e-7));
|
||||
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
|
||||
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
|
||||
|
|
@ -68,7 +69,7 @@ TEST( Cal3Bundler, assert_equal)
|
|||
/* ************************************************************************* */
|
||||
TEST( Cal3Bundler, retract)
|
||||
{
|
||||
Cal3Bundler expected(510, 2e-3, 2e-3);
|
||||
Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
|
||||
Vector d(3);
|
||||
d << 10, 1e-3, 1e-3;
|
||||
Cal3Bundler actual = K.retract(d);
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ int main()
|
|||
// Cal3DS2: 0.14201 musecs/call
|
||||
// After Cal3DS2 fix: 0.12231 musecs/call
|
||||
// Cal3Bundler: 0.12000 musecs/call
|
||||
// Cal3Bundler fix: 0.13864 musecs/call
|
||||
// Cal3Bundler fix: 0.14637 musecs/call
|
||||
{
|
||||
long timeLog = clock();
|
||||
for(int i = 0; i < n; i++)
|
||||
|
|
@ -64,12 +64,12 @@ int main()
|
|||
}
|
||||
|
||||
// Oct 12 2013, iMac 3.06GHz Core i3
|
||||
// Original: 3.87199 musecs/call
|
||||
// After collapse: 2.62684 musecs/call
|
||||
// Cal3DS2: 4.33297 musecs/call
|
||||
// After Cal3DS2 fix: 3.28565 musecs/call
|
||||
// Cal3Bundler: 2.65559 musecs/call
|
||||
// Cal3Bundler fix: 2.18481 musecs/call
|
||||
// Original: 3.8720 musecs/call
|
||||
// After collapse: 2.6269 musecs/call
|
||||
// Cal3DS2: 4.3330 musecs/call
|
||||
// After Cal3DS2 fix: 3.2857 musecs/call
|
||||
// Cal3Bundler: 2.6556 musecs/call
|
||||
// Cal3Bundler fix: 2.1613 musecs/call
|
||||
{
|
||||
Matrix Dpose, Dpoint;
|
||||
long timeLog = clock();
|
||||
|
|
@ -84,10 +84,10 @@ int main()
|
|||
// Oct 12 2013, iMac 3.06GHz Core i3
|
||||
// Original: 4.0119 musecs/call
|
||||
// After collapse: 2.5698 musecs/call
|
||||
// Cal3DS2: 4.83248 musecs/call
|
||||
// After Cal3DS2 fix: 3.44832 musecs/call
|
||||
// Cal3Bundler: 2.51124 musecs/call
|
||||
// Cal3Bundler fix: 2.17292 musecs/call
|
||||
// Cal3DS2: 4.8325 musecs/call
|
||||
// After Cal3DS2 fix: 3.4483 musecs/call
|
||||
// Cal3Bundler: 2.5112 musecs/call
|
||||
// Cal3Bundler fix: 2.0946 musecs/call
|
||||
{
|
||||
Matrix Dpose, Dpoint, Dcal;
|
||||
long timeLog = clock();
|
||||
|
|
|
|||
Loading…
Reference in New Issue