Final cleanup
							parent
							
								
									8d134fd120
								
							
						
					
					
						commit
						bba4b8731f
					
				|  | @ -25,8 +25,8 @@ | |||
| namespace gtsam { | ||||
| 
 | ||||
| Vector4 triangulateHomogeneousDLT( | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&  | ||||
|     projection_matrices, | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& | ||||
|         projection_matrices, | ||||
|     const Point2Vector& measurements, double rank_tol) { | ||||
|   // number of cameras
 | ||||
|   size_t m = projection_matrices.size(); | ||||
|  | @ -56,8 +56,8 @@ Vector4 triangulateHomogeneousDLT( | |||
| } | ||||
| 
 | ||||
| Vector4 triangulateHomogeneousDLT( | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&  | ||||
|     projection_matrices, | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& | ||||
|         projection_matrices, | ||||
|     const std::vector<Unit3>& measurements, double rank_tol) { | ||||
|   // number of cameras
 | ||||
|   size_t m = projection_matrices.size(); | ||||
|  | @ -68,7 +68,7 @@ Vector4 triangulateHomogeneousDLT( | |||
|   for (size_t i = 0; i < m; i++) { | ||||
|     size_t row = i * 2; | ||||
|     const Matrix34& projection = projection_matrices.at(i); | ||||
|     const Point3& p =  | ||||
|     const Point3& p = | ||||
|         measurements.at(i) | ||||
|             .point3();  // to get access to x,y,z of the bearing vector
 | ||||
| 
 | ||||
|  | @ -130,7 +130,7 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses, | |||
|     // Note: Setting q_i = 1.0 gives same results as DLT.
 | ||||
|     const double q_i = num_i / (measurementNoise->sigma() * den_i); | ||||
| 
 | ||||
|     const Matrix23 coefficientMat =  | ||||
|     const Matrix23 coefficientMat = | ||||
|         q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * | ||||
|         wTi.rotation().matrix().transpose(); | ||||
| 
 | ||||
|  | @ -147,8 +147,8 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses, | |||
| } | ||||
| 
 | ||||
| Point3 triangulateDLT( | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&  | ||||
|     projection_matrices, | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& | ||||
|         projection_matrices, | ||||
|     const Point2Vector& measurements, double rank_tol) { | ||||
|   Vector4 v =  | ||||
|       triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); | ||||
|  | @ -161,7 +161,7 @@ Point3 triangulateDLT( | |||
|     projection_matrices, | ||||
|     const std::vector<Unit3>& measurements, double rank_tol) { | ||||
|   // contrary to previous triangulateDLT, this is now taking Unit3 inputs
 | ||||
|   Vector4 v =  | ||||
|   Vector4 v = | ||||
|       triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); | ||||
|   // Create 3D point from homogeneous coordinates
 | ||||
|   return Point3(v.head<3>() / v[3]); | ||||
|  | @ -174,7 +174,7 @@ Point3 triangulateDLT( | |||
|  * @param landmarkKey to refer to landmark | ||||
|  * @return refined Point3 | ||||
|  */ | ||||
| Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,  | ||||
| Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, | ||||
|                 Key landmarkKey) { | ||||
|   // Maybe we should consider Gauss-Newton?
 | ||||
|   LevenbergMarquardtParams params; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue