編集履歴

質問編集履歴

書き損じ

2019/09/30 02:09

投稿

wakasagi

スコア7

test CHANGED Viewed

File without changes

test CHANGED Viewed

@@ -280,7 +280,7 @@
     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);
+      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);
     }

自己解決した問題の削除と新たな問題の追記

2019/09/30 02:09

投稿

wakasagi

スコア7

test CHANGED Viewed

	@@ -1 +1 @@
1	- 点群の~~位置合わせを行うICPアルゴリズムを実現~~する~~コード~~を~~自分で書こうとして~~い~~ます。その中~~で~~どうしても解消できないエラーが五個あるので、何かアドバイスをいただけると助かりま~~す。
1	+ 二つの点群の対応する点間の距離を求めたいです。

test CHANGED Viewed

@@ -4,7 +4,7 @@
 点群の位置合わせを行うICPアルゴリズムを自分で実装しようと考えています。
-現在発生しているエラーをどうしても解消できず困っています。
+二つの点群の対応する点の距離を求めたいのですが、なかなかうまくいきません。
 なにかアドバイスをいただけると助かります。
@@ -16,35 +16,13 @@
 ```
-36行目 doesConverged 内の42行目 pointDistance　に次のエラー
-　no matching function for call 'pointDistance(pcl::PointXYZ&, __gnu_cxx::__alloc_traits<std::allocator<int>>::value_type&)' GCC
-main 内126行目 while(doesConverged..... の文に次のエラー
-　could not convert 'affine' from 'Eigen::Affine3d {aka Eigen::Transform<double, 3, 2>}' to 'std::vector<int>' GCC
-main 内128行目 pcl::transformPointCloud(..... の文に次のエラー
-　no matching function for call 'transformPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr&, pcl::PointCloud<pcl::PointXYZ>::Ptr&, Eigen::Affine3d&)' GCC
-main 内132行目 weight.push_back(..... の文に次のエラー
-　invalid use of void expression GCC
-main 内134行目 affine = ..... の文に次のエラー
-　could not cnvert 'sourceCloud' from 'pcl::PointCloud<pcl::PointXYZ>::Ptr {aka boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>}' to 'pcl::PointCloud<pcl::PointNormal>::Ptr {aka boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>}' GCC
+main内、while(doesConverged(...　内
+　weight.push_back(pow(..... の文
+invalid use of ‘boost::detail::sp_array_access<pcl::PointCloud<pcl::PointXYZ> >::type {aka void}’
 ```
@@ -106,228 +84,212 @@
-template<typename PointT>
-float pointDistance(PointT p[], PointT q[]){
-  pointDistance = pow( (q.x-p.x)*(q.x-p.x) + (q.y-p.y)*(q.y-p.y) + (q.z-p.z)*(q.z-p.z), 0.5 );
-  return pointDistance;
+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;
 }
-template<typename PointT>
-float squareDistance(pcl::PointXYZ p[], pcl::PointNormal q[]){
-  squareDistance = sqrt(pow( (q->x-p->x)*(q->x-p->x) + (q->y-p->y)*(q->y-p->y) + (q->z-p->z)*(q->z-p->z), 0.5 ));
-  return squareDistance;
+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);
+	}
 }
-bool doesConverged(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, std::vector<int> closest, Eigen::Affine3d &trans){
-  pcl::PointCloud<pcl::PointXYZ> transed;
-  pcl::transformPointCloud(*source,transed,trans);
-	float Sum_Distance = 0;
-	for(int i = 0;i = transed.size ();i++)
-	{
-	  Sum_Distance += pointDistance( transed[i], closest[i]);
-        }
-  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,const T *const q)
-  {
-    float squareDistance(pcl::PointXYZ p[], pcl::PointNormal q[]);//2行確認
-    squareDistance = sqrt(pow( (q->x-p->x)*(q->x-p->x) + (q->y-p->y)*(q->y-p->y) + (q->z-p->z)*(q->z-p->z), 0.5 ));
-    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(squareDistance(Tb,target_));
-    return true;
-  }
-private:
-  pcl::PointNormal source_;
-  pcl::PointNormal target_;
-  double w_;
-};
-Eigen::Matrix3d minimizeTransform(pcl::PointCloud<pcl::PointNormal>::Ptr source,pcl::PointCloud<pcl::PointNormal>::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 (auto 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("filename1.pcd", *sourceCloud);
-  pcl::io::loadPCDFile("filename2.pcd", *targetCloud);
-	std::vector<int> closestPoints(sourceCloud->size());
-  pcl::KdTreeFLANN<pcl::PointXYZ> tTree;
-  tTree.setInputCloud(targetCloud);
-	// アフィン行列の初期化
-    Eigen::Translation<double, 3> 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(pointDistance(transedCloud[i],targetCloud[closestPoints[i]]) <= 0.1);//Dmax
-    }
-    affine = minimizeTransform(sourceCloud,targetCloud, closestPoints, weight);
-	}
-}
 ```
@@ -338,9 +300,9 @@
-変数の型が合っていないと思い変更したりしましたが改善しませんでした。
-まだまだ試せることがあるのかもしれませんでしたが、今の知識では限界でした。
+以前投稿した時に発生していたエラーは、関数の型を修正することで自己解決しました。