Formatting only
Frank Dellaert
parent
8d54086f92
commit
ea0e16caf5
192
cpp/Pose3.h
192
cpp/Pose3.h
|
|
@ -12,111 +12,121 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/** A 3D pose (R,t) : (Rot3,Point3) */
|
||||
class Pose3 {
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
/** A 3D pose (R,t) : (Rot3,Point3) */
|
||||
class Pose3 {
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
|
||||
public:
|
||||
Pose3() {} // default is origin
|
||||
Pose3(const Pose3& pose):R_(pose.R_),t_(pose.t_) {}
|
||||
Pose3(const Rot3& R, const Point3& t): R_(R), t_(t) {}
|
||||
public:
|
||||
Pose3() {
|
||||
} // default is origin
|
||||
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 */
|
||||
Pose3(const Matrix &T) :R_( T(0,0), T(0,1), T(0,2),
|
||||
T(1,0), T(1,1), T(1,2),
|
||||
T(2,0), T(2,1), T(2,2) ),
|
||||
t_( T(0,3), T(1,3), T(2,3) ) {}
|
||||
/** constructor from 4*4 matrix */
|
||||
Pose3(const Matrix &T) :
|
||||
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
|
||||
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
|
||||
}
|
||||
|
||||
/** constructor from 12D vector */
|
||||
Pose3(const Vector &V) :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(11) ) {}
|
||||
/** constructor from 12D vector */
|
||||
Pose3(const Vector &V) :
|
||||
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(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 */
|
||||
size_t dim() const { return 6;}
|
||||
/** return DOF, dimensionality of tangent space = 6 */
|
||||
size_t dim() const {
|
||||
return 6;
|
||||
}
|
||||
|
||||
/** Given 6-dim tangent vector, create new pose */
|
||||
Pose3 exmap(const Vector& d) const;
|
||||
/** Given 6-dim tangent vector, create new pose */
|
||||
Pose3 exmap(const Vector& d) const;
|
||||
|
||||
/** inverse transformation */
|
||||
Pose3 inverse() const;
|
||||
/** inverse transformation */
|
||||
Pose3 inverse() const;
|
||||
|
||||
/** composition */
|
||||
inline Pose3 operator*(const Pose3& B) const {
|
||||
return Pose3(matrix()*B.matrix());
|
||||
}
|
||||
/** composition */
|
||||
inline Pose3 operator*(const Pose3& B) const {
|
||||
return Pose3(matrix() * B.matrix());
|
||||
}
|
||||
|
||||
/** return 12D vectorized form (column-wise) */
|
||||
Vector vector() const {
|
||||
Vector r = R_.vector(), t = t_.vector();
|
||||
return concatVectors(2,&r,&t);
|
||||
}
|
||||
/** return 12D vectorized form (column-wise) */
|
||||
Vector vector() const {
|
||||
Vector r = R_.vector(), t = t_.vector();
|
||||
return concatVectors(2, &r, &t);
|
||||
}
|
||||
|
||||
/** convert to 4*4 matrix */
|
||||
Matrix matrix() const {
|
||||
const double row4[] = {0,0,0,1};
|
||||
Matrix A34 = Matrix_(3,4,vector()), A14 = Matrix_(1,4,row4);
|
||||
return stack(2, &A34, &A14);
|
||||
}
|
||||
/** convert to 4*4 matrix */
|
||||
Matrix matrix() const {
|
||||
const double row4[] = { 0, 0, 0, 1 };
|
||||
Matrix A34 = Matrix_(3, 4, vector()), A14 = Matrix_(1, 4, row4);
|
||||
return stack(2, &A34, &A14);
|
||||
}
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const {
|
||||
R_.print(s+".R");
|
||||
t_.print(s+".t");
|
||||
}
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const {
|
||||
R_.print(s + ".R");
|
||||
t_.print(s + ".t");
|
||||
}
|
||||
|
||||
/** transforms */
|
||||
Pose3 transformPose_to(const Pose3& transform) const;
|
||||
/** transforms */
|
||||
Pose3 transformPose_to(const Pose3& transform) const;
|
||||
|
||||
/** assert equality up to a tolerance */
|
||||
bool equals(const Pose3& pose, double tol = 1e-9) const;
|
||||
|
||||
/** friends */
|
||||
friend Point3 transform_from(const Pose3& pose, const Point3& p);
|
||||
friend Point3 transform_to(const Pose3& pose, const Point3& p);
|
||||
friend Pose3 composeTransform(const Pose3& current, const Pose3& target);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(R_);
|
||||
ar & BOOST_SERIALIZATION_NVP(t_);
|
||||
}
|
||||
}; // Pose3 class
|
||||
|
||||
/** finds a transform from the current frame to the target frame given two coordinates of the same point */
|
||||
Pose3 composeTransform(const Pose3& current, const Pose3& target);
|
||||
|
||||
/** receives the point in Pose coordinates and transforms it to world coordinates */
|
||||
Point3 transform_from(const Pose3& pose, const Point3& p);
|
||||
Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
|
||||
Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
|
||||
|
||||
/** receives the point in world coordinates and transforms it to Pose coordinates */
|
||||
Point3 transform_to(const Pose3& pose, const Point3& p);
|
||||
Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
|
||||
Matrix Dtransform_to2(const Pose3& pose); // does not depend on p !
|
||||
|
||||
/** direct measurement of a pose */
|
||||
Vector hPose(const Vector& x);
|
||||
|
||||
/**
|
||||
* derivative of direct measurement
|
||||
* 12*6, entry i,j is how measurement error will change
|
||||
*/
|
||||
Matrix DhPose(const Vector& x);
|
||||
|
||||
/** assert equality up to a tolerance */
|
||||
bool equals(const Pose3& pose, double tol=1e-9) const;
|
||||
|
||||
/** friends */
|
||||
friend Point3 transform_from(const Pose3& pose, const Point3& p);
|
||||
friend Point3 transform_to (const Pose3& pose, const Point3& p);
|
||||
friend Pose3 composeTransform(const Pose3& current, const Pose3& target);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version)
|
||||
{
|
||||
ar & BOOST_SERIALIZATION_NVP(R_);
|
||||
ar & BOOST_SERIALIZATION_NVP(t_);
|
||||
}
|
||||
}; // Pose3 class
|
||||
|
||||
/** finds a transform from the current frame to the target frame given two coordinates of the same point */
|
||||
Pose3 composeTransform(const Pose3& current, const Pose3& target);
|
||||
|
||||
/** receives the point in Pose coordinates and transforms it to world coordinates */
|
||||
Point3 transform_from (const Pose3& pose, const Point3& p);
|
||||
Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
|
||||
Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
|
||||
|
||||
/** receives the point in world coordinates and transforms it to Pose coordinates */
|
||||
Point3 transform_to (const Pose3& pose, const Point3& p);
|
||||
Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
|
||||
Matrix Dtransform_to2(const Pose3& pose); // does not depend on p !
|
||||
|
||||
/** direct measurement of a pose */
|
||||
Vector hPose (const Vector& x);
|
||||
|
||||
/**
|
||||
* derivative of direct measurement
|
||||
* 12*6, entry i,j is how measurement error will change
|
||||
*/
|
||||
Matrix DhPose(const Vector& x);
|
||||
|
||||
/** assert equality up to a tolerance */
|
||||
bool assert_equal(const Pose3& A, const Pose3& B, double tol=1e-9);
|
||||
bool assert_equal(const Pose3& A, const Pose3& B, double tol = 1e-9);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue