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

Formatting only

release/4.3a0
Frank Dellaert 2009-08-29 01:22:37 +00:00
parent 8d54086f92
commit ea0e16caf5
1 changed files with 101 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
cpp/Pose3.h
View File

@ -19,28 +19,39 @@ private:
Point3 t_; Point3 t_;
public: public:
Pose3() {} // default is origin Pose3() {
Pose3(const Pose3& pose):R_(pose.R_),t_(pose.t_) {} } // default is origin
Pose3(const Rot3& R, const Point3& t): R_(R), t_(t) {} Pose3(const Pose3& pose) :
R_(pose.R_), t_(pose.t_) {
}
Pose3(const Rot3& R, const Point3& t) :
R_(R), t_(t) {
}
/** constructor from 4*4 matrix */ /** constructor from 4*4 matrix */
Pose3(const Matrix &T) :R_( T(0,0), T(0,1), T(0,2), Pose3(const Matrix &T) :
T(1,0), T(1,1), T(1,2), R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
T(2,0), T(2,1), T(2,2) ), T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
t_( T(0,3), T(1,3), T(2,3) ) {} }
/** constructor from 12D vector */ /** constructor from 12D vector */
Pose3(const Vector &V) :R_( V(0), V(3), V(6), Pose3(const Vector &V) :
V(1), V(4), V(7), R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), t_(V(9), V(10),
V(2), V(5), V(8) ), V(11)) {
t_( V(9), V(10), V(11) ) {} }
const Rot3& rotation() const {return R_;} const Rot3& rotation() const {
return R_;
}
const Point3& translation() const {return t_;} const Point3& translation() const {
return t_;
}
/** return DOF, dimensionality of tangent space = 6 */ /** return DOF, dimensionality of tangent space = 6 */
size_t dim() const { return 6;} size_t dim() const {
return 6;
}
/** Given 6-dim tangent vector, create new pose */ /** Given 6-dim tangent vector, create new pose */
Pose3 exmap(const Vector& d) const; Pose3 exmap(const Vector& d) const;
@ -87,8 +98,7 @@ private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version) {
{
ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(t_);
} }