Replace rotAveraging with gtsam::FindKarcherMean.
							parent
							
								
									c80cfe068f
								
							
						
					
					
						commit
						e94aae10bf
					
				|  | @ -140,23 +140,6 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) { | |||
|   return Similarity3(aRb, aTb, s); | ||||
| } | ||||
| 
 | ||||
| //  Use the geodesic L2 mean to solve the mean of rotations,
 | ||||
| //  Refer to: http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf (on page 18)
 | ||||
| Rot3 Similarity3::rotationAveraging(const std::vector<Rot3>& rotations, double error) { | ||||
|   Rot3 R = rotations[0]; | ||||
|   const size_t n = rotations.size(); | ||||
|   Vector3 r; | ||||
|   r << 0, 0, 0; | ||||
|   while (1) { | ||||
|     for (const Rot3 R_i : rotations) { | ||||
|       r += Rot3::Logmap(R.inverse().compose(R_i)); | ||||
|     } | ||||
|     r /= n; | ||||
|     if (r.norm() < error) return R; | ||||
|     R = R.compose(Rot3::Expmap(r)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) { | ||||
|   const size_t n = abPosePairs.size(); | ||||
|   if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses");  // we need at least two pairs
 | ||||
|  | @ -172,7 +155,7 @@ Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) { | |||
|   const double f = 1.0 / n; | ||||
|   aCentroid *= f; | ||||
|   bCentroid *= f; | ||||
|   Rot3 aRb = Similarity3::rotationAveraging(rotationList); | ||||
|   const Rot3 aRb = FindKarcherMean<Rot3>(rotationList); | ||||
| 
 | ||||
|   // Calculate scale
 | ||||
|   double x = 0; | ||||
|  |  | |||
|  | @ -23,6 +23,9 @@ | |||
| #include <gtsam/base/Lie.h> | ||||
| #include <gtsam/base/Manifold.h> | ||||
| #include <gtsam_unstable/dllexport.h> | ||||
| #include <gtsam/slam/KarcherMeanFactor-inl.h> | ||||
| #include <gtsam/slam/KarcherMeanFactor.h> | ||||
| 
 | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -112,12 +115,7 @@ public: | |||
|    *  Create Similarity3 by aligning at least three point pairs | ||||
|    */ | ||||
|   GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs); | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Calculate the average rotation from a list of rotations | ||||
|    */ | ||||
|   GTSAM_UNSTABLE_EXPORT static Rot3 rotationAveraging(const std::vector<Rot3>& rotations, double error = 1e-10); | ||||
| 
 | ||||
|    | ||||
|   /**
 | ||||
|    *  Create Similarity3 by aligning at least two pose pairs | ||||
|    */ | ||||
|  |  | |||
|  | @ -321,15 +321,6 @@ TEST(Similarity3, AlignPoint3_3) { | |||
|   EXPECT(assert_equal(expected_aSb, actual_aSb)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| // Rotation Averaging
 | ||||
| TEST(Similarity3, RotationAveraging) { | ||||
|   Rot3 expected = Rot3::Rx(90 * degree); | ||||
|   vector<Rot3> rotations{Rot3(), Rot3::Rx(90 * degree), Rot3::Rx(180 * degree)}; | ||||
|   Rot3 actual = Similarity3::rotationAveraging(rotations); | ||||
|   EXPECT(assert_equal(expected, actual)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| // Align with Pose3 Pairs
 | ||||
| TEST(Similarity3, AlignPose3) { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue