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 // 1. Test simple triangulation, perfect in no noise situation
boost::optional<Point3> actual1 = // boost::optional<Point3> actual1 = //
triangulatePoint3<PinholeCamera<Cal3_S2>>( triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
cameras, measurements, rank_tol, /*optimize=*/false,
/*optimize=*/false, measurementNoise, measurementNoise,
/*use_lost_triangulation=*/true); /*useLOST=*/true);
EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); EXPECT(assert_equal(kLandmark, *actual1, 1e-12));
// 3. Add some noise and try again: result should be ~ (4.995, // 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); SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2);
boost::optional<Point3> estimateLOST = // boost::optional<Point3> estimateLOST = //
triangulatePoint3<PinholeCamera<Cal3_S2>>( triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
cameras, measurements, rank_tol, /*optimize=*/false,
/*optimize=*/false, measurementNoise, measurementNoise,
/*use_lost_triangulation=*/true); /*useLOST=*/true);
// These values are from a MATLAB implementation. // These values are from a MATLAB implementation.
EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); 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 d_ij = wTj.translation() - wTi.translation();
const Point3 w_measurement_i = const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
wTi.rotation().rotate(calibratedMeasurements[i]); const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
const Point3 w_measurement_j =
wTj.rotation().rotate(calibratedMeasurements[j]);
const double q_i = // Note: Setting q_i = 1.0 gives same results as DLT.
w_measurement_i.cross(w_measurement_j).norm() / const double q_i = wZi.cross(wZj).norm() /
(measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); (measurementNoise->sigma() * d_ij.cross(wZj).norm());
const Matrix23 coefficient_mat =
const Matrix23 coefficientMat =
q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
wTi.rotation().matrix().transpose(); wTi.rotation().matrix().transpose();
A.block<2, 3>(2 * i, 0) << coefficient_mat; A.block<2, 3>(2 * i, 0) << coefficientMat;
b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation(); b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
} }
return A.colPivHouseholderQr().solve(b); return A.colPivHouseholderQr().solve(b);
} }

28
gtsam/geometry/triangulation.h
View File

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