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

more camelcase changes

release/4.3a0
Akshay Krishnan 2022-07-12 19:30:41 -07:00
parent b52aaeab22
commit f347209d4c
3 changed files with 31 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -108,10 +108,10 @@ TEST(triangulation, twoCamerasUsingLOST) {
// 1. Test simple triangulation, perfect in no noise situation
boost::optional<Point3> actual1 = //
triangulatePoint3<PinholeCamera<Cal3_S2>>(
cameras, measurements, rank_tol,
/*optimize=*/false, measurementNoise,
/*use_lost_triangulation=*/true);
triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
/*optimize=*/false,
measurementNoise,
/*useLOST=*/true);
EXPECT(assert_equal(kLandmark, *actual1, 1e-12));
// 3. Add some noise and try again: result should be ~ (4.995,
@ -145,10 +145,10 @@ TEST(triangulation, twoCamerasLOSTvsDLT) {
SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2);
boost::optional<Point3> estimateLOST = //
triangulatePoint3<PinholeCamera<Cal3_S2>>(
cameras, measurements, rank_tol,
/*optimize=*/false, measurementNoise,
/*use_lost_triangulation=*/true);
triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
/*optimize=*/false,
measurementNoise,
/*useLOST=*/true);
// These values are from a MATLAB implementation.
EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3));

19
gtsam/geometry/triangulation.cpp
View File

@ -107,20 +107,19 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
const Point3 d_ij = wTj.translation() - wTi.translation();
const Point3 w_measurement_i =
wTi.rotation().rotate(calibratedMeasurements[i]);
const Point3 w_measurement_j =
wTj.rotation().rotate(calibratedMeasurements[j]);
const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
const double q_i =
w_measurement_i.cross(w_measurement_j).norm() /
(measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm());
const Matrix23 coefficient_mat =
// 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 Matrix23 coefficientMat =
q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
wTi.rotation().matrix().transpose();
A.block<2, 3>(2 * i, 0) << coefficient_mat;
b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation();
A.block<2, 3>(2 * i, 0) << coefficientMat;
b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
}
return A.colPivHouseholderQr().solve(b);
}

30
gtsam/geometry/triangulation.h
View File

@ -415,8 +415,7 @@ inline Point3Vector calibrateMeasurements(
* @param measurements A vector of camera measurements
* @param rank_tol rank tolerance, default 1e-9
* @param optimize Flag to turn on nonlinear refinement of triangulation
* @param use_lost_triangulation whether to use the LOST algorithm instead of
* DLT
* @param useLOST whether to use the LOST algorithm instead of DLT
* @return Returns a Point3
*/
template <class CALIBRATION>
@ -425,13 +424,13 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
const Point2Vector& measurements,
double rank_tol = 1e-9, bool optimize = false,
const SharedNoiseModel& model = nullptr,
const bool use_lost_triangulation = false) {
const bool useLOST = false) {
assert(poses.size() == measurements.size());
if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
// Triangulate linearly
Point3 point;
if (use_lost_triangulation) {
if (useLOST) {
// Reduce input noise model to an isotropic noise model using the mean of
// the diagonal.
const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
@ -481,7 +480,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
* @param measurements A vector of camera measurements
* @param rank_tol rank tolerance, default 1e-9
* @param optimize Flag to turn on nonlinear refinement of triangulation
* @param use_lost_triangulation whether to use the LOST algorithm instead of
* @param useLOST whether to use the LOST algorithm instead of
* DLT
* @return Returns a Point3
*/
@ -490,7 +489,7 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements,
double rank_tol = 1e-9, bool optimize = false,
const SharedNoiseModel& model = nullptr,
const bool use_lost_triangulation = false) {
const bool useLOST = false) {
size_t m = cameras.size();
assert(measurements.size() == m);
@ -498,7 +497,7 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
// Triangulate linearly
Point3 point;
if (use_lost_triangulation) {
if (useLOST) {
// Reduce input noise model to an isotropic noise model using the mean of
// the diagonal.
const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
@ -545,15 +544,14 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
}
/// Pinhole-specific version
template<class CALIBRATION>
Point3 triangulatePoint3(
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false,
const SharedNoiseModel& model = nullptr,
const bool use_lost_triangulation = false) {
return triangulatePoint3<PinholeCamera<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize, model, use_lost_triangulation);
template <class CALIBRATION>
Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
const Point2Vector& measurements,
double rank_tol = 1e-9, bool optimize = false,
const SharedNoiseModel& model = nullptr,
const bool useLOST = false) {
return triangulatePoint3<PinholeCamera<CALIBRATION>> //
(cameras, measurements, rank_tol, optimize, model, useLOST);
}
struct GTSAM_EXPORT TriangulationParameters {