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);
}
static const ManifoldType identity = ManifoldType::Identity();
static ManifoldType Identity() {
return ManifoldType::identity();
}
static TangentVector Logmap(const ManifoldType& m) {
return ManifoldType::Logmap(m);

18
gtsam/geometry/Rot3.h
View File

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

98
gtsam/geometry/SO3.cpp
View File

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