12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added several methods

release/4.3a0
Frank Dellaert 2019-04-16 23:45:27 -04:00 committed by Fan Jiang
parent 19315cc3f3
commit 6e07636302
3 changed files with 159 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
gtsam/geometry/SO3.cpp
View File

@ -20,14 +20,32 @@
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h>
#include <Eigen/SVD>
#include <cmath>
#include <limits>
#include <iostream>
namespace gtsam {
/* ************************************************************************* */
namespace so3 {
Matrix99 Dcompose(const SO3& R) {
Matrix99 H;
H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), //
I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), //
I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
return H;
}
Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
Matrix3 MR = M * R.matrix();
if (H) *H = Dcompose(R);
return MR;
}
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) {
@ -116,11 +134,44 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
return so3::ExpmapFunctor(axis, theta).expmap();
}
/* ************************************************************************* */
SO3 SO3::ClosestTo(const Matrix3& M) {
Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
const auto& U = svd.matrixU();
const auto& V = svd.matrixV();
const double det = (U * V.transpose()).determinant();
return U * Vector3(1, 1, det).asDiagonal() * V.transpose();
}
/* ************************************************************************* */
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
// See Hartley13ijcv:
// Cost function C(R) = \sum sqr(|R-R_i|_F)
// Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
Matrix3 C_e {Z_3x3};
for (const auto& R_i : rotations) {
C_e += R_i;
}
return ClosestTo(C_e);
}
/* ************************************************************************* */
void SO3::print(const std::string& s) const {
std::cout << s << *this << std::endl;
}
//******************************************************************************
Matrix3 SO3::Hat(const Vector3& xi) { return skewSymmetric(xi); }
/* ************************************************************************* */
Vector3 SO3::Vee(const Matrix3& X) {
Vector3 xi;
xi(0) = -X(1, 2);
xi(1) = X(0, 2);
xi(2) = -X(0, 1);
return xi;
}
/* ************************************************************************* */
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
@ -199,6 +250,27 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
W * W;
}
/* ************************************************************************* */
static Vector9 vec(const SO3& R) { return Eigen::Map<const Vector9>(R.data()); }
static const std::vector<const Matrix3> G({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))});
static const Matrix93 P =
(Matrix93() << vec(G[0]), vec(G[1]), vec(G[2])).finished();
/* ************************************************************************* */
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
const SO3& R = *this;
if (H) {
// As Luca calculated (for SO4), this is (I3 \oplus R) * P
*H << R * P.block<3, 3>(0, 0), R * P.block<3, 3>(3, 0),
R * P.block<3, 3>(6, 0);
}
return gtsam::vec(R);
};
/* ************************************************************************* */
} // end namespace gtsam

66
gtsam/geometry/SO3.h
View File

@ -38,14 +38,13 @@ class SO3: public Matrix3, public LieGroup<SO3, 3> {
protected:
public:
enum {
dimension = 3
};
enum { N = 3 };
enum { dimension = 3 };
/// @name Constructors
/// @{
/// Constructor from AngleAxisd
/// Default constructor creates identity
SO3() :
Matrix3(I_3x3) {
}
@ -61,9 +60,15 @@ public:
Matrix3(angleAxis) {
}
/// Static, named constructor TODO think about relation with above
/// Static, named constructor. TODO(dellaert): think about relation with above
GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta);
/// Static, named constructor that finds SO(3) matrix closest to M in Frobenius norm.
static SO3 ClosestTo(const Matrix3& M);
/// Static, named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F).
static SO3 ChordalMean(const std::vector<SO3>& rotations);
/// @}
/// @name Testable
/// @{
@ -85,13 +90,16 @@ public:
/// inverse of a rotation = transpose
SO3 inverse() const {
return this->Matrix3::inverse();
return this->transpose();
}
/// @}
/// @name Lie Group
/// @{
static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix
static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
@ -127,13 +135,53 @@ public:
using LieGroup<SO3, 3>::inverse;
/// @}
/// @name Other methods
/// @{
/// Vectorize
Vector9 vec(OptionalJacobian<9, 3> H = boost::none) const;
/// Return matrix (for wrapper)
const Matrix3& matrix() const { return *this;}
/// @
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
ar & boost::serialization::make_nvp("R11", (*this)(0,0));
ar & boost::serialization::make_nvp("R12", (*this)(0,1));
ar & boost::serialization::make_nvp("R13", (*this)(0,2));
ar & boost::serialization::make_nvp("R21", (*this)(1,0));
ar & boost::serialization::make_nvp("R22", (*this)(1,1));
ar & boost::serialization::make_nvp("R23", (*this)(1,2));
ar & boost::serialization::make_nvp("R31", (*this)(2,0));
ar & boost::serialization::make_nvp("R32", (*this)(2,1));
ar & boost::serialization::make_nvp("R33", (*this)(2,2));
}
};
// This namespace exposes two functors that allow for saving computation when
// exponential map and its derivatives are needed at the same location in so<3>
// The second functor also implements dedicated methods to apply dexp and/or inv(dexp)
namespace so3 {
/**
* Compose general matrix with an SO(3) element.
* We only provide the 9*9 derivative in the first argument M.
*/
Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H = boost::none);
/// (constant) Jacobian of compose wrpt M
Matrix99 Dcompose(const SO3& R);
// Below are two functors that allow for saving computation when exponential map
// and its derivatives are needed at the same location in so<3>. The second
// functor also implements dedicated methods to apply dexp and/or inv(dexp).
/// Functor implementing Exponential map
class GTSAM_EXPORT ExpmapFunctor {
protected:

31
gtsam/geometry/tests/testSO3.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file testQuaternion.cpp
* @file testSO3.cpp
* @brief Unit tests for SO3, as a GTSAM-adapted Lie Group
* @author Frank Dellaert
**/
@ -278,6 +278,35 @@ TEST(SO3, ApplyInvDexp) {
}
}
/* ************************************************************************* */
TEST(SO3, vec) {
const Vector9 expected = Eigen::Map<Vector9>(R2.data());
Matrix actualH;
const Vector9 actual = R2.vec(actualH);
CHECK(assert_equal(expected, actual));
boost::function<Vector9(const SO3&)> f = [](const SO3& Q) {
return Q.vec();
};
const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
CHECK(assert_equal(numericalH, actualH));
}
//******************************************************************************
TEST(Matrix, compose) {
Matrix3 M;
M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
SO3 R = SO3::Expmap(Vector3(1, 2, 3));
const Matrix3 expected = M * R.matrix();
Matrix actualH;
const Matrix3 actual = so3::compose(M, R, actualH);
CHECK(assert_equal(expected, actual));
boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
return so3::compose(M, R);
};
Matrix numericalH = numericalDerivative11(f, M, 1e-2);
CHECK(assert_equal(numericalH, actualH));
}
//******************************************************************************
int main() {
TestResult tr;