Skip to content

LOST Triangulation Underconstrained #1534

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

Merged
dellaert merged 6 commits into from
Jun 13, 2023
Merged

LOST Triangulation Underconstrained #1534

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
3eec88f
Added rank threshold to triangulateLOST
travisdriver Jun 2, 2023
fcee29e
Handle q_i = 0 (or NaN) for LOST
travisdriver Jun 6, 2023
f6f91ce
Revert formatting for triangulation.cpp
travisdriver Jun 6, 2023
8d134fd
Revert formatting for triangulation.h
travisdriver Jun 6, 2023
bba4b87
Final cleanup
travisdriver Jun 6, 2023
1e6f1b7
Final final cleanup
travisdriver Jun 6, 2023
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
46 changes: 37 additions & 9 deletions gtsam/geometry/triangulation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,8 @@ Vector4 triangulateHomogeneousDLT(

Point3 triangulateLOST(const std::vector<Pose3>& poses,
const Point3Vector& calibratedMeasurements,
const SharedIsotropic& measurementNoise) {
const SharedIsotropic& measurementNoise,
double rank_tol) {
size_t m = calibratedMeasurements.size();
assert(m == poses.size());

Expand All @@ -96,17 +97,38 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
for (size_t i = 0; i < m; i++) {
const Pose3& wTi = poses[i];
// TODO(akshay-krishnan): are there better ways to select j?
const int j = (i + 1) % m;
int j = (i + 1) % m;
const Pose3& wTj = poses[j];

const Point3 d_ij = wTj.translation() - wTi.translation();

const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
Point3 d_ij = wTj.translation() - wTi.translation();
Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
double num_i = wZi.cross(wZj).norm();
double den_i = d_ij.cross(wZj).norm();

// Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and
// wZj, coincide (or the baseline vector coincides with the jth measurement
// vector).
if (num_i == 0 || den_i == 0) {
bool success = false;
for (size_t k = 2; k < m; k++) {
j = (i + k) % m;
const Pose3& wTj = poses[j];

d_ij = wTj.translation() - wTi.translation();
wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
num_i = wZi.cross(wZj).norm();
den_i = d_ij.cross(wZj).norm();
if (num_i > 0 && den_i > 0) {
success = true;
break;
}
}
if (!success) throw(TriangulationUnderconstrainedException());
}

// Note: Setting q_i = 1.0 gives same results as DLT.
const double q_i = wZi.cross(wZj).norm() /
(measurementNoise->sigma() * d_ij.cross(wZj).norm());
const double q_i = num_i / (measurementNoise->sigma() * den_i);

const Matrix23 coefficientMat =
q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
Expand All @@ -115,7 +137,13 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
A.block<2, 3>(2 * i, 0) << coefficientMat;
b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
}
return A.colPivHouseholderQr().solve(b);

Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
A_Qr.setThreshold(rank_tol);

if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());

return A_Qr.solve(b);
}

Point3 triangulateDLT(
Expand Down
10 changes: 6 additions & 4 deletions gtsam/geometry/triangulation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,8 @@ GTSAM_EXPORT Point3 triangulateDLT(
*/
GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
const Point3Vector& calibratedMeasurements,
const SharedIsotropic& measurementNoise);
const SharedIsotropic& measurementNoise,
double rank_tol = 1e-9);

/**
* Create a factor graph with projection factors from poses and one calibration
Expand Down Expand Up @@ -439,7 +440,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
auto calibratedMeasurements =
calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);

point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise,
rank_tol);
} else {
// construct projection matrices from poses & calibration
auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
Expand Down Expand Up @@ -512,7 +514,8 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
auto calibratedMeasurements =
calibrateMeasurements<CAMERA>(cameras, measurements);

point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise,
rank_tol);
} else {
// construct projection matrices from poses & calibration
auto projection_matrices = projectionMatricesFromCameras(cameras);
Expand Down Expand Up @@ -750,4 +753,3 @@ using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>;
} // \namespace gtsam