Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix initial guess for Cal3Bundler calibrate #775

Merged
merged 6 commits into from
Jun 4, 2021
Merged

Fix initial guess for Cal3Bundler calibrate #775

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
9930952
Add Akshay's Cal3Bundler test
varunagrawal May 30, 2021
c252937
account for radial distortion in initial guess for `calibrate`
varunagrawal May 30, 2021
c2f4cc8
initialize with intrinsic coordinates which has radial distortion mod…
varunagrawal Jun 2, 2021
fb784ee
add all of Akshay's tests for default model
varunagrawal Jun 2, 2021
b844026
addressed comments and added an additional test
varunagrawal Jun 3, 2021
2d739dd
make rr and g as const
varunagrawal Jun 3, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 17 additions & 13 deletions gtsam/geometry/Cal3Bundler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -148,4 +152,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
return H;
}

} // \ namespace gtsam
} // namespace gtsam
54 changes: 54 additions & 0 deletions gtsam/geometry/tests/testCal3Bundler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There’s also a global static variable by name K, just FYI in case you missed that. I don’t think it should cause any issues though.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah I know about that. The local scope will take precedence so it's all good.

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;
Expand Down