teratail
回答率
85.47%
質問するログイン新規登録
トップに関する質問二つの点群の対応する点間の距離を求めたいです。

編集履歴

質問編集履歴

2

書き損じ

2019/09/30 02:09

投稿

wakasagi
wakasagi

スコア7

test CHANGED
File without changes
test CHANGED
@@ -280,7 +280,7 @@
280
280
 
281
281
  for(int i = 0; i <= transedCloud->size(); i++){
282
282
 
283
- 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);
283
+ 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);
284
284
 
285
285
  }
286
286
 

1

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

2019/09/30 02:09

投稿

wakasagi
wakasagi

スコア7

test CHANGED
@@ -1 +1 @@
1
- 点群の位置合わせを行うICPアルゴリズムを実現するコードを自分で書こうとしています。その中でどうしても解消できないエラーが五個あるので、何かアドバイスをいただけると助かります。
1
+ 二つの点群の対応する点間の距離を求めたいです。
test CHANGED
@@ -4,7 +4,7 @@
4
4
 
5
5
  点群の位置合わせを行うICPアルゴリズムを自分で実装しようと考えています。
6
6
 
7
- 現在発生しているエラーをどうしても解消できず困っています。
7
+ 二つの点群の対応する点の距離を求めたいのですが、なかなかうまくいきません。
8
8
 
9
9
  なにかアドバイスをいただけると助かります。
10
10
 
@@ -16,35 +16,13 @@
16
16
 
17
17
  ```
18
18
 
19
- 36行目 doesConverged 内の42行目 pointDistance　に次のエラー
20
-
21
- 　no matching function for call 'pointDistance(pcl::PointXYZ&, __gnu_cxx::__alloc_traits<std::allocator<int>>::value_type&)' GCC
22
-
23
-
24
-
25
- main 内126行目 while(doesConverged..... の文に次のエラー
26
-
27
- 　could not convert 'affine' from 'Eigen::Affine3d {aka Eigen::Transform<double, 3, 2>}' to 'std::vector<int>' GCC
28
-
29
-
30
-
31
- main 内128行目 pcl::transformPointCloud(..... の文に次のエラー
32
-
33
- 　no matching function for call 'transformPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr&, pcl::PointCloud<pcl::PointXYZ>::Ptr&, Eigen::Affine3d&)' GCC
34
-
35
-
36
-
37
- main 内132行目 weight.push_back(..... の文に次のエラー
38
-
39
- 　invalid use of void expression GCC
40
-
41
-
42
-
43
- main 内134行目 affine = ..... の文に次のエラー
44
-
45
- 　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
46
-
47
-
19
+ main内、while(doesConverged(...　内
20
+
21
+ 　weight.push_back(pow(..... の文
22
+
23
+
24
+
25
+ invalid use of ‘boost::detail::sp_array_access<pcl::PointCloud<pcl::PointXYZ> >::type {aka void}’
48
26
 
49
27
  ```
50
28
 
@@ -106,228 +84,212 @@
106
84
 
107
85
 
108
86
 
109
- template<typename PointT>
110
-
111
- float pointDistance(PointT p[], PointT q[]){
112
-
113
- 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 );
114
-
115
- return pointDistance;
87
+ bool doesConverged(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ> target, Eigen::Affine3d trans){
88
+
89
+ pcl::PointCloud<pcl::PointXYZ> closest;
90
+
91
+
92
+
93
+ pcl::PointCloud<pcl::PointXYZ> transed;
94
+
95
+ pcl::transformPointCloud(*source,transed,trans);
96
+
97
+ float Sum_Distance = 0;
98
+
99
+
100
+
101
+ for(size_t i = 0;i = transed.size ();i++)
102
+
103
+ {
104
+
105
+ 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 );
106
+
107
+ }
108
+
109
+ return Sum_Distance <= CONVERGE_THRESHOLD;
116
110
 
117
111
  }
118
112
 
119
- template<typename PointT>
120
-
121
- float squareDistance(pcl::PointXYZ p[], pcl::PointNormal q[]){
122
-
123
- 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 ));
124
-
125
- return squareDistance;
113
+
114
+
115
+ struct CostFunc
116
+
117
+ {
118
+
119
+ CostFunc(const pcl::PointNormal source, const pcl::PointNormal target, const bool w)
120
+
121
+ : source_(source), target_(target), w_(w) {}
122
+
123
+
124
+
125
+ template <typename T>
126
+
127
+ bool operator()(const T *const p, T *residual, const bool w)
128
+
129
+ {
130
+
131
+ T x = T(source_.x);
132
+
133
+ T y = T(source_.y);
134
+
135
+ T z = T(source_.z);
136
+
137
+
138
+
139
+ pcl::PointXYZ Tb;
140
+
141
+ Tb.x = p[0]*x+p[1]*y+p[2]*z+p[3];
142
+
143
+ Tb.y = p[4]*x+p[5]*y+p[6]*z+p[7];
144
+
145
+ Tb.z = p[8]*x+p[9]*y+p[10]*z+p[11];
146
+
147
+
148
+
149
+ 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 )));
150
+
151
+ return true;
152
+
153
+ }
154
+
155
+
156
+
157
+ private:
158
+
159
+ pcl::PointNormal source_;
160
+
161
+ pcl::PointNormal target_;
162
+
163
+ double w_;
164
+
165
+ };
166
+
167
+
168
+
169
+ Eigen::Matrix3d minimizeTransform(pcl::PointCloud<pcl::PointXYZ>::Ptr source,pcl::PointCloud<pcl::PointXYZ>::Ptr target,std::vector<int> &closest,std::vector<bool> &weight){
170
+
171
+ Eigen::Matrix3d matrix;
172
+
173
+ ceres::Problem problem;
174
+
175
+ std::vector<double> param(9, INITIAL_SURFACE_PARAMETER);
176
+
177
+ for (int i = 0; i < target->size(); ++i)
178
+
179
+ {
180
+
181
+ ceres::CostFunction *cost =
182
+
183
+ new ceres::AutoDiffCostFunction<CostFunc, 1, 9>(new CostFunc(source->at[i], target->at(closest[i]), weight[i]));
184
+
185
+ problem.AddResidualBlock(cost, NULL, param.data());
186
+
187
+ }
188
+
189
+ //最適化のためのオプションを設定
190
+
191
+ ceres::Solver::Options options;
192
+
193
+ options.linear_solver_type = ceres::DENSE_QR;
194
+
195
+ options.minimizer_progress_to_stdout = PRINT_SOLVER_PROGRESS;
196
+
197
+ //最適化過程の情報保持用
198
+
199
+ ceres::Solver::Summary summary;
200
+
201
+ //最適化開始
202
+
203
+ ceres::Solve(options, &problem, &summary);
204
+
205
+
206
+
207
+ matrix << param[0],param[1],param[2],
208
+
209
+ param[3],param[4],param[5],
210
+
211
+ param[6],param[7],param[8];
212
+
213
+ return matrix;
214
+
215
+ }
216
+
217
+
218
+
219
+ int main(){
220
+
221
+ const double Dmax = 0.1;
222
+
223
+ pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud(new pcl::PointCloud<pcl::PointXYZ>);
224
+
225
+ pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud(new pcl::PointCloud<pcl::PointXYZ>);
226
+
227
+ Eigen::Affine3d affine;
228
+
229
+
230
+
231
+ //点群(PCD)の読み込み
232
+
233
+ pcl::io::loadPCDFile("raw_1.pcd", *sourceCloud); //ファイル名は仮
234
+
235
+ pcl::io::loadPCDFile("raw_2.pcd", *targetCloud); //ファイル名は仮
236
+
237
+
238
+
239
+ std::vector<int> closestPoints(sourceCloud->size());
240
+
241
+
242
+
243
+ pcl::KdTreeFLANN<pcl::PointXYZ> tTree;
244
+
245
+ tTree.setInputCloud(targetCloud);
246
+
247
+
248
+
249
+ // アフィン行列の初期化
250
+
251
+ Eigen::Translation<double, 3> offset(0,0,0);
252
+
253
+ //offset = (0,0,0);// 平行移動量
254
+
255
+ Eigen::Quaternion<double> rot(
256
+
257
+ // 回転量
258
+
259
+ Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitX()) *
260
+
261
+ Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitY()) *
262
+
263
+ Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitZ()));
264
+
265
+
266
+
267
+ affine = offset * rot;
268
+
269
+
270
+
271
+ while(doesConverged(sourceCloud,*targetCloud,affine)){
272
+
273
+ pcl::PointCloud<pcl::PointXYZ>::Ptr transedCloud;
274
+
275
+ pcl::transformPointCloud(*sourceCloud,*transedCloud,affine);
276
+
277
+ std::vector<bool> weight;
278
+
279
+ FindClosestPoint(transedCloud, tTree,closestPoints);
280
+
281
+ for(int i = 0; i <= transedCloud->size(); i++){
282
+
283
+ 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);
284
+
285
+ }
286
+
287
+ affine = minimizeTransform(sourceCloud,targetCloud, closestPoints, weight);
288
+
289
+ }
126
290
 
127
291
  }
128
292
 
129
- bool doesConverged(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, std::vector<int> closest, Eigen::Affine3d &trans){
130
-
131
- pcl::PointCloud<pcl::PointXYZ> transed;
132
-
133
- pcl::transformPointCloud(*source,transed,trans);
134
-
135
- float Sum_Distance = 0;
136
-
137
- for(int i = 0;i = transed.size ();i++)
138
-
139
- {
140
-
141
- Sum_Distance += pointDistance( transed[i], closest[i]);
142
-
143
- }
144
-
145
- return Sum_Distance <= CONVERGE_THRESHOLD;
146
-
147
- }
148
-
149
- struct CostFunc
150
-
151
- {
152
-
153
- CostFunc(const pcl::PointNormal source, const pcl::PointNormal target, const bool w)
154
-
155
- : source_(source), target_(target), w_(w) {}
156
-
157
-
158
-
159
- template <typename T>
160
-
161
- bool operator()(const T *const p, T *residual, const bool w,const T *const q)
162
-
163
- {
164
-
165
- float squareDistance(pcl::PointXYZ p[], pcl::PointNormal q[]);//2行確認
166
-
167
- 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 ));
168
-
169
- T x = T(source_.x);
170
-
171
- T y = T(source_.y);
172
-
173
- T z = T(source_.z);
174
-
175
-
176
-
177
- pcl::PointXYZ Tb;
178
-
179
- Tb.x = p[0]*x+p[1]*y+p[2]*z+p[3];
180
-
181
- Tb.y = p[4]*x+p[5]*y+p[6]*z+p[7];
182
-
183
- Tb.z = p[8]*x+p[9]*y+p[10]*z+p[11];
184
-
185
-
186
-
187
- residual[0] = T(w?1:0)*T(squareDistance(Tb,target_));
188
-
189
- return true;
190
-
191
- }
192
-
193
-
194
-
195
- private:
196
-
197
- pcl::PointNormal source_;
198
-
199
- pcl::PointNormal target_;
200
-
201
- double w_;
202
-
203
- };
204
-
205
-
206
-
207
- Eigen::Matrix3d minimizeTransform(pcl::PointCloud<pcl::PointNormal>::Ptr source,pcl::PointCloud<pcl::PointNormal>::Ptr target,std::vector<int> &closest,std::vector<bool> &weight){
208
-
209
- Eigen::Matrix3d matrix;
210
-
211
- ceres::Problem problem;
212
-
213
- std::vector<double> param(9, INITIAL_SURFACE_PARAMETER);
214
-
215
- for (auto i = 0; i < target->size(); ++i)
216
-
217
- {
218
-
219
- ceres::CostFunction *cost =
220
-
221
- new ceres::AutoDiffCostFunction<CostFunc, 1, 9>(new CostFunc(source->at(i), target->at(closest[i]), weight[i]));
222
-
223
- problem.AddResidualBlock(cost, NULL, param.data());
224
-
225
- }
226
-
227
- //最適化のためのオプションを設定
228
-
229
- ceres::Solver::Options options;
230
-
231
- options.linear_solver_type = ceres::DENSE_QR;
232
-
233
- options.minimizer_progress_to_stdout = PRINT_SOLVER_PROGRESS;
234
-
235
- //最適化過程の情報保持用
236
-
237
- ceres::Solver::Summary summary;
238
-
239
- //最適化開始
240
-
241
- ceres::Solve(options, &problem, &summary);
242
-
243
-
244
-
245
- matrix << param[0],param[1],param[2],
246
-
247
- param[3],param[4],param[5],
248
-
249
- param[6],param[7],param[8];
250
-
251
- return matrix;
252
-
253
- }
254
-
255
-
256
-
257
- int main(){
258
-
259
- // 宣言部
260
-
261
- const double Dmax = 0.1;
262
-
263
- pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud(new pcl::PointCloud<pcl::PointXYZ>);
264
-
265
- pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud(new pcl::PointCloud<pcl::PointXYZ>);
266
-
267
- Eigen::Affine3d affine;
268
-
269
-
270
-
271
- //点群(PCD)の読み込み
272
-
273
- pcl::io::loadPCDFile("filename1.pcd", *sourceCloud);
274
-
275
- pcl::io::loadPCDFile("filename2.pcd", *targetCloud);
276
-
277
-
278
-
279
- std::vector<int> closestPoints(sourceCloud->size());
280
-
281
-
282
-
283
- pcl::KdTreeFLANN<pcl::PointXYZ> tTree;
284
-
285
- tTree.setInputCloud(targetCloud);
286
-
287
-
288
-
289
- // アフィン行列の初期化
290
-
291
- Eigen::Translation<double, 3> offset(0,0,0); // 平行移動量
292
-
293
- Eigen::Quaternion<double> rot(
294
-
295
- // 回転量
296
-
297
- Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitX()) *
298
-
299
- Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitY()) *
300
-
301
- Eigen::AngleAxisd(degreeToRadian(0), Eigen::Vector3d::UnitZ()));
302
-
303
-
304
-
305
- affine = offset * rot;
306
-
307
-
308
-
309
- while(doesConverged(sourceCloud,targetCloud,affine)){
310
-
311
- pcl::PointCloud<pcl::PointXYZ>::Ptr transedCloud;
312
-
313
- pcl::transformPointCloud(sourceCloud,transedCloud,affine);
314
-
315
- std::vector<bool> weight;
316
-
317
- FindClosestPoint(transedCloud, tTree,closestPoints);
318
-
319
- for(int i = 0; i <= transedCloud->size(); i++){
320
-
321
- weight.push_back(pointDistance(transedCloud[i],targetCloud[closestPoints[i]]) <= 0.1);//Dmax
322
-
323
- }
324
-
325
- affine = minimizeTransform(sourceCloud,targetCloud, closestPoints, weight);
326
-
327
- }
328
-
329
- }
330
-
331
293
 
332
294
 
333
295
  ```
@@ -338,9 +300,9 @@
338
300
 
339
301
 
340
302
 
341
- 変数の型が合っていないと思い変更したりしましたが改善しませんでした。
342
-
343
- まだまだ試せることがあるのかもしれませんでしたが、今の知識では限界でした。
303
+ 以前投稿した時に発生していたエラーは、関数の型を修正することで自己解決しました。
304
+
305
+
344
306
 
345
307
 
346
308