二つの点群の対応する点間の距離を求めたいです。

Question

前提・実現したいこと

点群の位置合わせを行うICPアルゴリズムを自分で実装しようと考えています。
二つの点群の対応する点の距離を求めたいのですが、なかなかうまくいきません。
なにかアドバイスをいただけると助かります。

発生している問題・エラーメッセージ

main内、while(doesConverged(...　内
　weight.push_back(pow(..... の文

invalid use of ‘boost::detail::sp_array_access<pcl::PointCloud<pcl::PointXYZ> >::type {aka void}’

該当のソースコード

constexpr double PI = 3.14159265358979;
constexpr double CONVERGE_THRESHOLD = 1.0;

double degreeToRadian(double degree){
	return  degree * (PI / 180); 
}

float FindClosestPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::KdTreeFLANN<pcl::PointXYZ> &targetTree,std::vector<int> &closestPoints){
  std::vector<float> distanceOfClosestPoints;
  //sourceの各点からもっとも近い点を探すループ
  for(auto p:*source){
    std::vector<int> tmp_i;
    std::vector<float> tmp_f;
    targetTree.nearestKSearch(p,1,tmp_i,tmp_f);
    closestPoints.push_back(tmp_i[0]);
    distanceOfClosestPoints.push_back(tmp_f[0]);
  }
  
  float sum=0;
  for(float d:distanceOfClosestPoints){
    sum += d;
  }
  return sum;
}

bool doesConverged(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ> target,  Eigen::Affine3d trans){
  pcl::PointCloud<pcl::PointXYZ> closest;

  pcl::PointCloud<pcl::PointXYZ> transed;
  pcl::transformPointCloud(*source,transed,trans);
  float Sum_Distance = 0;

  for(size_t i = 0;i = transed.size ();i++)
	{
        Sum_Distance += pow( (target[i].x - transed[i].x) * (target[i].x - transed[i].x) + (target[i].y - transed[i].y) * (target[i].y - transed[i].y) + (target[i].z - transed[i].z) * (target[i].z - transed[i].z), 0.5 );
    }
    return Sum_Distance <= CONVERGE_THRESHOLD;
}

struct CostFunc
{
  CostFunc(const pcl::PointNormal source, const pcl::PointNormal target, const bool w)
      : source_(source), target_(target), w_(w) {}

  template <typename T>
  bool operator()(const T *const p, T *residual, const bool w)
  {
      T x = T(source_.x);
    T y = T(source_.y);
    T z = T(source_.z);

    pcl::PointXYZ Tb;
	  Tb.x = p[0]*x+p[1]*y+p[2]*z+p[3];
	  Tb.y = p[4]*x+p[5]*y+p[6]*z+p[7];
	  Tb.z = p[8]*x+p[9]*y+p[10]*z+p[11];

  	residual[0] = T(w?1:0)*T(sqrt(pow( (target_.x - Tb.x)*(target_.x - Tb.x) + (target_.y - Tb.y) * (target_.y - Tb.y) + (target_.z - Tb.z) * (target_.z - Tb.z), 0.5 )));
    return true;
  }

  private:
    pcl::PointNormal source_;
    pcl::PointNormal target_;
    double w_;
};

Eigen::Matrix3d minimizeTransform(pcl::PointCloud<pcl::PointXYZ>::Ptr source,pcl::PointCloud<pcl::PointXYZ>::Ptr target,std::vector<int> &closest,std::vector<bool> &weight){
  Eigen::Matrix3d matrix;
	ceres::Problem problem;
  std::vector<double> param(9, INITIAL_SURFACE_PARAMETER);
  for (int i = 0; i < target->size(); ++i)
  {
    ceres::CostFunction *cost =
        new ceres::AutoDiffCostFunction<CostFunc, 1, 9>(new CostFunc(source->at[i], target->at(closest[i]), weight[i]));
    problem.AddResidualBlock(cost, NULL, param.data());
  }
  //最適化のためのオプションを設定
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = PRINT_SOLVER_PROGRESS;
  //最適化過程の情報保持用
  ceres::Solver::Summary summary;
  //最適化開始
  ceres::Solve(options, &problem, &summary);

  matrix << param[0],param[1],param[2],
            param[3],param[4],param[5],
            param[6],param[7],param[8];
            return matrix;
  }

int main(){
	const double Dmax = 0.1;
	pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud(new pcl::PointCloud<pcl::PointXYZ>);
	Eigen::Affine3d affine;

  //点群(PCD)の読み込み
  pcl::io::loadPCDFile("raw_1.pcd", *sourceCloud); //ファイル名は仮
  pcl::io::loadPCDFile("raw_2.pcd", *targetCloud); //ファイル名は仮

  std::vector<int> closestPoints(sourceCloud->size());
  
  pcl::KdTreeFLANN<pcl::PointXYZ> tTree;
  tTree.setInputCloud(targetCloud);
	
	// アフィン行列の初期化
    Eigen::Translation<double, 3> offset(0,0,0);
    //offset = (0,0,0);// 平行移動量
    Eigen::Quaternion<double> rot(
	// 回転量
    	Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitZ()));
        
	affine = offset * rot;

  while(doesConverged(sourceCloud,*targetCloud,affine)){
    pcl::PointCloud<pcl::PointXYZ>::Ptr transedCloud;
    pcl::transformPointCloud(*sourceCloud,*transedCloud,affine);
    std::vector<bool> weight;
    FindClosestPoint(transedCloud, tTree,closestPoints);
    for(int i = 0; i <= transedCloud->size(); i++){
      weight.push_back(pow((targetCloud[closestPoints[i]].x - transedCloud[i].x) * (targetCloud[closestPoints[i]].x - transedCloud[i].x) + (targetCloud[closestPoints[i]].y - transedCloud[i].y) * (targetCloud[closestPoints[i]].y - transedCloud[i].y) + (targetCloud[closestPoints[i]].z - transedCloud[i].z) * (targetCloud[closestPoints[i]].z - transedCloud[i].z), 0.5 ) <= 0.1);
    }
    affine = minimizeTransform(sourceCloud,targetCloud, closestPoints, weight);
	}
}

試したこと

以前投稿した時に発生していたエラーは、関数の型を修正することで自己解決しました。

補足情報（FW/ツールのバージョンなど）

開発環境
ubuntu 16.04
visual studio code
C++

使用ツール
point cloud library
Ceres Solver
Eigen

Answer 1

該当行にtypoがありませんか？ .x とすべき箇所が x になっているように見えます。

weight.push_back(
  pow(
    (targetCloud[closestPoints[i]].x - transedCloud[i].x) * (targetCloud[closestPoints[i]]x - transedCloud[i].x) +  // ★
    (targetCloud[closestPoints[i]].y - transedCloud[i].y) * (targetCloud[closestPoints[i]].y - transedCloud[i].y) +
    (targetCloud[closestPoints[i]].z - transedCloud[i].z) * (targetCloud[closestPoints[i]].z - transedCloud[i].z)
    , 0.5 )
  <= 0.1
  );

コーディングの話になりますが、1行に情報を詰め込み過ぎですね。途中計算式を一時変数にいれるか、せめて改行を入れたほうが良いと思います。

Comments

[wakasagi]

回答ありがとうございます。コーディングについてのアドバイスもありがとう御座います。たしかに詰め込み過ぎで見難いです。参考にさせていただきます。
すいません書き損じでした。実際のコードでは .x となっていました。ご指摘ありがとうございます。

[yohhoy]

1行に詰め込んでいる計算式をバラすことで、エラーメッセージ「invalid use of ‘～’」がどのタイミングで出ているかも追いやすくなるかと思います。
ついでに、質問文のエラーメッセージも過度に省略しない方がよいですね。情報が欠落しているように思えます。

[wakasagi]

ありがとうございます。おかげさまで該当のエラーを修復できました。
今後は積極的に改行していこうと思います。