🎈 perf(transform): conditional compute

icp.cpp

@@ -135,7 +135,7 @@ IterResult iteration(const PointCloud &srcPC, const PointCloud &dstPC, const KDT
  return IterResult{Eigen::Matrix4f::Identity(), size};
 
  auto p = minimizePointToPlaneMetric(srcPC, dstPC, modelScenePair);
- auto pct = transformPointCloud(extraIndices(srcPC, modelScenePair.first), p);
+ auto pct = transformPointCloud(extraIndices(srcPC, modelScenePair.first), p, false);
 
  std::vector<int> indices2;
  std::vector<float> distances2;
@@ -199,7 +199,7 @@ std::vector<ConvergenceResult> ICP::regist(const PointCloud &src, const PointClo
 
  ConvergenceResult result;
  result.pose = initPose;
- auto srcTmp = initPose.isIdentity() ? src : transformPointCloud(src, initPose);
+ auto srcTmp = initPose.isIdentity() ? src : transformPointCloud(src, initPose, false);
 
  while (result.iterations < criteria_.iterations) {
  auto tmpResult = iteration(srcTmp, dst, kdtree, criteria_.rejectDist);
@@ -237,12 +237,12 @@ std::vector<ConvergenceResult> ICP::regist(const PointCloud &src, const PointClo
  }
 
  if (stop) {
- auto pct = transformPointCloud(src, result.pose);
+ auto pct = transformPointCloud(src, result.pose, false);
  result.inliner = inliner(pct, kdtree, criteria_.inlinerDist);
  break;
  }
 
- srcTmp = transformPointCloud(srcTmp, tmpResult.pose);
+ srcTmp = transformPointCloud(srcTmp, tmpResult.pose, false);
  }
 
  results[ i ] = result;

util.cpp

@@ -102,13 +102,14 @@ BoundingBox computeBoundingBox(const ppf::PointCloud &pc) {
  return {min, max};
 }
 
-PointCloud transformPointCloud(const ppf::PointCloud &pc, const Eigen::Matrix4f &pose) {
- auto size = pc.size();
- auto hasNormal = pc.hasNormal();
+PointCloud transformPointCloud(const ppf::PointCloud &pc, const Eigen::Matrix4f &pose,
+ bool useNormal) {
+ auto size = pc.size();
+ auto doNormal = pc.hasNormal() & useNormal;
 
  PointCloud result;
  result.point.resize(size);
- if (hasNormal)
+ if (doNormal)
  result.normal.resize(size);
 
  auto r = pose.topLeftCorner(3, 3);
@@ -117,7 +118,7 @@ PointCloud transformPointCloud(const ppf::PointCloud &pc, const Eigen::Matrix4f
 #pragma omp parallel for
  for (int i = 0; i < size; i++) {
  result.point[ i ] = r * pc.point[ i ] + t;
- if (hasNormal)
+ if (doNormal)
  result.normal[ i ] = r * pc.normal[ i ];
  }
 

util.h

@@ -27,7 +27,8 @@ void normalizeNormal(ppf::PointCloud &pc);
 
 BoundingBox computeBoundingBox(const ppf::PointCloud &pc);
 
-PointCloud transformPointCloud(const ppf::PointCloud &pc, const Eigen::Matrix4f &pose);
+PointCloud transformPointCloud(const ppf::PointCloud &pc, const Eigen::Matrix4f &pose,
+ bool useNormal = true);
 
 std::vector<Eigen::Vector3f> estimateNormal(const ppf::PointCloud &pc);