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

internal::LieGroup needed if you also want Testable traits

release/4.3a0
dellaert 2015-05-25 22:04:04 -07:00
parent 42b0f0f4d4
commit 9d522c72f3
8 changed files with 12 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose2.h
View File

@ -291,10 +291,10 @@ typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs); GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
template<> template<>
struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {}; struct traits<Pose2> : public internal::LieGroup<Pose2> {};
template<> template<>
struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {}; struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
} // namespace gtsam } // namespace gtsam

4
gtsam/geometry/Pose3.h
View File

@ -324,10 +324,10 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
typedef std::vector<Pose3> Pose3Vector; typedef std::vector<Pose3> Pose3Vector;
template<> template<>
struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {}; struct traits<Pose3> : public internal::LieGroup<Pose3> {};
template<> template<>
struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {}; struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
} // namespace gtsam } // namespace gtsam

4
gtsam/geometry/Rot2.h
View File

@ -208,9 +208,9 @@ namespace gtsam {
}; };
template<> template<>
struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {}; struct traits<Rot2> : public internal::LieGroup<Rot2> {};
template<> template<>
struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {}; struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
} // gtsam } // gtsam

4
gtsam/geometry/Rot3.h
View File

@ -463,9 +463,9 @@ namespace gtsam {
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A); GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
template<> template<>
struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {}; struct traits<Rot3> : public internal::LieGroup<Rot3> {};
template<> template<>
struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {}; struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
} }

4
gtsam/geometry/SO3.h
View File

@ -129,11 +129,11 @@ public:
}; };
template<> template<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> { struct traits<SO3> : public internal::LieGroup<SO3> {
}; };
template<> template<>
struct traits<const SO3> : public internal::LieGroupTraits<SO3> { struct traits<const SO3> : public internal::LieGroup<SO3> {
}; };
} // end namespace gtsam } // end namespace gtsam

24
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -74,30 +74,6 @@ Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) {
return (Vector9() << Lx, Lv).finished(); return (Vector9() << Lx, Lv).finished();
} }
/* ************************************************************************* */
PoseRTV PoseRTV::retract(const Vector& v,
ChartJacobian Horigin,
ChartJacobian Hv) const {
if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED;
assert(v.size() == 9);
// First order approximation
Pose3 newPose = pose().retract(sub(v, 0, 6));
Velocity3 newVel = velocity() + rotation() * Point3(sub(v, 6, 9));
return PoseRTV(newPose, newVel);
}
/* ************************************************************************* */
Vector PoseRTV::localCoordinates(const PoseRTV& p1,
ChartJacobian Horigin,
ChartJacobian Hp) const {
if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED;
const Pose3& x0 = pose(), &x1 = p1.pose();
// First order approximation
Vector6 poseLogmap = x0.localCoordinates(x1);
Vector3 lv = rotation().unrotate(p1.velocity() - velocity()).vector();
return (Vector(9) << poseLogmap, lv).finished();
}
/* ************************************************************************* */ /* ************************************************************************* */
PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
PoseRTV PoseRTV::inverse(ChartJacobian H1) const { PoseRTV PoseRTV::inverse(ChartJacobian H1) const {

2
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -223,6 +223,6 @@ private:
template<> template<>
struct traits<PoseRTV> : public internal::LieGroupTraits<PoseRTV> {}; struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
} // \namespace gtsam } // \namespace gtsam

2
gtsam_unstable/geometry/Similarity3.h
View File

@ -148,5 +148,5 @@ public:
}; };
template<> template<>
struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {}; struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
} }