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

Fix initial guess for Cal3Bundler calibrate #775

Merged
merged 6 commits into from
Jun 4, 2021
+71 −13
Merged

Fix initial guess for Cal3Bundler calibrate #775

Show file tree
Hide file tree
Changes from 2 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
15 changes: 8 additions & 7 deletions gtsam/geometry/Cal3Bundler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,18 +99,19 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
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);

// initialize pn with distortion included
double px = pi.x(), py = pi.y(), rr = px * px + py * py;
double g = (1 + k1_ * rr + k2_ * rr * rr);
Point2 pn = invKPi / g;
Copy link
Contributor

Choose a reason for hiding this comment

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

invKPi is in normalized coordinates but g is in pixel (unnormalized image) coordinates here. Shouldnt px and py be initialized to x and y respectively?

Copy link
Contributor

Choose a reason for hiding this comment

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

If you change that, it becomes as though writing the first iteration of the loop outside the loop.
So can the loop can be changed to a do-while? (I am not a fan of do-while or anything, but maybe this is a valid use case? )

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

I did think about using a do-while yesterday. Let me update.

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

I believe g should be initialized using the pixel coordinates since we capture the radial distortions that way. If g was initialized to x and y (aka the intrinsic coordinates), we wouldn't see any effect of the radial distortion in the inverse process.

Copy link
Contributor

Choose a reason for hiding this comment

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

But for rr we need the radius from the center of the image, so we should subtract the principal point right?

I think Pi = K * g * Pn where g is computed using Pn (was able to find a couple quick references: slide 30 here and slide 13 here). This is how it is implemented inside the for loop. While initializing, Pn is unknown, so we can only do as much as computing g using invKPi. Please correct me if I am wrong.

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Great references. I agree that g should be defined using the intrinsic coordinates and not the image coordinates. I like your suggestion to use invKPi since that makes sense to me as per your references. I'll update the PR later tonight.

Copy link
Contributor

Choose a reason for hiding this comment

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

@akshay-krishnan could you explain this point a bit more? I didn't quite follow

Copy link
Contributor

Choose a reason for hiding this comment

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

John, could you check the references? They explain the pinhole camera model with distortion parameters.
In this model,

Pi = K * Pn_distorted where Pn_distorted = g * Pn_undistorted.
And g is a nonlinear function of Pn_undistorted.

In calibrate() we are trying to find Pn_undistorted by optimization. The initial guess for Pn_undistorted = (invK * Pi) / g (using the above). But since g is a function of Pn_undistorted itself, we need an initial guess for g as well. The best we can do is initialize g using Pn_distorted ( = invK * Pi).
I hope that makes sense.


// 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;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
px = pn.x(), py = pn.y();
rr = (px * px) + (py * py);
double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
}

Expand Down
14 changes: 14 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 @@ -87,6 +87,20 @@ TEST(Cal3Bundler, Dcalibrate) {
CHECK(assert_equal(numerical2, Dp, 1e-5));
}

/* ************************************************************************* */
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, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }

Expand Down