diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index 1c05eac47..344822c1f 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -36,15 +36,18 @@ typename SO::TangentVector SO::Vee(const MatrixNN& X) { template SO SO::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) { + if (H) throw std::runtime_error("SO::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 typename SO::TangentVector SO::ChartAtOrigin::Local(const SO& R, ChartJacobian H) { + if (H) throw std::runtime_error("SO::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::VectorN2 SO::vec( VectorN2 X(n2); X << Eigen::Map(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++) { diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 06031b093..7954c4d6c 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -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. - * 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 class SO : public LieGroup, internal::DimensionSO(N)> { @@ -53,7 +52,6 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW protected: @@ -100,7 +98,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// Constructor from AngleAxisd template > - 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, internal::DimensionSO(N)> { template > 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 randomAngle(-M_PI, M_PI); const size_t d = SO::Dimension(n); Vector xi(d); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 0365bc659..c95b85f21 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -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, // diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3c5b947ba..b2c781b35 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -15,10 +15,12 @@ * @author Frank Dellaert **/ -#include +#include + #include #include -#include + +#include 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, // diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 384150c52..4e5f12c0c 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -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::GetDimension(R)); +} + //****************************************************************************** TEST(SOn, SO5) { const auto R = SOn(5); diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index d0f2f57db..c098c9665 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -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 T FindKarcherMean(const std::vector& 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 @@ -67,4 +67,4 @@ class KarcherMeanFactor : public NonlinearFactor { } }; // \KarcherMeanFactor -} // namespace gtsam \ No newline at end of file +} // namespace gtsam