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

Progress on compilation

release/4.3a0
Paul Furgale 2014-12-12 17:02:15 +01:00
parent 91efa7f2a1
commit 02ceb1366b
3 changed files with 55 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/concepts.h
View File

@ -169,7 +169,9 @@ struct LieGroup {
return m.inverse(H); return m.inverse(H);
} }
static const ManifoldType identity = ManifoldType::Identity(); static ManifoldType Identity() {
return ManifoldType::identity();
}
static TangentVector Logmap(const ManifoldType& m) { static TangentVector Logmap(const ManifoldType& m) {
return ManifoldType::Logmap(m); return ManifoldType::Logmap(m);

18
gtsam/geometry/Rot3.h
View File

@ -66,6 +66,8 @@ namespace gtsam {
#endif #endif
public: public:
/// The intrinsic dimension of this manifold.
enum { dimension = 3 };
/// @name Constructors and named constructors /// @name Constructors and named constructors
/// @{ /// @{
@ -470,20 +472,6 @@ namespace gtsam {
*/ */
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A); GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_group<Rot3> : public boost::true_type{ struct traits_x<Rot3> : public internal::LieGroup<Rot3> {};
};
template<>
struct GTSAM_EXPORT is_manifold<Rot3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Rot3> : public boost::integral_constant<int, 3>{
};
}
} }

98
gtsam/geometry/SO3.cpp
View File

@ -44,54 +44,54 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) {
return R; return R;
} }
//
namespace lie_group { //namespace lie_group {
/// simply convert omega to axis/angle representation ///// simply convert omega to axis/angle representation
template <> //template <>
SO3 expmap<SO3>(const Eigen::Ref<const Vector3>& omega) { //SO3 expmap<SO3>(const Eigen::Ref<const Vector3>& omega) {
if (omega.isZero()) // if (omega.isZero())
return SO3::Identity(); // return SO3::Identity();
else { // else {
double angle = omega.norm(); // double angle = omega.norm();
return Rodrigues(angle, omega / angle); // return Rodrigues(angle, omega / angle);
} // }
} //}
//
template <> //template <>
Vector3 logmap<SO3>(const SO3& R) { //Vector3 logmap<SO3>(const SO3& R) {
//
// note switch to base 1 // // note switch to base 1
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); // const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); // const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); // const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
//
// Get trace(R) // // Get trace(R)
double tr = R.trace(); // double tr = R.trace();
//
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // // we do something special
if (std::abs(tr + 1.0) < 1e-10) { // if (std::abs(tr + 1.0) < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10) // if (std::abs(R33 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); // return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10) // else if (std::abs(R22 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); // return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else // else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit // // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); // return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else { // } else {
double magnitude; // double magnitude;
double tr_3 = tr - 3.0; // always negative // double tr_3 = tr - 3.0; // always negative
if (tr_3 < -1e-7) { // if (tr_3 < -1e-7) {
double theta = acos((tr - 1.0) / 2.0); // double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta)); // magnitude = theta / (2.0 * sin(theta));
} else { // } else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) // // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0; // magnitude = 0.5 - tr_3 * tr_3 / 12.0;
} // }
return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); // return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
} // }
} //}
} // end namespace lie_group //} // end namespace lie_group
} // end namespace gtsam } // end namespace gtsam