32 #include <pcl/io/pcd_io.h> 33 #include <pcl/point_types.h> 66 m_largestDistanceFromOrigin(0),
67 m_heightfilter_z_min(-10),
68 m_heightfilter_z_max(10),
69 m_heightfilter_enabled(false)
77 CPointsMap::~CPointsMap() {}
83 bool CPointsMap::save2D_to_text_file(
const string& file)
const 88 for (
unsigned int i = 0; i < m_x.size(); i++)
100 bool CPointsMap::save3D_to_text_file(
const string& file)
const 103 if (!f)
return false;
105 for (
unsigned int i = 0; i < m_x.size(); i++)
106 os::fprintf(f,
"%f %f %f\n", m_x[i], m_y[i], m_z[i]);
117 bool CPointsMap::load2Dor3D_from_text_file(
125 if (!f)
return false;
128 char *ptr, *ptr1, *ptr2, *ptr3;
137 if (!fgets(str,
sizeof(str), f))
break;
141 while (ptr[0] && (ptr[0] ==
' ' || ptr[0] ==
'\t' || ptr[0] ==
'\r' ||
146 float xx = strtod(ptr, &ptr1);
149 float yy = strtod(ptr1, &ptr2);
154 this->insertPoint(xx, yy, 0);
158 float zz = strtod(ptr2, &ptr3);
161 this->insertPoint(xx, yy, zz);
182 mxArray* CPointsMap::writeToMatlab()
const 185 MRPT_TODO(
"Create 3xN array xyz of points coordinates")
186 const
char* fields[] = {
"x",
"y",
"z"};
187 mexplus::MxArray map_struct(
188 mexplus::MxArray::Struct(
sizeof(fields) /
sizeof(fields[0]), fields));
190 map_struct.set(
"x", m_x);
191 map_struct.set(
"y", m_y);
192 map_struct.set(
"z", m_z);
193 return map_struct.release();
203 unsigned long CPointsMap::getPoint(
size_t index,
float&
x,
float&
y)
const 209 return getPointWeight(
index);
211 unsigned long CPointsMap::getPoint(
212 size_t index,
float&
x,
float&
y,
float&
z)
const 219 return getPointWeight(
index);
221 unsigned long CPointsMap::getPoint(
size_t index,
double&
x,
double&
y)
const 227 return getPointWeight(
index);
230 unsigned long CPointsMap::getPoint(
231 size_t index,
double&
x,
double&
y,
double&
z)
const 238 return getPointWeight(
index);
245 void CPointsMap::getPointsBuffer(
246 size_t& outPointsCount,
const float*& xs,
const float*& ys,
247 const float*& zs)
const 249 outPointsCount =
size();
251 if (outPointsCount > 0)
259 xs = ys = zs =
nullptr;
266 void CPointsMap::clipOutOfRangeInZ(
float zMin,
float zMax)
268 const size_t n =
size();
269 vector<bool> deletionMask(
n);
272 for (
size_t i = 0; i <
n; i++)
273 deletionMask[i] = (m_z[i] < zMin || m_z[i] > zMax);
276 applyDeletionMask(deletionMask);
284 void CPointsMap::clipOutOfRange(
const TPoint2D&
p,
float maxRange)
286 size_t i,
n =
size();
287 vector<bool> deletionMask;
290 deletionMask.resize(
n);
292 const auto max_sq = maxRange * maxRange;
295 for (i = 0; i <
n; i++)
296 deletionMask[i] =
square(
p.x - m_x[i]) +
square(
p.y - m_y[i]) > max_sq;
299 applyDeletionMask(deletionMask);
304 void CPointsMap::determineMatching2D(
315 params.offset_other_map_points,
params.decimation_other_map_points);
320 otherMapPose_.
x(), otherMapPose_.
y(), otherMapPose_.
phi());
322 const size_t nLocalPoints = otherMap->
size();
323 const size_t nGlobalPoints = this->
size();
324 float _sumSqrDist = 0;
325 size_t _sumSqrCount = 0;
326 size_t nOtherMapPointsWithCorrespondence =
329 float local_x_min = std::numeric_limits<float>::max(),
330 local_x_max = -std::numeric_limits<float>::max();
331 float global_x_min = std::numeric_limits<float>::max(),
332 global_x_max = -std::numeric_limits<float>::max();
333 float local_y_min = std::numeric_limits<float>::max(),
334 local_y_max = -std::numeric_limits<float>::max();
335 float global_y_min = std::numeric_limits<float>::max(),
336 global_y_max = -std::numeric_limits<float>::max();
338 double maxDistForCorrespondenceSquared;
339 float x_local, y_local;
340 unsigned int localIdx;
342 const float *x_other_it, *y_other_it, *z_other_it;
345 correspondences.clear();
346 correspondences.reserve(nLocalPoints);
347 extraResults.correspondencesRatio = 0;
350 _correspondences.reserve(nLocalPoints);
353 if (!nGlobalPoints || !nLocalPoints)
return;
355 const double sin_phi = sin(otherMapPose.phi);
356 const double cos_phi = cos(otherMapPose.phi);
364 size_t nPackets = nLocalPoints / 4;
366 Eigen::ArrayXf x_locals(nLocalPoints), y_locals(nLocalPoints);
369 const __m128 cos_4val = _mm_set1_ps(cos_phi);
370 const __m128 sin_4val = _mm_set1_ps(sin_phi);
371 const __m128 x0_4val = _mm_set1_ps(otherMapPose.x);
372 const __m128 y0_4val = _mm_set1_ps(otherMapPose.y);
375 __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
377 __m128 y_mins = x_mins;
378 __m128 y_maxs = x_maxs;
380 const float* ptr_in_x = &otherMap->
m_x[0];
381 const float* ptr_in_y = &otherMap->
m_y[0];
382 float* ptr_out_x = &x_locals[0];
383 float* ptr_out_y = &y_locals[0];
385 for (; nPackets; nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_out_x += 4,
388 const __m128 xs = _mm_loadu_ps(ptr_in_x);
389 const __m128 ys = _mm_loadu_ps(ptr_in_y);
391 const __m128 lxs = _mm_add_ps(
393 _mm_sub_ps(_mm_mul_ps(xs, cos_4val), _mm_mul_ps(ys, sin_4val)));
394 const __m128 lys = _mm_add_ps(
396 _mm_add_ps(_mm_mul_ps(xs, sin_4val), _mm_mul_ps(ys, cos_4val)));
397 _mm_store_ps(ptr_out_x, lxs);
398 _mm_store_ps(ptr_out_y, lys);
400 x_mins = _mm_min_ps(x_mins, lxs);
401 x_maxs = _mm_max_ps(x_maxs, lxs);
402 y_mins = _mm_min_ps(y_mins, lys);
403 y_maxs = _mm_max_ps(y_maxs, lys);
409 _mm_store_ps(temp_nums, x_mins);
411 min(
min(temp_nums[0], temp_nums[1]),
min(temp_nums[2], temp_nums[3]));
412 _mm_store_ps(temp_nums, y_mins);
414 min(
min(temp_nums[0], temp_nums[1]),
min(temp_nums[2], temp_nums[3]));
415 _mm_store_ps(temp_nums, x_maxs);
417 max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
418 _mm_store_ps(temp_nums, y_maxs);
420 max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
423 for (
size_t k = 0; k < nLocalPoints % 4; k++)
425 float x = ptr_in_x[k];
426 float y = ptr_in_y[k];
427 float out_x = otherMapPose.x + cos_phi *
x - sin_phi *
y;
428 float out_y = otherMapPose.y + sin_phi *
x + cos_phi *
y;
430 local_x_min =
std::min(local_x_min, out_x);
431 local_x_max = std::max(local_x_max, out_x);
433 local_y_min =
std::min(local_y_min, out_y);
434 local_y_max = std::max(local_y_max, out_y);
436 ptr_out_x[k] = out_x;
437 ptr_out_y[k] = out_y;
442 const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> x_org(
443 const_cast<float*>(&otherMap->
m_x[0]), otherMap->
m_x.size(), 1);
444 const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> y_org(
445 const_cast<float*>(&otherMap->
m_y[0]), otherMap->
m_y.size(), 1);
447 Eigen::Array<float, Eigen::Dynamic, 1> x_locals =
448 otherMapPose.x + cos_phi * x_org.array() - sin_phi * y_org.array();
449 Eigen::Array<float, Eigen::Dynamic, 1> y_locals =
450 otherMapPose.y + sin_phi * x_org.array() + cos_phi * y_org.array();
452 local_x_min = x_locals.minCoeff();
453 local_y_min = y_locals.minCoeff();
454 local_x_max = x_locals.maxCoeff();
455 local_y_max = y_locals.maxCoeff();
459 float global_z_min, global_z_max;
461 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
466 if (local_x_min > global_x_max || local_x_max < global_x_min ||
467 local_y_min > global_y_max || local_y_max < global_y_min)
472 for (localIdx =
params.offset_other_map_points,
473 x_other_it = &otherMap->
m_x[
params.offset_other_map_points],
474 y_other_it = &otherMap->
m_y[
params.offset_other_map_points],
475 z_other_it = &otherMap->
m_z[
params.offset_other_map_points];
476 localIdx < nLocalPoints;
477 x_other_it +=
params.decimation_other_map_points,
478 y_other_it +=
params.decimation_other_map_points,
479 z_other_it +=
params.decimation_other_map_points,
480 localIdx +=
params.decimation_other_map_points)
483 x_local = x_locals[localIdx];
484 y_local = y_locals[localIdx];
493 float tentativ_err_sq;
494 unsigned int tentativ_this_idx = kdTreeClosestPoint2D(
500 maxDistForCorrespondenceSquared =
square(
501 params.maxAngularDistForCorrespondence *
505 params.maxDistForCorrespondence);
508 if (tentativ_err_sq < maxDistForCorrespondenceSquared)
511 _correspondences.resize(_correspondences.size() + 1);
515 p.this_idx = tentativ_this_idx;
516 p.this_x = m_x[tentativ_this_idx];
517 p.this_y = m_y[tentativ_this_idx];
518 p.this_z = m_z[tentativ_this_idx];
520 p.other_idx = localIdx;
521 p.other_x = *x_other_it;
522 p.other_y = *y_other_it;
523 p.other_z = *z_other_it;
525 p.errorSquareAfterTransformation = tentativ_err_sq;
528 nOtherMapPointsWithCorrespondence++;
531 _sumSqrDist +=
p.errorSquareAfterTransformation;
541 if (
params.onlyUniqueRobust)
544 params.onlyKeepTheClosest,
545 "ERROR: onlyKeepTheClosest must be also set to true when " 546 "onlyUniqueRobust=true.");
548 nGlobalPoints, correspondences);
552 correspondences.swap(_correspondences);
558 extraResults.sumSqrDist =
559 _sumSqrDist /
static_cast<double>(_sumSqrCount);
561 extraResults.sumSqrDist = 0;
564 extraResults.correspondencesRatio =
params.decimation_other_map_points *
565 nOtherMapPointsWithCorrespondence /
566 static_cast<float>(nLocalPoints);
574 void CPointsMap::changeCoordinatesReference(
const CPose2D& newBase)
576 const size_t N = m_x.size();
578 const CPose3D newBase3D(newBase);
580 for (
size_t i = 0; i < N; i++)
582 m_x[i], m_y[i], m_z[i],
583 m_x[i], m_y[i], m_z[i]
592 void CPointsMap::changeCoordinatesReference(
const CPose3D& newBase)
594 const size_t N = m_x.size();
596 for (
size_t i = 0; i < N; i++)
598 m_x[i], m_y[i], m_z[i],
599 m_x[i], m_y[i], m_z[i]
608 void CPointsMap::changeCoordinatesReference(
612 changeCoordinatesReference(newBase);
618 bool CPointsMap::isEmpty()
const {
return m_x.empty(); }
622 CPointsMap::TInsertionOptions::TInsertionOptions()
623 : minDistBetweenLaserPoints(0.02f),
624 addToExistingPointsMap(true),
625 also_interpolate(false),
626 disableDeletion(true),
627 fuseWithExisting(false),
629 horizontalTolerance(
DEG2RAD(0.05)),
630 maxDistForInterpolatePoints(2.0f),
631 insertInvalidPoints(false)
642 out << minDistBetweenLaserPoints << addToExistingPointsMap
643 << also_interpolate << disableDeletion << fuseWithExisting
644 << isPlanarMap << horizontalTolerance << maxDistForInterpolatePoints
645 << insertInvalidPoints;
657 in >> minDistBetweenLaserPoints >> addToExistingPointsMap >>
658 also_interpolate >> disableDeletion >> fuseWithExisting >>
659 isPlanarMap >> horizontalTolerance >>
660 maxDistForInterpolatePoints >> insertInvalidPoints;
669 : sigma_dist(0.0025), max_corr_distance(1.0), decimation(10)
678 out << sigma_dist << max_corr_distance << decimation;
690 in >> sigma_dist >> max_corr_distance >> decimation;
715 in >> point_size >> this->
color;
726 out <<
"\n----------- [CPointsMap::TInsertionOptions] ------------ \n\n";
745 out <<
"\n----------- [CPointsMap::TLikelihoodOptions] ------------ \n\n";
754 out <<
"\n----------- [CPointsMap::TRenderOptions] ------------ \n\n";
811 obj->loadFromPointsMap(
this);
814 obj->enableColorFromZ(
true);
822 obj->loadFromPointsMap(
this);
826 obj->recolorizeByCoordinate(
859 float maxDistSq = 0, d;
860 for (
auto X =
m_x.begin(), Y =
m_y.begin(), Z =
m_z.begin();
861 X !=
m_x.end(); ++X, ++Y, ++Z)
864 maxDistSq = max(d, maxDistSq);
877 vector<float>& xs, vector<float>& ys,
size_t decimation)
const 883 xs = vector<float>(
m_x.begin(),
m_x.end());
884 ys = vector<float>(
m_y.begin(),
m_y.end());
888 size_t N =
m_x.size() / decimation;
893 auto X =
m_x.begin();
894 auto Y =
m_y.begin();
895 for (
auto oX = xs.begin(), oY = ys.begin(); oX != xs.end();
896 X += decimation, Y += decimation, ++oX, ++oY)
909 float x0,
float y0)
const 917 float x1, y1, x2, y2, d1, d2;
928 if (d12 > 0.20f * 0.20f || d12 < 0.03f * 0.03f)
934 double interp_x, interp_y;
952 float& min_x,
float& max_x,
float& min_y,
float& max_y,
float& min_z,
955 const size_t nPoints =
m_x.size();
970 size_t nPackets = nPoints / 4;
973 __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
975 __m128 y_mins = x_mins, y_maxs = x_maxs;
976 __m128 z_mins = x_mins, z_maxs = x_maxs;
978 const float* ptr_in_x = &
m_x[0];
979 const float* ptr_in_y = &
m_y[0];
980 const float* ptr_in_z = &
m_z[0];
983 nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_in_z += 4)
985 const __m128 xs = _mm_loadu_ps(ptr_in_x);
986 x_mins = _mm_min_ps(x_mins, xs);
987 x_maxs = _mm_max_ps(x_maxs, xs);
989 const __m128 ys = _mm_loadu_ps(ptr_in_y);
990 y_mins = _mm_min_ps(y_mins, ys);
991 y_maxs = _mm_max_ps(y_maxs, ys);
993 const __m128 zs = _mm_loadu_ps(ptr_in_z);
994 z_mins = _mm_min_ps(z_mins, zs);
995 z_maxs = _mm_max_ps(z_maxs, zs);
1001 _mm_store_ps(temp_nums, x_mins);
1003 min(
min(temp_nums[0], temp_nums[1]),
1004 min(temp_nums[2], temp_nums[3]));
1005 _mm_store_ps(temp_nums, y_mins);
1007 min(
min(temp_nums[0], temp_nums[1]),
1008 min(temp_nums[2], temp_nums[3]));
1009 _mm_store_ps(temp_nums, z_mins);
1011 min(
min(temp_nums[0], temp_nums[1]),
1012 min(temp_nums[2], temp_nums[3]));
1013 _mm_store_ps(temp_nums, x_maxs);
1015 max(max(temp_nums[0], temp_nums[1]),
1016 max(temp_nums[2], temp_nums[3]));
1017 _mm_store_ps(temp_nums, y_maxs);
1019 max(max(temp_nums[0], temp_nums[1]),
1020 max(temp_nums[2], temp_nums[3]));
1021 _mm_store_ps(temp_nums, z_maxs);
1023 max(max(temp_nums[0], temp_nums[1]),
1024 max(temp_nums[2], temp_nums[3]));
1027 for (
size_t k = 0; k < nPoints % 4; k++)
1041 (std::numeric_limits<float>::max)();
1044 -(std::numeric_limits<float>::max)();
1046 for (
auto xi =
m_x.begin(), yi =
m_y.begin(), zi =
m_z.begin();
1047 xi !=
m_x.end(); xi++, yi++, zi++)
1083 params.offset_other_map_points,
params.decimation_other_map_points);
1088 const size_t nLocalPoints = otherMap->
size();
1089 const size_t nGlobalPoints = this->
size();
1090 float _sumSqrDist = 0;
1091 size_t _sumSqrCount = 0;
1092 size_t nOtherMapPointsWithCorrespondence =
1095 float local_x_min = std::numeric_limits<float>::max(),
1096 local_x_max = -std::numeric_limits<float>::max();
1097 float local_y_min = std::numeric_limits<float>::max(),
1098 local_y_max = -std::numeric_limits<float>::max();
1099 float local_z_min = std::numeric_limits<float>::max(),
1100 local_z_max = -std::numeric_limits<float>::max();
1102 double maxDistForCorrespondenceSquared;
1105 correspondences.clear();
1106 correspondences.reserve(nLocalPoints);
1109 _correspondences.reserve(nLocalPoints);
1112 if (!nGlobalPoints || !nLocalPoints)
return;
1116 vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
1117 z_locals(nLocalPoints);
1119 for (
unsigned int localIdx =
params.offset_other_map_points;
1120 localIdx < nLocalPoints;
1121 localIdx +=
params.decimation_other_map_points)
1123 float x_local, y_local, z_local;
1125 otherMap->m_x[localIdx], otherMap->m_y[localIdx],
1126 otherMap->m_z[localIdx], x_local, y_local, z_local);
1128 x_locals[localIdx] = x_local;
1129 y_locals[localIdx] = y_local;
1130 z_locals[localIdx] = z_local;
1133 local_x_min =
min(local_x_min, x_local);
1134 local_x_max = max(local_x_max, x_local);
1135 local_y_min =
min(local_y_min, y_local);
1136 local_y_max = max(local_y_max, y_local);
1137 local_z_min =
min(local_z_min, z_local);
1138 local_z_max = max(local_z_max, z_local);
1142 float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1145 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1150 if (local_x_min > global_x_max || local_x_max < global_x_min ||
1151 local_y_min > global_y_max || local_y_max < global_y_min)
1156 for (
unsigned int localIdx =
params.offset_other_map_points;
1157 localIdx < nLocalPoints;
1158 localIdx +=
params.decimation_other_map_points)
1161 const float x_local = x_locals[localIdx];
1162 const float y_local = y_locals[localIdx];
1163 const float z_local = z_locals[localIdx];
1171 float tentativ_err_sq;
1173 x_local, y_local, z_local,
1178 maxDistForCorrespondenceSquared =
square(
1179 params.maxAngularDistForCorrespondence *
1180 params.angularDistPivotPoint.distanceTo(
1181 TPoint3D(x_local, y_local, z_local)) +
1182 params.maxDistForCorrespondence);
1185 if (tentativ_err_sq < maxDistForCorrespondenceSquared)
1188 _correspondences.resize(_correspondences.size() + 1);
1192 p.this_idx = tentativ_this_idx;
1193 p.this_x =
m_x[tentativ_this_idx];
1194 p.this_y =
m_y[tentativ_this_idx];
1195 p.this_z =
m_z[tentativ_this_idx];
1197 p.other_idx = localIdx;
1198 p.other_x = otherMap->m_x[localIdx];
1199 p.other_y = otherMap->m_y[localIdx];
1200 p.other_z = otherMap->m_z[localIdx];
1202 p.errorSquareAfterTransformation = tentativ_err_sq;
1205 nOtherMapPointsWithCorrespondence++;
1208 _sumSqrDist +=
p.errorSquareAfterTransformation;
1219 if (
params.onlyUniqueRobust)
1222 params.onlyKeepTheClosest,
1223 "ERROR: onlyKeepTheClosest must be also set to true when " 1224 "onlyUniqueRobust=true.");
1226 nGlobalPoints, correspondences);
1230 correspondences.swap(_correspondences);
1235 extraResults.sumSqrDist =
1236 (_sumSqrCount) ? _sumSqrDist / static_cast<double>(_sumSqrCount) : 0;
1237 extraResults.correspondencesRatio =
params.decimation_other_map_points *
1238 nOtherMapPointsWithCorrespondence /
1239 static_cast<float>(nLocalPoints);
1248 const TPoint2D& center,
const double radius,
const double zmin,
1252 for (
size_t k = 0; k <
m_x.size(); k++)
1266 const double&
R,
const double&
G,
const double& B)
1269 double minX, maxX, minY, maxY, minZ, maxZ;
1270 minX =
min(corner1.
x, corner2.
x);
1271 maxX = max(corner1.
x, corner2.
x);
1272 minY =
min(corner1.
y, corner2.
y);
1273 maxY = max(corner1.
y, corner2.
y);
1274 minZ =
min(corner1.
z, corner2.
z);
1275 maxZ = max(corner1.
z, corner2.
z);
1276 for (
size_t k = 0; k <
m_x.size(); k++)
1278 if ((
m_x[k] >= minX &&
m_x[k] <= maxX) &&
1279 (
m_y[k] >= minY &&
m_y[k] <= maxY) &&
1280 (
m_z[k] >= minZ &&
m_z[k] <= maxZ))
1291 float& correspondencesRatio)
1298 const size_t nLocalPoints = otherMap->
size();
1299 const size_t nGlobalPoints = this->
size();
1300 size_t nOtherMapPointsWithCorrespondence =
1304 correspondences.clear();
1305 correspondences.reserve(nLocalPoints);
1306 correspondencesRatio = 0;
1310 _correspondences.reserve(nLocalPoints);
1313 if (!nGlobalPoints)
return;
1316 if (!nLocalPoints)
return;
1319 float local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1322 local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1326 float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1329 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1334 if (local_x_min > global_x_max || local_x_max < global_x_min ||
1335 local_y_min > global_y_max || local_y_max < global_y_min)
1339 std::vector<std::vector<size_t>> vIdx;
1344 std::vector<size_t> outIdx;
1345 for (
unsigned int localIdx = 0; localIdx < nLocalPoints; ++localIdx)
1348 const float x_local = otherMap->
m_x[localIdx];
1349 const float y_local = otherMap->
m_y[localIdx];
1350 const float z_local = otherMap->
m_z[localIdx];
1358 x_local, y_local, z_local,
1374 if (distanceForThisPoint < maxDistForCorrespondence)
1377 _correspondences.resize(_correspondences.size() + 1);
1381 p.this_idx = nOtherMapPointsWithCorrespondence++;
1388 p.other_idx = localIdx;
1389 p.other_x = otherMap->
m_x[localIdx];
1390 p.other_y = otherMap->
m_y[localIdx];
1391 p.other_z = otherMap->
m_z[localIdx];
1393 p.errorSquareAfterTransformation = distanceForThisPoint;
1396 std::sort(outIdx.begin(), outIdx.end());
1397 vIdx.push_back(outIdx);
1406 std::map<size_t, std::map<size_t, std::map<size_t, pair<size_t, float>>>>
1409 for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1411 const size_t i0 = vIdx[it->this_idx][0];
1412 const size_t i1 = vIdx[it->this_idx][1];
1413 const size_t i2 = vIdx[it->this_idx][2];
1415 if (best.find(i0) != best.end() &&
1416 best[i0].find(i1) != best[i0].end() &&
1417 best[i0][i1].find(i2) !=
1421 if (best[i0][i1][i2].second > it->errorSquareAfterTransformation)
1423 best[i0][i1][i2].first = it->this_idx;
1424 best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1429 best[i0][i1][i2].first = it->this_idx;
1430 best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1434 for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1436 const size_t i0 = vIdx[it->this_idx][0];
1437 const size_t i1 = vIdx[it->this_idx][1];
1438 const size_t i2 = vIdx[it->this_idx][2];
1440 if (best[i0][i1][i2].
first == it->this_idx)
1441 correspondences.push_back(*it);
1445 correspondencesRatio =
1446 nOtherMapPointsWithCorrespondence /
static_cast<float>(nLocalPoints);
1477 float sumSqrDist = 0;
1479 const size_t N = scanPoints->
m_x.size();
1480 if (!N || !this->
size())
return -100;
1482 const float* xs = &scanPoints->
m_x[0];
1483 const float* ys = &scanPoints->
m_y[0];
1484 const float* zs = &scanPoints->
m_z[0];
1486 float closest_x, closest_y, closest_z;
1489 int nPtsForAverage = 0;
1496 const float ccos = cos(takenFrom2D.
phi);
1497 const float csin = sin(takenFrom2D.
phi);
1499 for (
size_t i = 0; i < N;
1504 const float xg = takenFrom2D.
x + ccos * xs[i] - csin * ys[i];
1505 const float yg = takenFrom2D.
y + csin * xs[i] + ccos * ys[i];
1509 closest_x, closest_y,
1516 sumSqrDist += closest_err;
1523 for (
size_t i = 0; i < N;
1529 takenFrom.
composePoint(xs[i], ys[i], zs[i], xg, yg, zg);
1533 closest_x, closest_y,
1541 sumSqrDist += closest_err;
1545 sumSqrDist /= nPtsForAverage;
1558 double sumSqrDist = 0;
1562 const_cast<CObservationVelodyneScan*>(o)->generatePointCloud();
1565 if (!N || !this->
size())
return -100;
1573 float closest_x, closest_y, closest_z;
1576 int nPtsForAverage = 0;
1578 for (
size_t i = 0; i < N;
1584 sensorAbsPose.
composePoint(xs[i], ys[i], zs[i], xg, yg, zg);
1588 closest_x, closest_y,
1596 sumSqrDist += closest_err;
1599 sumSqrDist /= nPtsForAverage;
1626 if (out_map)
return;
1628 out_map = mrpt::make_aligned_shared<CSimplePointsMap>();
1632 *static_cast<const CPointsMap::TInsertionOptions*>(insertOps);
1634 out_map->insertObservation(&obs,
nullptr);
1677 pt_has_color =
false;
1688 const size_t nThis = this->
size();
1689 const size_t nOther = anotherMap.
size();
1691 const size_t nTot = nThis + nOther;
1695 for (
size_t i = 0, j = nThis; i < nOther; i++, j++)
1697 m_x[j] = anotherMap.
m_x[i];
1698 m_y[j] = anotherMap.
m_y[i];
1699 m_z[j] = anotherMap.
m_z[i];
1711 const std::string& filename,
bool save_as_binary)
const 1714 pcl::PointCloud<pcl::PointXYZ> cloud;
1717 return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
1722 THROW_EXCEPTION(
"Operation not available: MRPT was built without PCL");
1731 pcl::PointCloud<pcl::PointXYZ> cloud;
1732 if (0 != pcl::io::loadPCDFile(filename, cloud))
return false;
1739 THROW_EXCEPTION(
"Operation not available: MRPT was built without PCL");
1750 const size_t n =
mask.size();
1753 for (i = 0, j = 0; i <
n; i++)
1775 const size_t N_this =
size();
1776 const size_t N_other = otherMap->
size();
1779 this->
resize(N_this + N_other);
1807 if (
this == &
obj)
return;
1814 obj.m_largestDistanceFromOriginIsUpdated;
1840 robotPose2D =
CPose2D(*robotPose);
1841 robotPose3D = (*robotPose);
1858 bool reallyInsertIt;
1864 reallyInsertIt =
true;
1868 std::vector<bool> checkForDeletion;
1898 const float *xs, *ys, *zs;
1905 for (
size_t i = 0; i <
n; i++)
1907 if (checkForDeletion[i])
1914 checkForDeletion[i] =
1950 bool reallyInsertIt;
1956 reallyInsertIt =
true;
2023 const CPose3D sensorPose = robotPose3D +
CPose3D(it->sensorPose);
2024 const double rang = it->sensedDistance;
2026 if (rang <= 0 || rang < o->minSensorDistance ||
2032 const unsigned int nSteps =
round(1 + arc_len / 0.05);
2036 for (
double a1 = -aper_2;
a1 < aper_2;
a1 += Aa)
2038 for (
double a2 = -aper_2;
a2 < aper_2;
a2 += Aa)
2040 loc.
x = cos(
a1) * cos(
a2) * rang;
2041 loc.
y = cos(
a1) * sin(
a2) * rang;
2042 loc.
z = sin(
a1) * rang;
2103 std::vector<bool>* notFusedPoints)
2107 const CPose2D nullPose(0, 0, 0);
2112 const size_t nOther = otherMap->
size();
2119 params.maxAngularDistForCorrespondence = 0;
2120 params.maxDistForCorrespondence = minDistForFuse;
2125 correspondences,
params, extraResults);
2130 notFusedPoints->clear();
2131 notFusedPoints->reserve(
m_x.size() + nOther);
2132 notFusedPoints->resize(
m_x.size(),
true);
2141 for (
size_t i = 0; i < nOther; i++)
2143 const unsigned long w_a =
2147 int closestCorr = -1;
2148 float minDist = std::numeric_limits<float>::max();
2150 correspondences.begin();
2151 corrsIt != correspondences.end(); ++corrsIt)
2153 if (corrsIt->other_idx == i)
2155 float dist =
square(corrsIt->other_x - corrsIt->this_x) +
2156 square(corrsIt->other_y - corrsIt->this_y) +
2157 square(corrsIt->other_z - corrsIt->this_z);
2161 closestCorr = corrsIt->this_idx;
2166 if (closestCorr != -1)
2168 unsigned long w_b =
getPoint(closestCorr,
b);
2172 const float F = 1.0f / (w_a + w_b);
2174 m_x[closestCorr] = F * (w_a *
a.x + w_b *
b.x);
2175 m_y[closestCorr] = F * (w_a *
a.y + w_b *
b.y);
2176 m_z[closestCorr] = F * (w_a *
a.z + w_b *
b.z);
2181 if (notFusedPoints) (*notFusedPoints)[closestCorr] =
false;
2186 if (notFusedPoints) (*notFusedPoints).push_back(
false);
2209 const size_t nOldPtsCount = this->
size();
2211 const size_t nNewPtsCount = nOldPtsCount + nScanPts;
2212 this->
resize(nNewPtsCount);
2214 const float K = 1.0f / 255;
2219 sensorGlobalPose = *robotPose + scan.
sensorPose;
2226 const double m00 = HM.get_unsafe(0, 0), m01 = HM.get_unsafe(0, 1),
2227 m02 = HM.get_unsafe(0, 2), m03 = HM.get_unsafe(0, 3);
2228 const double m10 = HM.get_unsafe(1, 0), m11 = HM.get_unsafe(1, 1),
2229 m12 = HM.get_unsafe(1, 2), m13 = HM.get_unsafe(1, 3);
2230 const double m20 = HM.get_unsafe(2, 0), m21 = HM.get_unsafe(2, 1),
2231 m22 = HM.get_unsafe(2, 2), m23 = HM.get_unsafe(2, 3);
2234 for (
size_t i = 0; i < nScanPts; i++)
2241 const double gx = m00 * lx + m01 * ly + m02 * lz + m03;
2242 const double gy = m10 * lx + m11 * ly + m12 * lz + m13;
2243 const double gz = m20 * lx + m21 * ly + m22 * lz + m23;
2246 nOldPtsCount + i, gx, gy, gz,
void clear()
Erase all the contents of the map.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - used in derived classes' serialization.
double x() const
Common members of all points & poses classes.
TLikelihoodOptions()
Initilization of default parameters.
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - for usage in derived classes' serialization.
void kdTreeNClosestPoint3DWithIdx(float x0, float y0, float z0, size_t knn, std::vector< float > &out_x, std::vector< float > &out_y, std::vector< float > &out_z, std::vector< size_t > &out_idx, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest points to some given 3D coordinates.
void insertAnotherMap(const CPointsMap *otherMap, const mrpt::poses::CPose3D &otherPose)
Insert the contents of another map into this one with some geometric transformation, without fusing close points.
Parameters for CMetricMap::compute3DMatchingRatio()
void colormap(const TColormap &color_map, const float color_index, float &r, float &g, float &b)
Transform a float number in the range [0,1] into RGB components.
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
mrpt::img::TColormap colormap
Colormap for points (index is "z" coordinates)
virtual void addFrom(const CPointsMap &anotherMap)
Adds all the points from anotherMap to this map, without fusing.
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight...
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr)=0
Transform the range scan into a set of cartessian coordinated points.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
#define MRPT_MAX_ALIGN_BYTES
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose) override
This is a common version of CMetricMap::insertObservation() for point maps (actually, CMetricMap::internal_insertObservation), so derived classes don't need to worry implementing that method unless something special is really necesary.
static Ptr Create(Args &&... args)
void internal_build_points_map_from_scan2D(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMap::Ptr &out_map, const void *insertOps)
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
#define THROW_EXCEPTION(msg)
void closestFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
int void fclose(FILE *f)
An OS-independent version of fclose.
#define ASSERT_BELOW_(__A, __B)
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call...
bool isHorizontal(const double tolerance=0) const
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards)...
mrpt::img::TColorf color
Color of points.
virtual void setPointAllFieldsFast(const size_t index, const std::vector< float > &point_data)=0
Set all the data fields for one point as a vector: depending on the implementation class this can be ...
double DEG2RAD(const double x)
Degrees to radians.
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
void generatePointCloud(const TGeneratePointCloudParameters ¶ms=TGeneratePointCloudParameters())
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
std::deque< TMeasurement >::const_iterator const_iterator
void base_copyFrom(const CPointsMap &obj)
Helper method for ::copyFrom()
virtual void setPointFast(size_t index, float x, float y, float z)=0
Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and wi...
void setAllVertices(const std::vector< double > &x, const std::vector< double > &y)
Set all vertices at once.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
void loadFromVelodyneScan(const mrpt::obs::CObservationVelodyneScan &scan, const mrpt::poses::CPose3D *robotPose=nullptr)
Like loadFromRangeScan() for Velodyne 3D scans.
A wrapper of a TPolygon2D class, implementing CSerializable.
GLuint GLenum GLenum outY
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void internal_set_build_points_map_from_scan2D(scan2pts_functor fn)
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
virtual void getPointAllFieldsFast(const size_t index, std::vector< float > &point_data) const =0
Get all the data fields for one point as a vector: depending on the implementation class this can be ...
TMapGenericParams genericMapParams
Common params to all maps.
virtual void reserve(size_t newLength)=0
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
bool m_boundingBoxIsUpdated
virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::img::TColorf *pt_color=nullptr) override
In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point...
GLsizei GLsizei GLuint * obj
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
size_t PLY_export_get_vertex_count() const override
In a base class, return the number of vertices.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
mrpt::math::TPose2D asTPose() const
float maxDistForCorr
(Default: 0.10f) The minimum distance between 2 non-probabilistic map elements for counting them as a...
static Ptr Create(Args &&... args)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
T square(const T x)
Inline function for the square of a number.
GLuint GLenum GLenum GLenum outZ
#define ASSERT_(f)
Defines an assertion mechanism.
TPointCloud point_cloud
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor...
#define IS_DERIVED(ptrObj, class_name)
Evaluates to true if a pointer to an object (derived from mrpt::rtti::CObject) is an instance of the ...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
A numeric matrix of compile-time fixed size.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Lightweight 3D point (float version).
void ReadAsAndCastTo(CAST_TO_TYPE &read_here)
Read a value from a stream stored in a type different of the target variable, making the conversion v...
bool isPlanarScan(const double tolerance=0) const
Return true if the laser scanner is "horizontal", so it has an absolute value of "pitch" and "roll" l...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
double sigma_dist
Sigma squared (variance, in meters) of the exponential used to model the likelihood (default= 0...
void getAllPoints(VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
Returns a copy of the 2D/3D points as a std::vector of float coordinates.
virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr) override
See CPointsMap::loadFromRangeScan()
bool PointIntoPolygon(double x, double y) const
Check if a point is inside the polygon.
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - used in derived classes' serialization.
void extractCylinder(const mrpt::math::TPoint2D ¢er, const double radius, const double zmin, const double zmax, CPointsMap *outMap)
Extracts the points in the map within a cylinder in 3D defined the provided radius and zmin/zmax valu...
void fuseWith(CPointsMap *anotherMap, float minDistForFuse=0.02f, std::vector< bool > *notFusedPoints=nullptr)
Insert the contents of another map into this one, fusing the previous content with the new one...
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - for usage in derived classes' serialization.
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints)=0
Auxiliary method called from within addFrom() automatically, to finish the copying of class-specific ...
virtual void setPointWeight(size_t index, unsigned long w)
Sets the point weight, which is ignored in all classes but those which actually store that field (Not...
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const
Save the point cloud as a PCL PCD file, in either ASCII or binary format (requires MRPT built against...
void getPCLPointCloud(POINTCLOUD &cloud) const
Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud<PointXYZ>).
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
GLsizei const GLchar ** string
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void compute3DDistanceToMesh(const mrpt::maps::CMetricMap *otherMap2, const mrpt::poses::CPose3D &otherMapPose, float maxDistForCorrespondence, mrpt::tfest::TMatchingPairList &correspondences, float &correspondencesRatio)
Computes the matchings between this and another 3D points map.
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
void boundingBox(float &min_x, float &max_x, float &min_y, float &max_y, float &min_z, float &max_z) const
Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points.
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
mrpt::aligned_std_vector< float > m_z
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
unsigned long getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
std::vector< uint8_t > intensity
Color [0,255].
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
virtual void resize(size_t newLength)=0
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
void mark_as_modified() const
Users normally don't need to call this.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
bool addToExistingPointsMap
Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded...
Virtual base class for "archives": classes abstracting I/O streams.
bool fuseWithExisting
If set to true (default=false), inserted points are "fused" with previously existent ones...
mrpt::aligned_std_vector< float > m_y
TLikelihoodOptions likelihoodOptions
void filterUniqueRobustPairs(const size_t num_elements_this_map, TMatchingPairList &out_filtered_list) const
Creates a filtered list of pairings with those ones which have a single correspondence which coincide...
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or nullptr if there is no points in the map...
void setPoint(size_t index, float x, float y, float z)
Changes a given point from map, with Z defaulting to 0 if not provided.
TRenderOptions renderOptions
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents any robot's observation.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
void kdtree_mark_as_outdated() const
To be called by child classes when KD tree data changes.
#define ASSERT_ABOVE_(__A, __B)
A RGB color - floats in the range [0,1].
#define LOADABLEOPTS_DUMP_VAR_DEG(variableName)
Macro for dumping a variable to a stream, transforming the argument from radians to degrees...
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams ¶ms, TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding: ...
virtual bool loadPCDFile(const std::string &filename)
Load the point cloud from a PCL PCD file (requires MRPT built against PCL)
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - for usage in derived classes' serialization.
void(*)(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMap::Ptr &out_map, const void *insertOps) scan2pts_functor
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
double max_corr_distance
Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point ...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const
If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points ...
TAuxLoadFunctor dummy_loader
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
size_t kdTreeClosestPoint2D(float x0, float y0, float &out_x, float &out_y, float &out_dist_sqr) const
KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.
virtual void insertPointFast(float x, float y, float z=0)=0
The virtual method for insertPoint() without calling mark_as_modified()
virtual void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams ¶ms, TMatchingExtraResults &extraResults) const override
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const override
Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map...
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=10) ...
void extractPoints(const mrpt::math::TPoint3D &corner1, const mrpt::math::TPoint3D &corner2, CPointsMap *outMap, const double &R=1, const double &G=1, const double &B=1)
Extracts the points in the map within the area defined by two corners.
Parameters for the determination of matchings between point clouds, etc.
mrpt::aligned_std_vector< float > m_x
The point coordinates.
size_t size() const
Returns the number of stored points in the map.
GLubyte GLubyte GLubyte a
bool disableDeletion
If set to false (default=true) points in the same plane as the inserted scan and inside the free spac...
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
void clear()
Clear the contents of this container.
const Scalar * const_iterator
double phi
Orientation (rads)
virtual void PLY_export_get_vertex(const size_t idx, mrpt::math::TPoint3Df &pt, bool &pt_has_color, mrpt::img::TColorf &pt_color) const override
In a base class, will be called after PLY_export_get_vertex_count() once for each exported point...
float kdTreeClosestPoint2DsqrError(float x0, float y0) const
Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor...
void kdTreeTwoClosestPoint2D(float x0, float y0, float &out_x1, float &out_y1, float &out_x2, float &out_y2, float &out_dist_sqr1, float &out_dist_sqr2) const
KD Tree-based search for the TWO closest point to some given 2D coordinates.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool's array as "true".
TMeasurementList sensedData
All the measurements.
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE( class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float &out_x, float &out_y, float &out_z, float &out_dist_sqr) const
KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - for usage in derived classes' serialization.
int round(const T value)
Returns the closer integer (int) to x.