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

Handle q_i = 0 (or NaN) for LOST

release/4.3a0
Travis Driver 2023-06-05 19:11:41 -07:00
parent 3eec88f60e
commit fcee29e628
1 changed files with 27 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/geometry/triangulation.cpp
View File

@ -95,16 +95,37 @@ 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();
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();
const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
// 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) *
@ -117,7 +138,7 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
A_Qr.setThreshold(rank_tol);
// if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
return A_Qr.solve(b);
}