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

moved some implementations to cpp to avoid some weird conflict involving stack

release/4.3a0
Frank Dellaert 2009-08-30 04:51:46 +00:00
parent 6311dd0147
commit 34d80b6a2b
2 changed files with 24 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
cpp/Pose3.cpp
View File

@ -5,6 +5,8 @@
#include "Pose3.h" #include "Pose3.h"
using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
@ -12,6 +14,25 @@ Pose3 Pose3::exmap(const Vector& v) const {
return Pose3(R_.exmap(sub(v,0,3)), t_.exmap(sub(v,3,6))); return Pose3(R_.exmap(sub(v,0,3)), t_.exmap(sub(v,3,6)));
} }
/* ************************************************************************* */
Vector Pose3::vector() const {
Vector r = R_.vector(), t = t_.vector();
return concatVectors(2, &r, &t);
}
/* ************************************************************************* */
Matrix Pose3::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);
}
/* ************************************************************************* */
void Pose3::print(const string& s) const {
R_.print(s + ".R");
t_.print(s + ".t");
}
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p) { Point3 transform_from(const Pose3& pose, const Point3& p) {
return pose.R_ * p + pose.t_; return pose.R_ * p + pose.t_;

16
cpp/Pose3.h
View File

@ -65,23 +65,13 @@ namespace gtsam {
} }
/** return 12D vectorized form (column-wise) */ /** return 12D vectorized form (column-wise) */
Vector vector() const { Vector vector() const;
Vector r = R_.vector(), t = t_.vector();
return concatVectors(2, &r, &t);
}
/** convert to 4*4 matrix */ /** convert to 4*4 matrix */
Matrix matrix() const { 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 */ /** print with optional string */
void print(const std::string& s = "") const { void print(const std::string& s = "") const;
R_.print(s + ".R");
t_.print(s + ".t");
}
/** transforms */ /** transforms */
Pose3 transformPose_to(const Pose3& transform) const; Pose3 transformPose_to(const Pose3& transform) const;