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

15
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;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -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) {