fix triangulatePoint3 for calibrations with distortion #1128
Conversation
Prior implementation only used the K() portion of all Cal3 calibrations for the linear triangulation of points with triangulatePoint3. - Added tests for triangulation with non-Cal3_S2 calibrations. - Added skew to the test Cal3_S2 calibration. - Added an undistortMeasurements step to triangulatePoint3 so that linear triangulation works for calibrations with distortion coefficients.
- added undistort for cameras version of triangulatePoint3 - changed to 2 space indent - changed to calibration from shared calibration
There was a problem hiding this comment.
Something I don't understand right now: it seems you only added code to triangulation.h, so how is the old code broken? And, if it is broken, why is it not modified in this PR?
PS It's been a while I dove deep into triangulation and I have not read the entire header again for this review, so I'm relying on you to jumpstart my memory with this discussion :-)
|
|
||
| PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal); | ||
|
|
||
| // 1. Project two landmarks into two cameras and triangulate |
There was a problem hiding this comment.
- ? Also, please format with Google style...
There was a problem hiding this comment.
good catch, fixed the numbering. I ran clang-format --style=Google for the new code, do you have a different formatter though? It made a lot of changes to the original code that I didn't include.
|
The old code is correct, but only for The reason it now works is the added |
- new code in triangulation and testTriangulation - clean up doc number and typos
gtsam/geometry/triangulation.h
Outdated
| Cal3_S2 pinholeCalibration = createPinholeCalibration(cal); | ||
| Point2Vector undistortedMeasurements; | ||
| // Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted. | ||
| std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements), |
There was a problem hiding this comment.
could we just use
for (auto&& measurement: measurements) ?
|
Will merge now, thanks for your contribution! |
Found what (appears to be) a bug in
triangulatePoint3where it silently uses only theK()component of allCal3calibrations even when they have distortion coefficients. To correct:undistortMeasurementsstep to triangulatePoint3 so that linear triangulation works for calibrations with distortion coefficients.This seemed like a robust way to solve the problem for all calibration types without breaking anything, but maybe a better solution would be to remove the misleading
K()function from distorted calibrations so it doesn't get used by accident and doing different implementations oftriangulatePoint3for different calibrations.Tests are copy pasted as well, if CppUnitTest has something like gtest's Parameterized Testing I'll fix them to be templated tests.