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

feature: add concept test for similarity3

release/4.3a0
zhaoyang 2015-06-26 16:01:26 -04:00
parent 400a17d9ab
commit 12f9b413ff
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -189,12 +189,19 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
double lambda = log(s.s_); double lambda = log(s.s_);
Vector7 result; Vector7 result;
result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda; result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda;
if (Hm) {
// incomplete
}
return result; return result;
} }
Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
Vector3 w(v.head<3>()); Vector3 w(v.head<3>());
double lambda = v[6]; double lambda = v[6];
if (Hm) {
Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>());
// incomplete
}
return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda)); return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda));
} }

7
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -49,6 +49,13 @@ static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1);
// Simpler transform // Simpler transform
Similarity3 T4(Rot3(), Point3(1, 1, 0), 2); Similarity3 T4(Rot3(), Point3(1, 1, 0), 2);
//******************************************************************************
TEST(Similarity3, concepts) {
BOOST_CONCEPT_ASSERT((IsGroup<Similarity3 >));
BOOST_CONCEPT_ASSERT((IsManifold<Similarity3 >));
BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity3 >));
}
//****************************************************************************** //******************************************************************************
TEST(Similarity3, Constructors) { TEST(Similarity3, Constructors) {
Similarity3 test; Similarity3 test;