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

Merge pull request #539 from borglab/fix/cal3bundler-jacobians 

Add calibrate with jacobians for Cal3Bundler
release/4.3a0
Varun Agrawal 2020-09-29 15:29:43 -04:00 committed by GitHub
parent 6fdf39d2c1 7dab718861
commit 1bbd233f0f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 72 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/geometry/Cal3Bundler.cpp
View File

@ -25,13 +25,13 @@ namespace gtsam {
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler() :
f_(1), k1_(0), k2_(0), u0_(0), v0_(0) {
f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
}
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) {
}
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
double tol)
: f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
/* ************************************************************************* */
Matrix3 Cal3Bundler::K() const {
@ -94,21 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
}
/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
Point2 Cal3Bundler::calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2 :-(
// but specialized with k1,k2 non-zero only and fx=fy and s=0
const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
Point2 pn(x, y);
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol)
if (distance2(uncalibrate(pn), pi) <= tol_)
break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
@ -118,6 +121,22 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
throw std::runtime_error(
"Cal3Bundler::calibrate fails to converge. need a better initialization");
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
// df/pi = -I (pn and pi are independent args)
// Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
// Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
Matrix23 H_uncal_K;
Matrix22 H_uncal_pn;
if (Dcal || Dp) {
// Compute uncalibrate Jacobians
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
if (Dp) *Dp = H_uncal_pn.inverse();
if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K;
}
return pn;
}

19
gtsam/geometry/Cal3Bundler.h
View File

@ -33,6 +33,7 @@ private:
double f_; ///< focal length
double k1_, k2_; ///< radial distortion
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
double tol_; ///< tolerance value when calibrating
public:
@ -51,8 +52,10 @@ public:
* @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
* @param tol optional calibration tolerance value
*/
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
double tol = 1e-5);
virtual ~Cal3Bundler() {}
@ -117,8 +120,17 @@ public:
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Convert a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
/**
* Convert a pixel coordinate to ideal coordinate xy
* @param p point in image coordinates
* @param tol optional tolerance threshold value for iterative minimization
* @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix2 D2d_intrinsic(const Point2& p) const;
@ -164,6 +176,7 @@ private:
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(tol_);
}
/// @}

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

@ -52,12 +52,14 @@ TEST( Cal3Bundler, calibrate )
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Bundler, Duncalibrate)
{
@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate)
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));
}
/* ************************************************************************* */
TEST( Cal3Bundler, Dcalibrate)
{
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 actual = K.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
CHECK(assert_equal(numerical2,Dp,1e-5));
}
/* ************************************************************************* */

9
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
TEST( PinholeCamera, Cal3Bundler) {
Cal3Bundler calibration;
Pose3 wTc;
PinholeCamera<Cal3Bundler> camera(wTc, calibration);
Point2 p(50, 100);
camera.backproject(p, 10);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

3
gtsam/gtsam.i
View File

@ -959,6 +959,7 @@ class Cal3Bundler {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
// Testable
void print(string s) const;
@ -971,7 +972,7 @@ class Cal3Bundler {
Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// Standard Interface