30 #include <pcl/io/pcd_io.h> 31 #include <pcl/point_types.h> 59 : insertionOptions(), likelihoodOptions(), m_x(), m_y(), m_z()
68 CPointsMap::~CPointsMap() =
default;
74 bool CPointsMap::save2D_to_text_file(
const string& file)
const 79 for (
unsigned int i = 0; i < m_x.size(); i++)
91 bool CPointsMap::save3D_to_text_file(
const string& file)
const 96 for (
unsigned int i = 0; i < m_x.size(); i++)
97 os::fprintf(f,
"%f %f %f\n", m_x[i], m_y[i], m_z[i]);
108 bool CPointsMap::load2Dor3D_from_text_file(
116 if (!f)
return false;
119 char *ptr, *ptr1, *ptr2, *ptr3;
128 if (!fgets(str,
sizeof(str), f))
break;
132 while (ptr[0] && (ptr[0] ==
' ' || ptr[0] ==
'\t' || ptr[0] ==
'\r' ||
137 float xx = strtod(ptr, &ptr1);
140 float yy = strtod(ptr1, &ptr2);
145 this->insertPoint(xx, yy, 0);
149 float zz = strtod(ptr2, &ptr3);
152 this->insertPoint(xx, yy, zz);
173 mxArray* CPointsMap::writeToMatlab()
const 176 MRPT_TODO(
"Create 3xN array xyz of points coordinates")
177 const
char* fields[] = {
"x",
"y",
"z"};
178 mexplus::MxArray map_struct(
179 mexplus::MxArray::Struct(
sizeof(fields) /
sizeof(fields[0]), fields));
181 map_struct.set(
"x", m_x);
182 map_struct.set(
"y", m_y);
183 map_struct.set(
"z", m_z);
184 return map_struct.release();
194 void CPointsMap::getPoint(
size_t index,
float&
x,
float&
y)
const 200 void CPointsMap::getPoint(
size_t index,
float&
x,
float&
y,
float&
z)
const 207 void CPointsMap::getPoint(
size_t index,
double&
x,
double&
y)
const 213 void CPointsMap::getPoint(
size_t index,
double&
x,
double&
y,
double&
z)
const 224 void CPointsMap::getPointsBuffer(
225 size_t& outPointsCount,
const float*& xs,
const float*& ys,
226 const float*& zs)
const 228 outPointsCount =
size();
230 if (outPointsCount > 0)
238 xs = ys = zs =
nullptr;
245 void CPointsMap::clipOutOfRangeInZ(
float zMin,
float zMax)
247 const size_t n =
size();
248 vector<bool> deletionMask(
n);
251 for (
size_t i = 0; i <
n; i++)
252 deletionMask[i] = (m_z[i] < zMin || m_z[i] > zMax);
255 applyDeletionMask(deletionMask);
263 void CPointsMap::clipOutOfRange(
const TPoint2D&
p,
float maxRange)
265 size_t i,
n =
size();
266 vector<bool> deletionMask;
269 deletionMask.resize(
n);
271 const auto max_sq = maxRange * maxRange;
274 for (i = 0; i <
n; i++)
275 deletionMask[i] =
square(
p.x - m_x[i]) +
square(
p.y - m_y[i]) > max_sq;
278 applyDeletionMask(deletionMask);
283 void CPointsMap::determineMatching2D(
294 params.offset_other_map_points,
params.decimation_other_map_points);
296 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
299 otherMapPose_.
x(), otherMapPose_.
y(), otherMapPose_.
phi());
301 const size_t nLocalPoints = otherMap->
size();
302 const size_t nGlobalPoints = this->
size();
303 float _sumSqrDist = 0;
304 size_t _sumSqrCount = 0;
305 size_t nOtherMapPointsWithCorrespondence =
308 float local_x_min = std::numeric_limits<float>::max(),
309 local_x_max = -std::numeric_limits<float>::max();
310 float global_x_min = std::numeric_limits<float>::max(),
311 global_x_max = -std::numeric_limits<float>::max();
312 float local_y_min = std::numeric_limits<float>::max(),
313 local_y_max = -std::numeric_limits<float>::max();
314 float global_y_min = std::numeric_limits<float>::max(),
315 global_y_max = -std::numeric_limits<float>::max();
317 double maxDistForCorrespondenceSquared;
318 float x_local, y_local;
319 unsigned int localIdx;
321 const float *x_other_it, *y_other_it, *z_other_it;
324 correspondences.clear();
325 correspondences.reserve(nLocalPoints);
326 extraResults.correspondencesRatio = 0;
329 _correspondences.reserve(nLocalPoints);
332 if (!nGlobalPoints || !nLocalPoints)
return;
334 const double sin_phi = sin(otherMapPose.phi);
335 const double cos_phi = cos(otherMapPose.phi);
343 size_t nPackets = nLocalPoints / 4;
345 Eigen::ArrayXf x_locals(nLocalPoints), y_locals(nLocalPoints);
348 const __m128 cos_4val = _mm_set1_ps(cos_phi);
349 const __m128 sin_4val = _mm_set1_ps(sin_phi);
350 const __m128 x0_4val = _mm_set1_ps(otherMapPose.x);
351 const __m128 y0_4val = _mm_set1_ps(otherMapPose.y);
354 __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
356 __m128 y_mins = x_mins;
357 __m128 y_maxs = x_maxs;
359 const float* ptr_in_x = &otherMap->m_x[0];
360 const float* ptr_in_y = &otherMap->m_y[0];
361 float* ptr_out_x = &x_locals[0];
362 float* ptr_out_y = &y_locals[0];
364 for (; nPackets; nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_out_x += 4,
367 const __m128 xs = _mm_loadu_ps(ptr_in_x);
368 const __m128 ys = _mm_loadu_ps(ptr_in_y);
370 const __m128 lxs = _mm_add_ps(
372 _mm_sub_ps(_mm_mul_ps(xs, cos_4val), _mm_mul_ps(ys, sin_4val)));
373 const __m128 lys = _mm_add_ps(
375 _mm_add_ps(_mm_mul_ps(xs, sin_4val), _mm_mul_ps(ys, cos_4val)));
376 _mm_store_ps(ptr_out_x, lxs);
377 _mm_store_ps(ptr_out_y, lys);
379 x_mins = _mm_min_ps(x_mins, lxs);
380 x_maxs = _mm_max_ps(x_maxs, lxs);
381 y_mins = _mm_min_ps(y_mins, lys);
382 y_maxs = _mm_max_ps(y_maxs, lys);
386 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
float temp_nums[4];
388 _mm_store_ps(temp_nums, x_mins);
390 min(
min(temp_nums[0], temp_nums[1]),
min(temp_nums[2], temp_nums[3]));
391 _mm_store_ps(temp_nums, y_mins);
393 min(
min(temp_nums[0], temp_nums[1]),
min(temp_nums[2], temp_nums[3]));
394 _mm_store_ps(temp_nums, x_maxs);
396 max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
397 _mm_store_ps(temp_nums, y_maxs);
399 max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
402 for (
size_t k = 0; k < nLocalPoints % 4; k++)
404 float x = ptr_in_x[k];
405 float y = ptr_in_y[k];
406 float out_x = otherMapPose.x + cos_phi *
x - sin_phi *
y;
407 float out_y = otherMapPose.y + sin_phi *
x + cos_phi *
y;
409 local_x_min =
std::min(local_x_min, out_x);
410 local_x_max = std::max(local_x_max, out_x);
412 local_y_min =
std::min(local_y_min, out_y);
413 local_y_max = std::max(local_y_max, out_y);
415 ptr_out_x[k] = out_x;
416 ptr_out_y[k] = out_y;
422 const_cast<float*>(&otherMap->m_x[0]), otherMap->m_x.size(), 1);
424 const_cast<float*>(&otherMap->m_y[0]), otherMap->m_y.size(), 1);
426 Eigen::Array<float, Eigen::Dynamic, 1> x_locals =
427 otherMapPose.x + cos_phi * x_org.array() - sin_phi * y_org.array();
428 Eigen::Array<float, Eigen::Dynamic, 1> y_locals =
429 otherMapPose.y + sin_phi * x_org.array() + cos_phi * y_org.array();
431 local_x_min = x_locals.minCoeff();
432 local_y_min = y_locals.minCoeff();
433 local_x_max = x_locals.maxCoeff();
434 local_y_max = y_locals.maxCoeff();
438 float global_z_min, global_z_max;
440 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
445 if (local_x_min > global_x_max || local_x_max < global_x_min ||
446 local_y_min > global_y_max || local_y_max < global_y_min)
451 for (localIdx =
params.offset_other_map_points,
452 x_other_it = &otherMap->m_x[
params.offset_other_map_points],
453 y_other_it = &otherMap->m_y[
params.offset_other_map_points],
454 z_other_it = &otherMap->m_z[
params.offset_other_map_points];
455 localIdx < nLocalPoints;
456 x_other_it +=
params.decimation_other_map_points,
457 y_other_it +=
params.decimation_other_map_points,
458 z_other_it +=
params.decimation_other_map_points,
459 localIdx +=
params.decimation_other_map_points)
462 x_local = x_locals[localIdx];
463 y_local = y_locals[localIdx];
472 float tentativ_err_sq;
473 unsigned int tentativ_this_idx = kdTreeClosestPoint2D(
479 maxDistForCorrespondenceSquared =
square(
480 params.maxAngularDistForCorrespondence *
484 params.maxDistForCorrespondence);
487 if (tentativ_err_sq < maxDistForCorrespondenceSquared)
490 _correspondences.resize(_correspondences.size() + 1);
494 p.this_idx = tentativ_this_idx;
495 p.this_x = m_x[tentativ_this_idx];
496 p.this_y = m_y[tentativ_this_idx];
497 p.this_z = m_z[tentativ_this_idx];
499 p.other_idx = localIdx;
500 p.other_x = *x_other_it;
501 p.other_y = *y_other_it;
502 p.other_z = *z_other_it;
504 p.errorSquareAfterTransformation = tentativ_err_sq;
507 nOtherMapPointsWithCorrespondence++;
510 _sumSqrDist +=
p.errorSquareAfterTransformation;
520 if (
params.onlyUniqueRobust)
523 params.onlyKeepTheClosest,
524 "ERROR: onlyKeepTheClosest must be also set to true when " 525 "onlyUniqueRobust=true.");
527 nGlobalPoints, correspondences);
531 correspondences.swap(_correspondences);
537 extraResults.sumSqrDist =
538 _sumSqrDist /
static_cast<double>(_sumSqrCount);
540 extraResults.sumSqrDist = 0;
543 extraResults.correspondencesRatio =
params.decimation_other_map_points *
544 nOtherMapPointsWithCorrespondence /
545 static_cast<float>(nLocalPoints);
553 void CPointsMap::changeCoordinatesReference(
const CPose2D& newBase)
555 const size_t N = m_x.size();
557 const CPose3D newBase3D(newBase);
559 for (
size_t i = 0; i < N; i++)
561 m_x[i], m_y[i], m_z[i],
562 m_x[i], m_y[i], m_z[i]
571 void CPointsMap::changeCoordinatesReference(
const CPose3D& newBase)
573 const size_t N = m_x.size();
575 for (
size_t i = 0; i < N; i++)
577 m_x[i], m_y[i], m_z[i],
578 m_x[i], m_y[i], m_z[i]
587 void CPointsMap::changeCoordinatesReference(
591 changeCoordinatesReference(newBase);
597 bool CPointsMap::isEmpty()
const {
return m_x.empty(); }
601 CPointsMap::TInsertionOptions::TInsertionOptions()
602 : horizontalTolerance(
DEG2RAD(0.05))
614 out << minDistBetweenLaserPoints << addToExistingPointsMap
615 << also_interpolate << disableDeletion << fuseWithExisting
616 << isPlanarMap << horizontalTolerance << maxDistForInterpolatePoints
617 << insertInvalidPoints;
629 in >> minDistBetweenLaserPoints >> addToExistingPointsMap >>
630 also_interpolate >> disableDeletion >> fuseWithExisting >>
631 isPlanarMap >> horizontalTolerance >>
632 maxDistForInterpolatePoints >> insertInvalidPoints;
649 out << sigma_dist << max_corr_distance << decimation;
661 in >> sigma_dist >> max_corr_distance >> decimation;
686 in >> point_size >> this->
color;
697 out <<
"\n----------- [CPointsMap::TInsertionOptions] ------------ \n\n";
716 out <<
"\n----------- [CPointsMap::TLikelihoodOptions] ------------ \n\n";
725 out <<
"\n----------- [CPointsMap::TRenderOptions] ------------ \n\n";
783 obj->loadFromPointsMap(
this);
786 obj->enableColorFromZ(
false);
792 obj->loadFromPointsMap(
this);
796 obj->recolorizeByCoordinate(
830 float maxDistSq = 0, d;
831 for (
auto X =
m_x.begin(), Y =
m_y.begin(), Z =
m_z.begin();
832 X !=
m_x.end(); ++X, ++Y, ++Z)
835 maxDistSq = max(d, maxDistSq);
848 vector<float>& xs, vector<float>& ys,
size_t decimation)
const 854 xs = vector<float>(
m_x.begin(),
m_x.end());
855 ys = vector<float>(
m_y.begin(),
m_y.end());
859 size_t N =
m_x.size() / decimation;
864 auto X =
m_x.begin();
865 auto Y =
m_y.begin();
866 for (
auto oX = xs.begin(), oY = ys.begin(); oX != xs.end();
867 X += decimation, Y += decimation, ++oX, ++oY)
880 float x0,
float y0)
const 888 float x1, y1, x2, y2, d1, d2;
899 if (d12 > 0.20f * 0.20f || d12 < 0.03f * 0.03f)
905 double interp_x, interp_y;
923 float& min_x,
float& max_x,
float& min_y,
float& max_y,
float& min_z,
928 const size_t nPoints =
m_x.size();
943 size_t nPackets = nPoints / 4;
946 __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
948 __m128 y_mins = x_mins, y_maxs = x_maxs;
949 __m128 z_mins = x_mins, z_maxs = x_maxs;
951 const float* ptr_in_x = &
m_x[0];
952 const float* ptr_in_y = &
m_y[0];
953 const float* ptr_in_z = &
m_z[0];
956 nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_in_z += 4)
958 const __m128 xs = _mm_loadu_ps(ptr_in_x);
959 x_mins = _mm_min_ps(x_mins, xs);
960 x_maxs = _mm_max_ps(x_maxs, xs);
962 const __m128 ys = _mm_loadu_ps(ptr_in_y);
963 y_mins = _mm_min_ps(y_mins, ys);
964 y_maxs = _mm_max_ps(y_maxs, ys);
966 const __m128 zs = _mm_loadu_ps(ptr_in_z);
967 z_mins = _mm_min_ps(z_mins, zs);
968 z_maxs = _mm_max_ps(z_maxs, zs);
972 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
float temp_nums[4];
974 _mm_store_ps(temp_nums, x_mins);
976 min(
min(temp_nums[0], temp_nums[1]),
977 min(temp_nums[2], temp_nums[3]));
978 _mm_store_ps(temp_nums, y_mins);
980 min(
min(temp_nums[0], temp_nums[1]),
981 min(temp_nums[2], temp_nums[3]));
982 _mm_store_ps(temp_nums, z_mins);
984 min(
min(temp_nums[0], temp_nums[1]),
985 min(temp_nums[2], temp_nums[3]));
986 _mm_store_ps(temp_nums, x_maxs);
988 max(max(temp_nums[0], temp_nums[1]),
989 max(temp_nums[2], temp_nums[3]));
990 _mm_store_ps(temp_nums, y_maxs);
992 max(max(temp_nums[0], temp_nums[1]),
993 max(temp_nums[2], temp_nums[3]));
994 _mm_store_ps(temp_nums, z_maxs);
996 max(max(temp_nums[0], temp_nums[1]),
997 max(temp_nums[2], temp_nums[3]));
1000 for (
size_t k = 0; k < nPoints % 4; k++)
1014 (std::numeric_limits<float>::max)();
1017 -(std::numeric_limits<float>::max)();
1019 for (
auto xi =
m_x.begin(), yi =
m_y.begin(), zi =
m_z.begin();
1020 xi !=
m_x.end(); xi++, yi++, zi++)
1057 params.offset_other_map_points,
params.decimation_other_map_points);
1060 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
1062 const size_t nLocalPoints = otherMap->
size();
1063 const size_t nGlobalPoints = this->
size();
1064 float _sumSqrDist = 0;
1065 size_t _sumSqrCount = 0;
1066 size_t nOtherMapPointsWithCorrespondence =
1069 float local_x_min = std::numeric_limits<float>::max(),
1070 local_x_max = -std::numeric_limits<float>::max();
1071 float local_y_min = std::numeric_limits<float>::max(),
1072 local_y_max = -std::numeric_limits<float>::max();
1073 float local_z_min = std::numeric_limits<float>::max(),
1074 local_z_max = -std::numeric_limits<float>::max();
1076 double maxDistForCorrespondenceSquared;
1079 correspondences.clear();
1080 correspondences.reserve(nLocalPoints);
1083 _correspondences.reserve(nLocalPoints);
1086 if (!nGlobalPoints || !nLocalPoints)
return;
1090 vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
1091 z_locals(nLocalPoints);
1093 for (
unsigned int localIdx =
params.offset_other_map_points;
1094 localIdx < nLocalPoints;
1095 localIdx +=
params.decimation_other_map_points)
1097 float x_local, y_local, z_local;
1099 otherMap->m_x[localIdx], otherMap->m_y[localIdx],
1100 otherMap->m_z[localIdx], x_local, y_local, z_local);
1102 x_locals[localIdx] = x_local;
1103 y_locals[localIdx] = y_local;
1104 z_locals[localIdx] = z_local;
1107 local_x_min =
min(local_x_min, x_local);
1108 local_x_max = max(local_x_max, x_local);
1109 local_y_min =
min(local_y_min, y_local);
1110 local_y_max = max(local_y_max, y_local);
1111 local_z_min =
min(local_z_min, z_local);
1112 local_z_max = max(local_z_max, z_local);
1116 float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1119 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1124 if (local_x_min > global_x_max || local_x_max < global_x_min ||
1125 local_y_min > global_y_max || local_y_max < global_y_min)
1130 for (
unsigned int localIdx =
params.offset_other_map_points;
1131 localIdx < nLocalPoints;
1132 localIdx +=
params.decimation_other_map_points)
1135 const float x_local = x_locals[localIdx];
1136 const float y_local = y_locals[localIdx];
1137 const float z_local = z_locals[localIdx];
1145 float tentativ_err_sq;
1147 x_local, y_local, z_local,
1152 maxDistForCorrespondenceSquared =
square(
1153 params.maxAngularDistForCorrespondence *
1154 params.angularDistPivotPoint.distanceTo(
1155 TPoint3D(x_local, y_local, z_local)) +
1156 params.maxDistForCorrespondence);
1159 if (tentativ_err_sq < maxDistForCorrespondenceSquared)
1162 _correspondences.resize(_correspondences.size() + 1);
1166 p.this_idx = tentativ_this_idx;
1167 p.this_x =
m_x[tentativ_this_idx];
1168 p.this_y =
m_y[tentativ_this_idx];
1169 p.this_z =
m_z[tentativ_this_idx];
1171 p.other_idx = localIdx;
1172 p.other_x = otherMap->m_x[localIdx];
1173 p.other_y = otherMap->m_y[localIdx];
1174 p.other_z = otherMap->m_z[localIdx];
1176 p.errorSquareAfterTransformation = tentativ_err_sq;
1179 nOtherMapPointsWithCorrespondence++;
1182 _sumSqrDist +=
p.errorSquareAfterTransformation;
1193 if (
params.onlyUniqueRobust)
1196 params.onlyKeepTheClosest,
1197 "ERROR: onlyKeepTheClosest must be also set to true when " 1198 "onlyUniqueRobust=true.");
1200 nGlobalPoints, correspondences);
1204 correspondences.swap(_correspondences);
1209 extraResults.sumSqrDist =
1210 (_sumSqrCount) ? _sumSqrDist / static_cast<double>(_sumSqrCount) : 0;
1211 extraResults.correspondencesRatio =
params.decimation_other_map_points *
1212 nOtherMapPointsWithCorrespondence /
1213 static_cast<float>(nLocalPoints);
1222 const TPoint2D& center,
const double radius,
const double zmin,
1226 for (
size_t k = 0; k <
m_x.size(); k++)
1240 const double&
R,
const double&
G,
const double& B)
1243 double minX, maxX, minY, maxY, minZ, maxZ;
1244 minX =
min(corner1.
x, corner2.
x);
1245 maxX = max(corner1.
x, corner2.
x);
1246 minY =
min(corner1.
y, corner2.
y);
1247 maxY = max(corner1.
y, corner2.
y);
1248 minZ =
min(corner1.
z, corner2.
z);
1249 maxZ = max(corner1.
z, corner2.
z);
1250 for (
size_t k = 0; k <
m_x.size(); k++)
1252 if ((
m_x[k] >= minX &&
m_x[k] <= maxX) &&
1253 (
m_y[k] >= minY &&
m_y[k] <= maxY) &&
1254 (
m_z[k] >= minZ &&
m_z[k] <= maxZ))
1265 float& correspondencesRatio)
1270 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
1272 const size_t nLocalPoints = otherMap->
size();
1273 const size_t nGlobalPoints = this->
size();
1274 size_t nOtherMapPointsWithCorrespondence =
1278 correspondences.clear();
1279 correspondences.reserve(nLocalPoints);
1280 correspondencesRatio = 0;
1284 _correspondences.reserve(nLocalPoints);
1287 if (!nGlobalPoints)
return;
1290 if (!nLocalPoints)
return;
1293 float local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1295 otherMap->boundingBox(
1296 local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1300 float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1303 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1308 if (local_x_min > global_x_max || local_x_max < global_x_min ||
1309 local_y_min > global_y_max || local_y_max < global_y_min)
1313 std::vector<std::vector<size_t>> vIdx;
1318 std::vector<size_t> outIdx;
1319 for (
unsigned int localIdx = 0; localIdx < nLocalPoints; ++localIdx)
1322 const float x_local = otherMap->m_x[localIdx];
1323 const float y_local = otherMap->m_y[localIdx];
1324 const float z_local = otherMap->m_z[localIdx];
1332 x_local, y_local, z_local,
1348 if (distanceForThisPoint < maxDistForCorrespondence)
1351 _correspondences.resize(_correspondences.size() + 1);
1355 p.this_idx = nOtherMapPointsWithCorrespondence++;
1362 p.other_idx = localIdx;
1363 p.other_x = otherMap->m_x[localIdx];
1364 p.other_y = otherMap->m_y[localIdx];
1365 p.other_z = otherMap->m_z[localIdx];
1367 p.errorSquareAfterTransformation = distanceForThisPoint;
1370 std::sort(outIdx.begin(), outIdx.end());
1371 vIdx.push_back(outIdx);
1380 std::map<size_t, std::map<size_t, std::map<size_t, pair<size_t, float>>>>
1382 TMatchingPairList::iterator it;
1383 for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1385 const size_t i0 = vIdx[it->this_idx][0];
1386 const size_t i1 = vIdx[it->this_idx][1];
1387 const size_t i2 = vIdx[it->this_idx][2];
1389 if (best.find(i0) != best.end() &&
1390 best[i0].find(i1) != best[i0].end() &&
1391 best[i0][i1].find(i2) !=
1395 if (best[i0][i1][i2].second > it->errorSquareAfterTransformation)
1397 best[i0][i1][i2].first = it->this_idx;
1398 best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1403 best[i0][i1][i2].first = it->this_idx;
1404 best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1408 for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1410 const size_t i0 = vIdx[it->this_idx][0];
1411 const size_t i1 = vIdx[it->this_idx][1];
1412 const size_t i2 = vIdx[it->this_idx][2];
1414 if (best[i0][i1][i2].
first == it->this_idx)
1415 correspondences.push_back(*it);
1419 correspondencesRatio =
1420 nOtherMapPointsWithCorrespondence /
static_cast<float>(nLocalPoints);
1427 const float* zs,
const std::size_t num_pts)
1431 float closest_x, closest_y, closest_z;
1434 double sumSqrDist = 0;
1436 std::size_t nPtsForAverage = 0;
1437 for (std::size_t i = 0; i < num_pts;
1443 pc_in_map.
composePoint(xs[i], ys[i], zs[i], xg, yg, zg);
1447 closest_x, closest_y,
1455 sumSqrDist +=
static_cast<double>(closest_err);
1457 if (nPtsForAverage) sumSqrDist /= nPtsForAverage;
1480 const size_t N = scanPoints->
m_x.size();
1481 if (!N || !this->
size())
return -100;
1483 const float* xs = &scanPoints->m_x[0];
1484 const float* ys = &scanPoints->m_y[0];
1485 const float* zs = &scanPoints->m_z[0];
1489 double sumSqrDist = 0;
1490 float closest_x, closest_y;
1492 const float max_sqr_err =
1498 const double ccos = cos(takenFrom2D.
phi);
1499 const double csin = sin(takenFrom2D.
phi);
1500 int nPtsForAverage = 0;
1502 for (
size_t i = 0; i < N;
1507 const float xg = takenFrom2D.
x + ccos * xs[i] - csin * ys[i];
1508 const float yg = takenFrom2D.
y + csin * xs[i] + ccos * ys[i];
1512 closest_x, closest_y,
1519 sumSqrDist +=
static_cast<double>(closest_err);
1521 sumSqrDist /= nPtsForAverage;
1529 takenFrom, xs, ys, zs, N);
1537 if (!o.point_cloud.size())
1540 const size_t N = o.point_cloud.size();
1541 if (!N || !this->
size())
return -100;
1543 const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1545 const float* xs = &o.point_cloud.
x[0];
1546 const float* ys = &o.point_cloud.y[0];
1547 const float* zs = &o.point_cloud.z[0];
1550 sensorAbsPose, xs, ys, zs, N);
1557 if (!N || !this->
size())
return -100;
1559 const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1561 auto xs = o.pointcloud->getPointsBufferRef_x();
1562 auto ys = o.pointcloud->getPointsBufferRef_y();
1563 auto zs = o.pointcloud->getPointsBufferRef_z();
1566 sensorAbsPose, &xs[0], &ys[0], &zs[0], N);
1589 if (out_map)
return;
1591 out_map = std::make_shared<CSimplePointsMap>();
1595 *static_cast<const CPointsMap::TInsertionOptions*>(insertOps);
1597 out_map->insertObservation(obs,
nullptr);
1639 pt_has_color =
false;
1650 const size_t nThis = this->
size();
1651 const size_t nOther = anotherMap.
size();
1653 const size_t nTot = nThis + nOther;
1657 for (
size_t i = 0, j = nThis; i < nOther; i++, j++)
1659 m_x[j] = anotherMap.
m_x[i];
1660 m_y[j] = anotherMap.
m_y[i];
1661 m_z[j] = anotherMap.
m_z[i];
1673 const std::string& filename,
bool save_as_binary)
const 1676 pcl::PointCloud<pcl::PointXYZ> cloud;
1679 return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
1684 THROW_EXCEPTION(
"Operation not available: MRPT was built without PCL");
1693 pcl::PointCloud<pcl::PointXYZ> cloud;
1694 if (0 != pcl::io::loadPCDFile(filename, cloud))
return false;
1701 THROW_EXCEPTION(
"Operation not available: MRPT was built without PCL");
1712 const size_t n =
mask.size();
1715 for (i = 0, j = 0; i <
n; i++)
1737 const size_t N_this =
size();
1738 const size_t N_other = otherMap->
size();
1741 this->
resize(N_this + N_other);
1769 if (
this == &
obj)
return;
1776 obj.m_largestDistanceFromOriginIsUpdated;
1802 robotPose2D =
CPose2D(*robotPose);
1803 robotPose3D = (*robotPose);
1819 bool reallyInsertIt;
1825 reallyInsertIt =
true;
1829 std::vector<bool> checkForDeletion;
1860 const float *xs, *ys, *zs;
1867 for (
size_t i = 0; i <
n; i++)
1869 if (checkForDeletion[i])
1877 checkForDeletion[i] =
1913 bool reallyInsertIt;
1919 reallyInsertIt =
true;
1973 this->
size() + o.sensedData.size() * 30);
1975 for (
auto it = o.begin(); it != o.end(); ++it)
1977 const CPose3D sensorPose = robotPose3D +
CPose3D(it->sensorPose);
1978 const double rang = it->sensedDistance;
1980 if (rang <= 0 || rang < o.minSensorDistance ||
1981 rang > o.maxSensorDistance)
1986 const double arc_len = o.sensorConeApperture * rang;
1987 const unsigned int nSteps =
round(1 + arc_len / 0.05);
1988 const double Aa = o.sensorConeApperture / double(nSteps);
1991 for (
double a1 = -aper_2;
a1 < aper_2;
a1 += Aa)
1993 for (
double a2 = -aper_2;
a2 < aper_2;
a2 += Aa)
1995 loc.
x = cos(
a1) * cos(
a2) * rang;
1996 loc.
y = cos(
a1) * sin(
a2) * rang;
1997 loc.
z = sin(
a1) * rang;
2016 if (!o.point_cloud.size())
2076 std::vector<bool>* notFusedPoints)
2080 const CPose2D nullPose(0, 0, 0);
2085 const size_t nOther = otherMap->
size();
2092 params.maxAngularDistForCorrespondence = 0;
2093 params.maxDistForCorrespondence = minDistForFuse;
2098 correspondences,
params, extraResults);
2103 notFusedPoints->clear();
2104 notFusedPoints->reserve(
m_x.size() + nOther);
2105 notFusedPoints->resize(
m_x.size(),
true);
2114 for (
size_t i = 0; i < nOther; i++)
2120 int closestCorr = -1;
2121 float minDist = std::numeric_limits<float>::max();
2122 for (
auto corrsIt = correspondences.begin();
2123 corrsIt != correspondences.end(); ++corrsIt)
2125 if (corrsIt->other_idx == i)
2127 float dist =
square(corrsIt->other_x - corrsIt->this_x) +
2128 square(corrsIt->other_y - corrsIt->this_y) +
2129 square(corrsIt->other_z - corrsIt->this_z);
2133 closestCorr = corrsIt->this_idx;
2138 if (closestCorr != -1)
2145 const float F = 1.0f / (w_a + w_b);
2147 m_x[closestCorr] = F * (w_a *
a.x + w_b *
b.x);
2148 m_y[closestCorr] = F * (w_a *
a.y + w_b *
b.y);
2149 m_z[closestCorr] = F * (w_a *
a.z + w_b *
b.z);
2154 if (notFusedPoints) (*notFusedPoints)[closestCorr] =
false;
2159 if (notFusedPoints) (*notFusedPoints).push_back(
false);
2182 const size_t nOldPtsCount = this->
size();
2184 const size_t nNewPtsCount = nOldPtsCount + nScanPts;
2185 this->
resize(nNewPtsCount);
2187 const float K = 1.0f / 255;
2192 sensorGlobalPose = *robotPose + scan.
sensorPose;
2199 const double m00 = HM(0, 0), m01 = HM(0, 1), m02 = HM(0, 2), m03 = HM(0, 3);
2200 const double m10 = HM(1, 0), m11 = HM(1, 1), m12 = HM(1, 2), m13 = HM(1, 3);
2201 const double m20 = HM(2, 0), m21 = HM(2, 1), m22 = HM(2, 2), m23 = HM(2, 3);
2204 for (
size_t i = 0; i < nScanPts; i++)
2211 const double gx = m00 * lx + m01 * ly + m02 * lz + m03;
2212 const double gy = m10 * lx + m11 * ly + m12 * lz + m13;
2213 const double gz = m20 * lx + m21 * ly + m22 * lz + m23;
2216 nOldPtsCount + i, gx, gy, gz,
void clear()
Erase all the contents of the map.
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class, NS)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
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.
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 getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
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.
A compile-time fixed-size numeric matrix container.
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.
double x
X,Y,Z coordinates.
static Ptr Create(Args &&... args)
#define MRPT_TRY_END
The end of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ex...
void internal_build_points_map_from_scan2D(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMap::Ptr &out_map, const void *insertOps)
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< 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...
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.
An observation from any sensor that can be summarized as a pointcloud.
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.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
void base_copyFrom(const CPointsMap &obj)
Helper method for ::copyFrom()
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 unsigned int getPointWeight(size_t index) const
Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually st...
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
void generatePointCloud(const TGeneratePointCloudParameters ¶ms=TGeneratePointCloudParameters())
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
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.
#define MRPT_TRY_START
The start of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ...
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...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
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.
double internal_computeObservationLikelihoodPointCloud3D(const mrpt::poses::CPose3D &pc_in_map, const float *xs, const float *ys, const float *zs, const std::size_t num_pts)
#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.
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.
#define IS_DERIVED(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is an instance of the given ...
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)
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
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...
#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...
double x() const
Common members of all points & poses classes.
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
void setPointFast(size_t index, float x, float y, float z)
Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and wi...
mrpt::maps::CPointsMap::Ptr pointcloud
The pointcloud.
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.
virtual void insertPointRGB(float x, float y, float z, float R, float G, float B)
overload (RGB data is ignored in classes without color information)
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.
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
std::vector< uint8_t > intensity
Color [0,255].
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
constexpr std::size_t size() const
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.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
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).
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 void setPointRGB(size_t index, float x, float y, float z, float R, float G, float B)
overload (RGB data is ignored in classes without color information)
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.
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.
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()
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.
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.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
double phi
Orientation (rads)
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.
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".
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.