🦄 refactor(pose type): matrix4f as tranform

SurfaceMan · Mar 26, 2022 · 8c6578c · 8c6578c
1 parent 3c085bc
commit 8c6578c
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 15 deletions.
diff --git a/ppf.cpp b/ppf.cpp
@@ -364,7 +364,7 @@ void Detector::matchScene(ppf::PointCloud &scene, std::vector<Eigen::Matrix4f> &
  using Target = std::pair<float, Eigen::Matrix4f>;
  std::vector<Target> result; //[score, pose]
  for (auto &p : cluster2) {
- auto pose = p.pose.matrix();
+ auto pose = p.pose;
  auto score = p.numVotes;
 
  if (score < voteThreshold)

diff --git a/type.cpp b/type.cpp
@@ -68,19 +68,19 @@ Pose::Pose(float votes)
 void Pose::updatePose(const Eigen::Matrix4f &newPose) {
  pose = newPose;
 
- auto rMatrix = pose.rotation();
- r = rMatrix;
- q = rMatrix;
+ Eigen::Matrix3f rMatrix = pose.topLeftCorner(3, 3);
+ r  = rMatrix;
+ q  = rMatrix;
 }
 
 void Pose::updatePoseT(const Eigen::Vector3f &t) {
- pose.translation() = t;
+ pose.topRightCorner(3, 1) = t;
 }
 
 void Pose::updatePoseQuat(const Eigen::Quaternionf &q_) {
- q = q_;
- r = q.matrix();
- pose.linear() = q.matrix();
+ q  = q_;
+ r  = q.matrix();
+ pose.topLeftCorner(3, 3) = q.matrix();
 }
 
 TrainParam::TrainParam(float featDistanceStepRel_, int featAngleResolution_,

diff --git a/type.h b/type.h
@@ -66,7 +66,7 @@ struct BoxGrid {
 struct Pose {
 public:
  float numVotes;
- Eigen::Affine3f pose;
+ Eigen::Matrix4f pose;
 
  Eigen::AngleAxisf r;
  Eigen::Quaternionf q;

diff --git a/util.cpp b/util.cpp
@@ -169,7 +169,7 @@ std::vector<Pose> sortPoses(std::vector<Pose> poseList) {
 }
 
 bool comparePose(const Pose &p1, const Pose &p2, float distanceThreshold, float angleThreshold) {
- float d = (p1.pose.translation() - p2.pose.translation()).norm();
+ float d = (p1.pose.topRightCorner(3, 1) - p2.pose.topRightCorner(3, 1)).norm();
  float phi = std::abs(p1.r.angle() - p2.r.angle());
  return (d < distanceThreshold && phi < angleThreshold);
 }
@@ -198,12 +198,10 @@ std::vector<std::vector<Pose>> clusterPose(const std::vector<Pose> &poseList,
 }
 
 std::vector<Pose> clusterPose2(std::vector<Pose> &poseList, Eigen::Vector3f &pos, float threshold) {
- Eigen::Vector4f p(pos.x(), pos.y(), pos.z(), 1);
-
- std::vector<Eigen::Vector4f> trans;
+ std::vector<Eigen::Vector3f> trans;
  trans.reserve(poseList.size());
  for (auto &pose : poseList) {
- trans.emplace_back(pose.pose.matrix() * p);
+ trans.emplace_back(pose.pose.topLeftCorner(3, 3) * pos + pose.pose.topRightCorner(3, 1));
  }
 
  float squaredThreshold = threshold * threshold;
@@ -265,7 +263,7 @@ std::vector<Pose> avgClusters(const std::vector<std::vector<Pose>> &clusters) {
  std::vector<Eigen::Quaternionf> qs;
  float votes = 0;
  for (auto &pose : cluster) {
- p += pose.pose.translation();
+ p += pose.pose.topRightCorner(3, 1);
  votes += pose.numVotes;
  qs.push_back(pose.q);
  }