27 #ifndef CORELIB_SRC_ICP_LIBPOINTMATCHER_H_
28 #define CORELIB_SRC_ICP_LIBPOINTMATCHER_H_
34 typedef PM::DataPoints
DP;
38 DP pclToDP(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & pclCloud,
bool is2D)
41 typedef DP::Label
Label;
43 typedef DP::View View;
45 if (pclCloud->empty())
53 featLabels.push_back(
Label(
"x", 1));
54 featLabels.push_back(
Label(
"y", 1));
57 featLabels.push_back(
Label(
"z", 1));
59 featLabels.push_back(
Label(
"pad", 1));
61 descLabels.push_back(
Label(
"intensity", 1));
64 DP cloud(featLabels, descLabels, pclCloud->size());
65 cloud.getFeatureViewByName(
"pad").setConstant(1);
68 View
view(cloud.getFeatureViewByName(
"x"));
69 View viewIntensity(cloud.getDescriptorRowViewByName(
"intensity",0));
70 for(
unsigned int i=0;
i<pclCloud->size(); ++
i)
72 view(0,
i) = pclCloud->at(
i).x;
73 view(1,
i) = pclCloud->at(
i).y;
76 view(2,
i) = pclCloud->at(
i).z;
78 viewIntensity(0,
i) = pclCloud->at(
i).intensity;
84 DP pclToDP(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & pclCloud,
bool is2D)
87 typedef DP::Label
Label;
89 typedef DP::View View;
91 if (pclCloud->empty())
99 featLabels.push_back(
Label(
"x", 1));
100 featLabels.push_back(
Label(
"y", 1));
103 featLabels.push_back(
Label(
"z", 1));
105 featLabels.push_back(
Label(
"pad", 1));
107 descLabels.push_back(
Label(
"normals", 3));
108 descLabels.push_back(
Label(
"intensity", 1));
111 DP cloud(featLabels, descLabels, pclCloud->size());
112 cloud.getFeatureViewByName(
"pad").setConstant(1);
115 View
view(cloud.getFeatureViewByName(
"x"));
116 View viewNormalX(cloud.getDescriptorRowViewByName(
"normals",0));
117 View viewNormalY(cloud.getDescriptorRowViewByName(
"normals",1));
118 View viewNormalZ(cloud.getDescriptorRowViewByName(
"normals",2));
119 View viewIntensity(cloud.getDescriptorRowViewByName(
"intensity",0));
120 for(
unsigned int i=0;
i<pclCloud->size(); ++
i)
122 view(0,
i) = pclCloud->at(
i).x;
123 view(1,
i) = pclCloud->at(
i).y;
126 view(2,
i) = pclCloud->at(
i).z;
128 viewNormalX(0,
i) = pclCloud->at(
i).normal_x;
129 viewNormalY(0,
i) = pclCloud->at(
i).normal_y;
130 viewNormalZ(0,
i) = pclCloud->at(
i).normal_z;
131 viewIntensity(0,
i) = pclCloud->at(
i).intensity;
140 typedef DP::Label
Label;
141 typedef DP::Labels
Labels;
142 typedef DP::View View;
152 featLabels.push_back(
Label(
"x", 1));
153 featLabels.push_back(
Label(
"y", 1));
156 featLabels.push_back(
Label(
"z", 1));
158 featLabels.push_back(
Label(
"pad", 1));
162 descLabels.push_back(
Label(
"normals", 3));
166 descLabels.push_back(
Label(
"intensity", 1));
171 DP cloud(featLabels, descLabels, scan.
size());
172 cloud.getFeatureViewByName(
"pad").setConstant(1);
180 View
view(cloud.getFeatureViewByName(
"x"));
181 View viewNormalX(nx!=-1?cloud.getDescriptorRowViewByName(
"normals",0):
view);
182 View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName(
"normals",1):
view);
183 View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName(
"normals",2):
view);
184 View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName(
"intensity",0):
view);
186 for(
int i=0;
i<scan.
size(); ++
i)
188 const float * ptr = scan.
data().ptr<
float>(0,
i);
192 if(hasLocalTransform)
196 cv::Point3f pt(ptr[0], ptr[1], scan.
is2d()?0:ptr[2]);
206 viewIntensity(0, oi) = ptr[offsetI];
215 pt.z=scan.
is2d()?0:ptr[2];
226 viewNormalX(0, oi) = pt.normal_x;
227 viewNormalY(0, oi) = pt.normal_y;
228 viewNormalZ(0, oi) = pt.normal_z;
232 viewIntensity(0, oi) = ptr[offsetI];
239 UWARN(
"Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f",
i, ptr[0], ptr[1], scan.
is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
244 view(0, oi) = ptr[0];
245 view(1, oi) = ptr[1];
248 view(2, oi) = ptr[2];
252 viewNormalX(0, oi) = ptr[nx];
253 viewNormalY(0, oi) = ptr[ny];
254 viewNormalZ(0, oi) = ptr[nz];
258 viewIntensity(0, oi) = ptr[offsetI];
264 UWARN(
"Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f",
i, ptr[0], ptr[1], scan.
is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
269 UWARN(
"Ignoring point %d with invalid data: pos=%f %f %f",
i, ptr[0], ptr[1], scan.
is2d()?0:ptr[3]);
273 if(oi != scan.
size())
275 cloud.conservativeResize(oi);
281 void pclFromDP(
const DP & cloud, pcl::PointCloud<pcl::PointXYZI> & pclCloud)
284 typedef DP::ConstView ConstView;
286 if (cloud.features.cols() == 0)
289 pclCloud.resize(cloud.features.cols());
290 pclCloud.is_dense =
true;
292 bool hasIntensity = cloud.descriptorExists(
"intensity");
295 ConstView
view(cloud.getFeatureViewByName(
"x"));
296 ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName(
"intensity",0):
view);
297 bool is3D = cloud.featureExists(
"z");
298 for(
unsigned int i=0;
i<pclCloud.size(); ++
i)
300 pclCloud.at(
i).x =
view(0,
i);
301 pclCloud.at(
i).y =
view(1,
i);
302 pclCloud.at(
i).z = is3D?
view(2,
i):0;
304 pclCloud.at(
i).intensity = viewIntensity(0,
i);
308 void pclFromDP(
const DP & cloud, pcl::PointCloud<pcl::PointXYZINormal> & pclCloud)
311 typedef DP::ConstView ConstView;
313 if (cloud.features.cols() == 0)
316 pclCloud.resize(cloud.features.cols());
317 pclCloud.is_dense =
true;
319 bool hasIntensity = cloud.descriptorExists(
"intensity");
322 ConstView
view(cloud.getFeatureViewByName(
"x"));
323 bool is3D = cloud.featureExists(
"z");
324 ConstView viewNormalX(cloud.getDescriptorRowViewByName(
"normals",0));
325 ConstView viewNormalY(cloud.getDescriptorRowViewByName(
"normals",1));
326 ConstView viewNormalZ(cloud.getDescriptorRowViewByName(
"normals",2));
327 ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName(
"intensity",0):
view);
328 for(
unsigned int i=0;
i<pclCloud.size(); ++
i)
330 pclCloud.at(
i).x =
view(0,
i);
331 pclCloud.at(
i).y =
view(1,
i);
332 pclCloud.at(
i).z = is3D?
view(2,
i):0;
333 pclCloud.at(
i).normal_x = viewNormalX(0,
i);
334 pclCloud.at(
i).normal_y = viewNormalY(0,
i);
335 pclCloud.at(
i).normal_z = viewNormalZ(0,
i);
337 pclCloud.at(
i).intensity = viewIntensity(0,
i);
344 typedef DP::ConstView ConstView;
348 if (cloud.features.cols() == 0)
352 bool transformValid = !localTransform.isNull() && !localTransform.isIdentity();
355 localTransformInv = localTransform.
inverse();
356 bool is3D = cloud.featureExists(
"z");
357 bool hasNormals = cloud.descriptorExists(
"normals");
358 bool hasIntensity = cloud.descriptorExists(
"intensity");
359 ConstView
view(cloud.getFeatureViewByName(
"x"));
360 ConstView viewNormalX(hasNormals?cloud.getDescriptorRowViewByName(
"normals",0):
view);
361 ConstView viewNormalY(hasNormals?cloud.getDescriptorRowViewByName(
"normals",1):
view);
362 ConstView viewNormalZ(hasNormals?cloud.getDescriptorRowViewByName(
"normals",2):
view);
363 ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName(
"intensity",0):
view);
364 int channels = 2+(is3D?1:0) + (hasNormals?3:0) + (hasIntensity?1:0);
365 cv::Mat
data(1, cloud.features.cols(), CV_32FC(channels));
366 for(
unsigned int i=0;
i<cloud.features.cols(); ++
i)
368 pcl::PointXYZINormal pt;
374 pt.intensity = viewIntensity(0,
i);
376 pt.normal_x = viewNormalX(0,
i);
377 pt.normal_y = viewNormalY(0,
i);
378 pt.normal_z = viewNormalZ(0,
i);
385 value[index++] = pt.x;
386 value[index++] = pt.y;
388 value[index++] = pt.z;
390 value[index++] = pt.intensity;
392 value[index++] = pt.normal_x;
393 value[index++] = pt.normal_y;
394 value[index++] = pt.normal_z;
415 assert((dimp1 == 3) || (dimp1 == 4));
417 if (
matrix.rows() == dimp1)
420 M out(M::Identity(dimp1,dimp1));
421 out.topLeftCorner(2,2) =
matrix.topLeftCorner(2,2);
422 out.topRightCorner(2,1) =
matrix.topRightCorner(2,1);
447 return "This matcher matches a point from the reading to its closest neighbors in the reference.";
452 {
"knn",
"number of nearest neighbors to consider it the reference",
"1",
"1",
"2147483647", &P::Comp<unsigned>},
453 {
"epsilon",
"approximation to use for the nearest-neighbor search",
"0",
"0",
"inf", &P::Comp<T>},
454 {
"searchType",
"Nabo search type. 0: brute force, check distance to every point in the data (very slow), 1: kd-tree with linear heap, good for small knn (~up to 30) and 2: kd-tree with tree heap, good for large knn (~from 30)",
"1",
"0",
"2", &P::Comp<unsigned>},
455 {
"maxDist",
"maximum distance to consider for neighbors",
"inf",
"0",
"inf", &P::Comp<T>}
488 UWARN(
"KDTreeMatcherIntensity: knn is not over 1 (%d), intensity re-ordering will be ignored.",
knn);
494 const int pointsCount(filteredReading.features.cols());
496 typename Matches::Dists(
knn, pointsCount),
497 typename Matches::Ids(
knn, pointsCount)
500 const BOOST_AUTO(filteredReadingIntensity, filteredReading.getDescriptorViewByName(
"intensity"));
508 Matches matchesOrderedByIntensity(
509 typename Matches::Dists(1, pointsCount),
510 typename Matches::Ids(1, pointsCount)
512 #pragma omp parallel for
513 for (
int i = 0;
i < pointsCount; ++
i)
516 bool minDistFound =
false;
519 int matchesIdsCoeff =
matches.ids.coeff(k,
i);
520 if (matchesIdsCoeff!=-1)
523 if(distIntensity < minDistance)
525 matchesOrderedByIntensity.ids.coeffRef(0,
i) =
matches.ids.coeff(k,
i);
526 matchesOrderedByIntensity.dists.coeffRef(0,
i) =
matches.dists.coeff(k,
i);
527 minDistance = distIntensity;
535 matchesOrderedByIntensity.ids.coeffRef(0,
i) =
matches.ids.coeff(0,
i);
536 matchesOrderedByIntensity.dists.coeffRef(0,
i) =
matches.dists.coeff(0,
i);
539 matches = matchesOrderedByIntensity;