more camelcase changes
							parent
							
								
									b52aaeab22
								
							
						
					
					
						commit
						f347209d4c
					
				|  | @ -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)); | ||||
|  |  | |||
|  | @ -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); | ||||
| } | ||||
|  |  | |||
|  | @ -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 { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue