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

Use matrix operations, silly me!

release/4.3a0
Frank Dellaert 2022-03-26 18:53:44 -04:00
parent bbf4e48d3c
commit 56b83af6b0
1 changed files with 6 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/geometry/Pose3.cpp
View File

@ -358,11 +358,8 @@ Matrix Pose3::transformFrom(const Matrix& points) const {
if (points.rows() != 3) { if (points.rows() != 3) {
throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix."); throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix.");
} }
Matrix result(3, points.cols()); const Matrix3 R = R_.matrix();
for (size_t j=0; j < points.cols(); j++) { return (R * points).colwise() + t_; // Eigen broadcasting!
result.col(j) = transformFrom(Point3(points.col(j)));
}
return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -389,11 +386,8 @@ Matrix Pose3::transformTo(const Matrix& points) const {
if (points.rows() != 3) { if (points.rows() != 3) {
throw std::invalid_argument("Pose3:transformTo expects 3*N matrix."); throw std::invalid_argument("Pose3:transformTo expects 3*N matrix.");
} }
Matrix result(3, points.cols()); const Matrix3 Rt = R_.transpose();
for (size_t j=0; j < points.cols(); j++) { return Rt * (points.colwise() - t_); // Eigen broadcasting!
result.col(j) = transformTo(Point3(points.col(j)));
}
return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -453,7 +447,7 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) { boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
const size_t n = abPointPairs.size(); const size_t n = abPointPairs.size();
if (n < 3) { if (n < 3) {
return boost::none; // we need at least three pairs return boost::none; // we need at least three pairs
} }
// calculate centroids // calculate centroids
@ -484,6 +478,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
} }
return Pose3::Align(abPointPairs); return Pose3::Align(abPointPairs);
} }
boost::optional<Pose3> align(const Point3Pairs &baPointPairs) { boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
Point3Pairs abPointPairs; Point3Pairs abPointPairs;
for (const Point3Pair &baPair : baPointPairs) { for (const Point3Pair &baPair : baPointPairs) {