Addressed Duy's comments
							parent
							
								
									e971c933d2
								
							
						
					
					
						commit
						520bb970f3
					
				|  | @ -36,15 +36,18 @@ typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) { | |||
| 
 | ||||
| template <int N> | ||||
| SO<N> SO<N>::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) { | ||||
|   if (H) throw std::runtime_error("SO<N>::Retract jacobian not implemented."); | ||||
|   const Matrix X = Hat(xi / 2.0); | ||||
|   size_t n = AmbientDim(xi.size()); | ||||
|   const auto I = Eigen::MatrixXd::Identity(n, n); | ||||
|   // https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf
 | ||||
|   return SO((I + X) * (I - X).inverse()); | ||||
| } | ||||
| 
 | ||||
| template <int N> | ||||
| typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R, | ||||
|                                                           ChartJacobian H) { | ||||
|   if (H) throw std::runtime_error("SO<N>::Local jacobian not implemented."); | ||||
|   const size_t n = R.rows(); | ||||
|   const auto I = Eigen::MatrixXd::Identity(n, n); | ||||
|   const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); | ||||
|  | @ -87,9 +90,11 @@ typename SO<N>::VectorN2 SO<N>::vec( | |||
|   VectorN2 X(n2); | ||||
|   X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1); | ||||
| 
 | ||||
|   // If requested, calculate H as (I \oplus Q) * P
 | ||||
|   // If requested, calculate H as (I \oplus Q) * P,
 | ||||
|   // where Q is the N*N rotation matrix, and P is calculated below.
 | ||||
|   if (H) { | ||||
|     // Calculate P matrix of vectorized generators
 | ||||
|     // TODO(duy): Should we refactor this as the jacobian of Hat?
 | ||||
|     const size_t d = dim(); | ||||
|     Matrix P(n2, d); | ||||
|     for (size_t j = 0; j < d; j++) { | ||||
|  |  | |||
|  | @ -11,8 +11,7 @@ | |||
| 
 | ||||
| /**
 | ||||
|  * @file    SOn.h | ||||
|  * @brief   n*n matrix representation of SO(n), template on N, which can be | ||||
|  * Eigen::Dynamic | ||||
|  * @brief   N*N matrix representation of SO(N). N can be Eigen::Dynamic | ||||
|  * @author  Frank Dellaert | ||||
|  * @date    March 2019 | ||||
|  */ | ||||
|  | @ -43,7 +42,7 @@ constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } | |||
| 
 | ||||
| /**
 | ||||
|  * Manifold of special orthogonal rotation matrices SO<N>. | ||||
|  * Template paramater N can be a fizxed integer or can be Eigen::Dynamic | ||||
|  * Template paramater N can be a fixed integer or can be Eigen::Dynamic | ||||
|  */ | ||||
| template <int N> | ||||
| class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> { | ||||
|  | @ -53,7 +52,6 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> { | |||
|   using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>; | ||||
|   using MatrixDD = Eigen::Matrix<double, dimension, dimension>; | ||||
| 
 | ||||
|  public: | ||||
|   EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||||
| 
 | ||||
|  protected: | ||||
|  | @ -100,7 +98,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> { | |||
| 
 | ||||
|   /// Constructor from AngleAxisd
 | ||||
|   template <int N_ = N, typename = IsSO3<N_>> | ||||
|   SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} | ||||
|   explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} | ||||
| 
 | ||||
|   /// Constructor from axis and angle. Only defined for SO3
 | ||||
|   static SO AxisAngle(const Vector3& axis, double theta); | ||||
|  | @ -117,7 +115,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> { | |||
|   template <int N_ = N, typename = IsDynamic<N_>> | ||||
|   static SO Random(boost::mt19937& rng, size_t n = 0) { | ||||
|     if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); | ||||
|     // This needs to be re-thought!
 | ||||
|     // TODO(frank): This needs to be re-thought!
 | ||||
|     static boost::uniform_real<double> randomAngle(-M_PI, M_PI); | ||||
|     const size_t d = SO::Dimension(n); | ||||
|     Vector xi(d); | ||||
|  |  | |||
|  | @ -647,7 +647,6 @@ TEST(Rot3 , ChartDerivatives) { | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Rot3, ClosestTo) { | ||||
|   // Example top-left of SO(4) matrix not quite on SO(3) manifold
 | ||||
|   Matrix3 M; | ||||
|   M << 0.79067393, 0.6051136, -0.0930814,   //
 | ||||
|       0.4155925, -0.64214347, -0.64324489,  //
 | ||||
|  |  | |||
|  | @ -15,10 +15,12 @@ | |||
|  * @author Frank Dellaert | ||||
|  **/ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/geometry/SO3.h> | ||||
| 
 | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/testLie.h> | ||||
| #include <gtsam/geometry/SO3.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
|  | @ -51,7 +53,6 @@ TEST(SO3, Constructors) { | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(SO3, ClosestTo) { | ||||
|   // Example top-left of SO(4) matrix not quite on SO(3) manifold
 | ||||
|   Matrix3 M; | ||||
|   M << 0.79067393, 0.6051136, -0.0930814,   //
 | ||||
|       0.4155925, -0.64214347, -0.64324489,  //
 | ||||
|  |  | |||
|  | @ -39,6 +39,17 @@ | |||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| // Test dhynamic with n=0
 | ||||
| TEST(SOn, SO0) { | ||||
|   const auto R = SOn(0); | ||||
|   EXPECT_LONGS_EQUAL(0, R.rows()); | ||||
|   EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); | ||||
|   EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); | ||||
|   EXPECT_LONGS_EQUAL(0, R.dim()); | ||||
|   EXPECT_LONGS_EQUAL(-1, traits<SOn>::GetDimension(R)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(SOn, SO5) { | ||||
|   const auto R = SOn(5); | ||||
|  |  | |||
|  | @ -12,7 +12,7 @@ | |||
| /*
 | ||||
|  * @file KarcherMeanFactor.h | ||||
|  * @author Frank Dellaert | ||||
|  * @date March 2019ƒ | ||||
|  * @date March 2019 | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
|  | @ -26,18 +26,18 @@ | |||
| namespace gtsam { | ||||
| /**
 | ||||
|  * Optimize for the Karcher mean, minimizing the geodesic distance to each of | ||||
|  * the given rotations, ,by constructing a factor graph out of simple | ||||
|  * the given rotations, by constructing a factor graph out of simple | ||||
|  * PriorFactors. | ||||
|  */ | ||||
| template <class T> | ||||
| T FindKarcherMean(const std::vector<T>& rotations); | ||||
| 
 | ||||
| /**
 | ||||
|  * The KarcherMeanFactor creates a constraint on all SO(3) variables with | ||||
|  * The KarcherMeanFactor creates a constraint on all SO(n) variables with | ||||
|  * given keys that the Karcher mean (see above) will stay the same. Note the | ||||
|  * mean itself is irrelevant to the constraint and is not a parameter: the | ||||
|  * constraint is implemented as enforcing that the sum of local updates is | ||||
|  * equal to zero, hence creating a rank-3 constraint. Note it is implemented as | ||||
|  * equal to zero, hence creating a rank-dim constraint. Note it is implemented as | ||||
|  * a soft constraint, as typically it is used to fix a gauge freedom. | ||||
|  * */ | ||||
| template <class T> | ||||
|  | @ -67,4 +67,4 @@ class KarcherMeanFactor : public NonlinearFactor { | |||
|   } | ||||
| }; | ||||
| // \KarcherMeanFactor
 | ||||
| }  // namespace gtsam
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue