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

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

release/4.3a0
Varun Agrawal 2021-06-04 10:45:17 -04:00 committed by GitHub
parent 26356db7af 2d739dd5e8
commit 2e29d45e1a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 71 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/geometry/Cal3Bundler.cpp
View File

@ -96,23 +96,27 @@ 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
double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for
// pixels around boundary
Point2 pn(x, y);
double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
const Point2 invKPi(px, py);
Point2 pn;
// 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_) break;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy;
int iteration = 0;
do {
// initialize pn with distortion included
const double rr = (px * px) + (py * py);
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
}
if (distance2(uncalibrate(pn), pi) <= tol_) break;
// Set px and py using intrinsic coordinates since that is where radial
// distortion correction is done.
px = pn.x(), py = pn.y();
iteration++;
} while (iteration < maxIterations);
if (iteration >= maxIterations)
throw std::runtime_error(
@ -148,4 +152,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
return H;
}
} // \ namespace gtsam
} // namespace gtsam

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

@ -62,6 +62,60 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibrateDefault) {
Cal3Bundler trueK(1, 0, 0);
Matrix Dcal, Dp;
Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
Point2 expected = p;
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
CHECK(assert_equal(numerical1, Dcal, 1e-7));
CHECK(assert_equal(numerical2, Dp, 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibrateDefault) {
Cal3Bundler trueK(1, 0, 0);
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = trueK.uncalibrate(pn);
Point2 actual = trueK.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
Cal3Bundler K(5, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(12, 17);
CHECK(assert_equal(expected, actual, 1e-7));
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));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibratePrincipalPoint) {
Cal3Bundler K(2, 0, 0, 2, 2);
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));
}
/* ************************************************************************* */
TEST(Cal3Bundler, Duncalibrate) {
Matrix Dcal, Dp;