53 : insertionOptions(), likelihoodOptions(), m_x(), m_y(), m_z()
62 CPointsMap::~CPointsMap() =
default;
64 bool CPointsMap::save2D_to_text_file(
const string& file)
const 68 for (
size_t i = 0; i < m_x.size(); i++)
74 bool CPointsMap::save3D_to_text_file(
const string& file)
const 79 for (
size_t i = 0; i < m_x.size(); i++)
80 os::fprintf(f,
"%f %f %f\n", m_x[i], m_y[i], m_z[i]);
86 bool CPointsMap::save2D_to_text_stream(std::ostream&
out)
const 89 for (
size_t i = 0; i < m_x.size(); i++)
91 os::sprintf(lin,
sizeof(lin),
"%f %f\n", m_x[i], m_y[i]);
96 bool CPointsMap::save3D_to_text_stream(std::ostream&
out)
const 99 for (
size_t i = 0; i < m_x.size(); i++)
101 os::sprintf(lin,
sizeof(lin),
"%f %f %f\n", m_x[i], m_y[i], m_z[i]);
107 bool CPointsMap::load2Dor3D_from_text_stream(
118 for (std::string line; std::getline(in, line); ++linIdx)
121 std::stringstream ss(line);
122 for (
int idxCoord = 0; idxCoord < (is_3D ? 3 : 2); idxCoord++)
124 if (!(ss >> coords[idxCoord]))
126 std::stringstream sErr;
127 sErr <<
"[CPointsMap::load2Dor3D_from_text_stream] Unexpected " 129 << linIdx <<
" for coordinate #" << (idxCoord + 1) <<
"\n";
132 outErrorMsg.value().get() = sErr.str();
134 std::cerr << sErr.str();
140 this->insertPoint(coords[0], coords[1], coords[2]);
146 bool CPointsMap::load2Dor3D_from_text_file(
147 const std::string& file,
const bool is_3D)
155 std::ifstream fi(file);
156 if (!fi.is_open())
return false;
158 return load2Dor3D_from_text_stream(fi, std::nullopt, is_3D);
171 mxArray* CPointsMap::writeToMatlab()
const 174 MRPT_TODO(
"Create 3xN array xyz of points coordinates")
175 const
char* fields[] = {
"x",
"y",
"z"};
176 mexplus::MxArray map_struct(
177 mexplus::MxArray::Struct(
sizeof(fields) /
sizeof(fields[0]), fields));
179 map_struct.set(
"x", m_x);
180 map_struct.set(
"y", m_y);
181 map_struct.set(
"z", m_z);
182 return map_struct.release();
192 void CPointsMap::getPoint(
size_t index,
float& x,
float& y)
const 198 void CPointsMap::getPoint(
size_t index,
float& x,
float& y,
float& z)
const 205 void CPointsMap::getPoint(
size_t index,
double& x,
double& y)
const 211 void CPointsMap::getPoint(
size_t index,
double& x,
double& y,
double& z)
const 222 void CPointsMap::getPointsBuffer(
223 size_t& outPointsCount,
const float*& xs,
const float*& ys,
224 const float*& zs)
const 226 outPointsCount =
size();
228 if (outPointsCount > 0)
236 xs = ys = zs =
nullptr;
243 void CPointsMap::clipOutOfRangeInZ(
float zMin,
float zMax)
245 const size_t n =
size();
246 vector<bool> deletionMask(n);
249 for (
size_t i = 0; i < n; i++)
250 deletionMask[i] = (m_z[i] < zMin || m_z[i] > zMax);
253 applyDeletionMask(deletionMask);
261 void CPointsMap::clipOutOfRange(
const TPoint2D& p,
float maxRange)
263 size_t i, n =
size();
264 vector<bool> deletionMask;
267 deletionMask.resize(n);
269 const auto max_sq = maxRange * maxRange;
272 for (i = 0; i < n; i++)
273 deletionMask[i] =
square(p.
x - m_x[i]) +
square(p.
y - m_y[i]) > max_sq;
276 applyDeletionMask(deletionMask);
281 void CPointsMap::determineMatching2D(
292 params.offset_other_map_points,
params.decimation_other_map_points);
294 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
297 otherMapPose_.
x(), otherMapPose_.
y(), otherMapPose_.
phi());
299 const size_t nLocalPoints = otherMap->
size();
300 const size_t nGlobalPoints = this->
size();
301 float _sumSqrDist = 0;
302 size_t _sumSqrCount = 0;
303 size_t nOtherMapPointsWithCorrespondence =
306 float local_x_min = std::numeric_limits<float>::max(),
307 local_x_max = -std::numeric_limits<float>::max();
308 float global_x_min = std::numeric_limits<float>::max(),
309 global_x_max = -std::numeric_limits<float>::max();
310 float local_y_min = std::numeric_limits<float>::max(),
311 local_y_max = -std::numeric_limits<float>::max();
312 float global_y_min = std::numeric_limits<float>::max(),
313 global_y_max = -std::numeric_limits<float>::max();
315 double maxDistForCorrespondenceSquared;
316 float x_local, y_local;
317 unsigned int localIdx;
319 const float *x_other_it, *y_other_it, *z_other_it;
322 correspondences.clear();
323 correspondences.reserve(nLocalPoints);
324 extraResults.correspondencesRatio = 0;
327 _correspondences.reserve(nLocalPoints);
330 if (!nGlobalPoints || !nLocalPoints)
return;
332 const double sin_phi = sin(otherMapPose.phi);
333 const double cos_phi = cos(otherMapPose.phi);
341 size_t nPackets = nLocalPoints / 4;
343 Eigen::ArrayXf x_locals(nLocalPoints), y_locals(nLocalPoints);
346 const __m128 cos_4val = _mm_set1_ps(cos_phi);
347 const __m128 sin_4val = _mm_set1_ps(sin_phi);
348 const __m128 x0_4val = _mm_set1_ps(otherMapPose.x);
349 const __m128 y0_4val = _mm_set1_ps(otherMapPose.y);
352 __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
353 __m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
354 __m128 y_mins = x_mins;
355 __m128 y_maxs = x_maxs;
357 const float* ptr_in_x = &otherMap->m_x[0];
358 const float* ptr_in_y = &otherMap->m_y[0];
359 float* ptr_out_x = &x_locals[0];
360 float* ptr_out_y = &y_locals[0];
362 for (; nPackets; nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_out_x += 4,
365 const __m128 xs = _mm_loadu_ps(ptr_in_x);
366 const __m128 ys = _mm_loadu_ps(ptr_in_y);
368 const __m128 lxs = _mm_add_ps(
370 _mm_sub_ps(_mm_mul_ps(xs, cos_4val), _mm_mul_ps(ys, sin_4val)));
371 const __m128 lys = _mm_add_ps(
373 _mm_add_ps(_mm_mul_ps(xs, sin_4val), _mm_mul_ps(ys, cos_4val)));
374 _mm_store_ps(ptr_out_x, lxs);
375 _mm_store_ps(ptr_out_y, lys);
377 x_mins = _mm_min_ps(x_mins, lxs);
378 x_maxs = _mm_max_ps(x_maxs, lxs);
379 y_mins = _mm_min_ps(y_mins, lys);
380 y_maxs = _mm_max_ps(y_maxs, lys);
384 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
float temp_nums[4];
386 _mm_store_ps(temp_nums, x_mins);
388 min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
389 _mm_store_ps(temp_nums, y_mins);
391 min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
392 _mm_store_ps(temp_nums, x_maxs);
394 max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
395 _mm_store_ps(temp_nums, y_maxs);
397 max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
400 for (
size_t k = 0; k < nLocalPoints % 4; k++)
402 float x = ptr_in_x[k];
403 float y = ptr_in_y[k];
404 float out_x = otherMapPose.x + cos_phi * x - sin_phi * y;
405 float out_y = otherMapPose.y + sin_phi * x + cos_phi * y;
407 local_x_min = std::min(local_x_min, out_x);
408 local_x_max = std::max(local_x_max, out_x);
410 local_y_min = std::min(local_y_min, out_y);
411 local_y_max = std::max(local_y_max, out_y);
413 ptr_out_x[k] = out_x;
414 ptr_out_y[k] = out_y;
420 const_cast<float*>(&otherMap->m_x[0]), otherMap->m_x.size(), 1);
422 const_cast<float*>(&otherMap->m_y[0]), otherMap->m_y.size(), 1);
424 Eigen::Array<float, Eigen::Dynamic, 1> x_locals =
425 otherMapPose.x + cos_phi * x_org.array() - sin_phi * y_org.array();
426 Eigen::Array<float, Eigen::Dynamic, 1> y_locals =
427 otherMapPose.y + sin_phi * x_org.array() + cos_phi * y_org.array();
429 local_x_min = x_locals.minCoeff();
430 local_y_min = y_locals.minCoeff();
431 local_x_max = x_locals.maxCoeff();
432 local_y_max = y_locals.maxCoeff();
436 float global_z_min, global_z_max;
438 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
443 if (local_x_min > global_x_max || local_x_max < global_x_min ||
444 local_y_min > global_y_max || local_y_max < global_y_min)
449 for (localIdx =
params.offset_other_map_points,
450 x_other_it = &otherMap->m_x[
params.offset_other_map_points],
451 y_other_it = &otherMap->m_y[
params.offset_other_map_points],
452 z_other_it = &otherMap->m_z[
params.offset_other_map_points];
453 localIdx < nLocalPoints;
454 x_other_it +=
params.decimation_other_map_points,
455 y_other_it +=
params.decimation_other_map_points,
456 z_other_it +=
params.decimation_other_map_points,
457 localIdx +=
params.decimation_other_map_points)
460 x_local = x_locals[localIdx];
461 y_local = y_locals[localIdx];
470 float tentativ_err_sq;
471 unsigned int tentativ_this_idx = kdTreeClosestPoint2D(
477 maxDistForCorrespondenceSquared =
square(
478 params.maxAngularDistForCorrespondence *
482 params.maxDistForCorrespondence);
485 if (tentativ_err_sq < maxDistForCorrespondenceSquared)
488 _correspondences.resize(_correspondences.size() + 1);
493 p.
this_x = m_x[tentativ_this_idx];
494 p.
this_y = m_y[tentativ_this_idx];
495 p.
this_z = m_z[tentativ_this_idx];
505 nOtherMapPointsWithCorrespondence++;
518 if (
params.onlyUniqueRobust)
521 params.onlyKeepTheClosest,
522 "ERROR: onlyKeepTheClosest must be also set to true when " 523 "onlyUniqueRobust=true.");
525 nGlobalPoints, correspondences);
529 correspondences.swap(_correspondences);
535 extraResults.sumSqrDist =
536 _sumSqrDist /
static_cast<double>(_sumSqrCount);
538 extraResults.sumSqrDist = 0;
541 extraResults.correspondencesRatio =
params.decimation_other_map_points *
542 nOtherMapPointsWithCorrespondence /
551 void CPointsMap::changeCoordinatesReference(
const CPose2D& newBase)
553 const size_t N = m_x.size();
555 const CPose3D newBase3D(newBase);
557 for (
size_t i = 0; i < N; i++)
559 m_x[i], m_y[i], m_z[i],
560 m_x[i], m_y[i], m_z[i]
569 void CPointsMap::changeCoordinatesReference(
const CPose3D& newBase)
571 const size_t N = m_x.size();
573 for (
size_t i = 0; i < N; i++)
575 m_x[i], m_y[i], m_z[i],
576 m_x[i], m_y[i], m_z[i]
585 void CPointsMap::changeCoordinatesReference(
589 changeCoordinatesReference(newBase);
595 bool CPointsMap::isEmpty()
const {
return m_x.empty(); }
599 CPointsMap::TInsertionOptions::TInsertionOptions()
600 : horizontalTolerance(0.05_deg)
609 const int8_t version = 0;
612 out << minDistBetweenLaserPoints << addToExistingPointsMap
613 << also_interpolate << disableDeletion << fuseWithExisting
614 << isPlanarMap << horizontalTolerance << maxDistForInterpolatePoints
615 << insertInvalidPoints;
627 in >> minDistBetweenLaserPoints >> addToExistingPointsMap >>
628 also_interpolate >> disableDeletion >> fuseWithExisting >>
629 isPlanarMap >> horizontalTolerance >>
630 maxDistForInterpolatePoints >> insertInvalidPoints;
645 const int8_t version = 0;
647 out << sigma_dist << max_corr_distance << decimation;
659 in >> sigma_dist >> max_corr_distance >> decimation;
670 const int8_t version = 0;
684 in >> point_size >> this->color;
695 out <<
"\n----------- [CPointsMap::TInsertionOptions] ------------ \n\n";
714 out <<
"\n----------- [CPointsMap::TLikelihoodOptions] ------------ \n\n";
723 out <<
"\n----------- [CPointsMap::TRenderOptions] ------------ \n\n";
781 obj->loadFromPointsMap(
this);
784 obj->enableColorFromZ(
false);
790 obj->loadFromPointsMap(
this);
794 obj->recolorizeByCoordinate(
828 float maxDistSq = 0, d;
829 for (
auto X =
m_x.begin(), Y =
m_y.begin(), Z =
m_z.begin();
830 X !=
m_x.end(); ++X, ++Y, ++Z)
833 maxDistSq = max(d, maxDistSq);
846 vector<float>& xs, vector<float>& ys,
size_t decimation)
const 852 xs = vector<float>(
m_x.begin(),
m_x.end());
853 ys = vector<float>(
m_y.begin(),
m_y.end());
857 size_t N =
m_x.size() / decimation;
862 auto X =
m_x.begin();
863 auto Y =
m_y.begin();
864 for (
auto oX = xs.begin(), oY = ys.begin(); oX != xs.end();
865 X += decimation, Y += decimation, ++oX, ++oY)
878 float x0,
float y0)
const 886 float x1, y1, x2, y2, d1, d2;
897 if (d12 > 0.20f * 0.20f || d12 < 0.03f * 0.03f)
903 double interp_x, interp_y;
921 float& min_x,
float& max_x,
float& min_y,
float& max_y,
float& min_z,
926 const size_t nPoints =
m_x.size();
941 size_t nPackets = nPoints / 4;
944 __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
945 __m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
946 __m128 y_mins = x_mins, y_maxs = x_maxs;
947 __m128 z_mins = x_mins, z_maxs = x_maxs;
949 const float* ptr_in_x = &
m_x[0];
950 const float* ptr_in_y = &
m_y[0];
951 const float* ptr_in_z = &
m_z[0];
954 nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_in_z += 4)
956 const __m128 xs = _mm_loadu_ps(ptr_in_x);
957 x_mins = _mm_min_ps(x_mins, xs);
958 x_maxs = _mm_max_ps(x_maxs, xs);
960 const __m128 ys = _mm_loadu_ps(ptr_in_y);
961 y_mins = _mm_min_ps(y_mins, ys);
962 y_maxs = _mm_max_ps(y_maxs, ys);
964 const __m128 zs = _mm_loadu_ps(ptr_in_z);
965 z_mins = _mm_min_ps(z_mins, zs);
966 z_maxs = _mm_max_ps(z_maxs, zs);
970 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
float temp_nums[4];
972 _mm_store_ps(temp_nums, x_mins);
974 min(min(temp_nums[0], temp_nums[1]),
975 min(temp_nums[2], temp_nums[3]));
976 _mm_store_ps(temp_nums, y_mins);
978 min(min(temp_nums[0], temp_nums[1]),
979 min(temp_nums[2], temp_nums[3]));
980 _mm_store_ps(temp_nums, z_mins);
982 min(min(temp_nums[0], temp_nums[1]),
983 min(temp_nums[2], temp_nums[3]));
984 _mm_store_ps(temp_nums, x_maxs);
986 max(max(temp_nums[0], temp_nums[1]),
987 max(temp_nums[2], temp_nums[3]));
988 _mm_store_ps(temp_nums, y_maxs);
990 max(max(temp_nums[0], temp_nums[1]),
991 max(temp_nums[2], temp_nums[3]));
992 _mm_store_ps(temp_nums, z_maxs);
994 max(max(temp_nums[0], temp_nums[1]),
995 max(temp_nums[2], temp_nums[3]));
998 for (
size_t k = 0; k < nPoints % 4; k++)
1012 (std::numeric_limits<float>::max)();
1015 -(std::numeric_limits<float>::max)();
1017 for (
auto xi =
m_x.begin(), yi =
m_y.begin(), zi =
m_z.begin();
1018 xi !=
m_x.end(); xi++, yi++, zi++)
1055 params.offset_other_map_points,
params.decimation_other_map_points);
1058 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
1060 const size_t nLocalPoints = otherMap->
size();
1061 const size_t nGlobalPoints = this->
size();
1062 float _sumSqrDist = 0;
1063 size_t _sumSqrCount = 0;
1064 size_t nOtherMapPointsWithCorrespondence =
1067 float local_x_min = std::numeric_limits<float>::max(),
1068 local_x_max = -std::numeric_limits<float>::max();
1069 float local_y_min = std::numeric_limits<float>::max(),
1070 local_y_max = -std::numeric_limits<float>::max();
1071 float local_z_min = std::numeric_limits<float>::max(),
1072 local_z_max = -std::numeric_limits<float>::max();
1074 double maxDistForCorrespondenceSquared;
1077 correspondences.clear();
1078 correspondences.reserve(nLocalPoints);
1081 _correspondences.reserve(nLocalPoints);
1084 if (!nGlobalPoints || !nLocalPoints)
return;
1088 vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
1089 z_locals(nLocalPoints);
1091 for (
unsigned int localIdx =
params.offset_other_map_points;
1092 localIdx < nLocalPoints;
1093 localIdx +=
params.decimation_other_map_points)
1095 float x_local, y_local, z_local;
1097 otherMap->m_x[localIdx], otherMap->m_y[localIdx],
1098 otherMap->m_z[localIdx], x_local, y_local, z_local);
1100 x_locals[localIdx] = x_local;
1101 y_locals[localIdx] = y_local;
1102 z_locals[localIdx] = z_local;
1105 local_x_min = min(local_x_min, x_local);
1106 local_x_max = max(local_x_max, x_local);
1107 local_y_min = min(local_y_min, y_local);
1108 local_y_max = max(local_y_max, y_local);
1109 local_z_min = min(local_z_min, z_local);
1110 local_z_max = max(local_z_max, z_local);
1114 float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1117 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1122 if (local_x_min > global_x_max || local_x_max < global_x_min ||
1123 local_y_min > global_y_max || local_y_max < global_y_min)
1128 for (
unsigned int localIdx =
params.offset_other_map_points;
1129 localIdx < nLocalPoints;
1130 localIdx +=
params.decimation_other_map_points)
1133 const float x_local = x_locals[localIdx];
1134 const float y_local = y_locals[localIdx];
1135 const float z_local = z_locals[localIdx];
1143 float tentativ_err_sq;
1145 x_local, y_local, z_local,
1150 maxDistForCorrespondenceSquared =
square(
1151 params.maxAngularDistForCorrespondence *
1152 params.angularDistPivotPoint.distanceTo(
1153 TPoint3D(x_local, y_local, z_local)) +
1154 params.maxDistForCorrespondence);
1157 if (tentativ_err_sq < maxDistForCorrespondenceSquared)
1160 _correspondences.resize(_correspondences.size() + 1);
1170 p.
other_x = otherMap->m_x[localIdx];
1171 p.
other_y = otherMap->m_y[localIdx];
1172 p.
other_z = otherMap->m_z[localIdx];
1177 nOtherMapPointsWithCorrespondence++;
1191 if (
params.onlyUniqueRobust)
1194 params.onlyKeepTheClosest,
1195 "ERROR: onlyKeepTheClosest must be also set to true when " 1196 "onlyUniqueRobust=true.");
1198 nGlobalPoints, correspondences);
1202 correspondences.swap(_correspondences);
1207 extraResults.sumSqrDist =
1208 (_sumSqrCount) ? _sumSqrDist / static_cast<double>(_sumSqrCount) : 0;
1209 extraResults.correspondencesRatio =
params.decimation_other_map_points *
1210 nOtherMapPointsWithCorrespondence /
1220 const TPoint2D& center,
const double radius,
const double zmin,
1224 for (
size_t k = 0; k <
m_x.size(); k++)
1226 if ((
m_z[k] <= zmax &&
m_z[k] >= zmin) &&
1238 double R,
double G,
double B)
1241 double minX, maxX, minY, maxY, minZ, maxZ;
1242 minX = min(corner1.
x, corner2.
x);
1243 maxX = max(corner1.
x, corner2.
x);
1244 minY = min(corner1.
y, corner2.
y);
1245 maxY = max(corner1.
y, corner2.
y);
1246 minZ = min(corner1.
z, corner2.
z);
1247 maxZ = max(corner1.
z, corner2.
z);
1248 for (
size_t k = 0; k <
m_x.size(); k++)
1250 if ((
m_x[k] >= minX &&
m_x[k] <= maxX) &&
1251 (
m_y[k] >= minY &&
m_y[k] <= maxY) &&
1252 (
m_z[k] >= minZ &&
m_z[k] <= maxZ))
1262 [[maybe_unused]]
const CPose3D& otherMapPose,
1264 float& correspondencesRatio)
1268 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
1270 const size_t nLocalPoints = otherMap->
size();
1271 const size_t nGlobalPoints = this->
size();
1272 size_t nOtherMapPointsWithCorrespondence =
1276 correspondences.clear();
1277 correspondences.reserve(nLocalPoints);
1278 correspondencesRatio = 0;
1282 _correspondences.reserve(nLocalPoints);
1285 if (!nGlobalPoints)
return;
1288 if (!nLocalPoints)
return;
1291 float local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1293 otherMap->boundingBox(
1294 local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1298 float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1301 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1306 if (local_x_min > global_x_max || local_x_max < global_x_min ||
1307 local_y_min > global_y_max || local_y_max < global_y_min)
1311 std::vector<std::vector<size_t>> vIdx;
1315 std::vector<float> outX, outY, outZ, tentativeErrSq;
1316 std::vector<size_t> outIdx;
1317 for (
unsigned int localIdx = 0; localIdx < nLocalPoints; ++localIdx)
1320 const float x_local = otherMap->m_x[localIdx];
1321 const float y_local = otherMap->m_y[localIdx];
1322 const float z_local = otherMap->m_z[localIdx];
1330 x_local, y_local, z_local,
1338 const float mX = (outX[0] + outX[1] + outX[2]) / 3.0;
1339 const float mY = (outY[0] + outY[1] + outY[2]) / 3.0;
1340 const float mZ = (outZ[0] + outZ[1] + outZ[2]) / 3.0;
1346 if (distanceForThisPoint < maxDistForCorrespondence)
1349 _correspondences.resize(_correspondences.size() + 1);
1353 p.
this_idx = nOtherMapPointsWithCorrespondence++;
1361 p.
other_x = otherMap->m_x[localIdx];
1362 p.
other_y = otherMap->m_y[localIdx];
1363 p.
other_z = otherMap->m_z[localIdx];
1368 std::sort(outIdx.begin(), outIdx.end());
1369 vIdx.push_back(outIdx);
1378 std::map<size_t, std::map<size_t, std::map<size_t, pair<size_t, float>>>>
1380 TMatchingPairList::iterator it;
1381 for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1383 const size_t i0 = vIdx[it->this_idx][0];
1384 const size_t i1 = vIdx[it->this_idx][1];
1385 const size_t i2 = vIdx[it->this_idx][2];
1387 if (best.find(i0) != best.end() &&
1388 best[i0].find(i1) != best[i0].end() &&
1389 best[i0][i1].find(i2) !=
1393 if (best[i0][i1][i2].second > it->errorSquareAfterTransformation)
1395 best[i0][i1][i2].first = it->this_idx;
1396 best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1401 best[i0][i1][i2].first = it->this_idx;
1402 best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1406 for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1408 const size_t i0 = vIdx[it->this_idx][0];
1409 const size_t i1 = vIdx[it->this_idx][1];
1410 const size_t i2 = vIdx[it->this_idx][2];
1412 if (best[i0][i1][i2].first == it->this_idx)
1413 correspondences.push_back(*it);
1417 correspondencesRatio =
1418 nOtherMapPointsWithCorrespondence /
d2f(nLocalPoints);
1425 const float* zs,
const std::size_t num_pts)
1429 float closest_x, closest_y, closest_z;
1432 double sumSqrDist = 0;
1434 std::size_t nPtsForAverage = 0;
1435 for (std::size_t i = 0; i < num_pts;
1441 pc_in_map.
composePoint(xs[i], ys[i], zs[i], xg, yg, zg);
1445 closest_x, closest_y,
1453 sumSqrDist +=
static_cast<double>(closest_err);
1455 if (nPtsForAverage) sumSqrDist /= nPtsForAverage;
1478 const size_t N = scanPoints->
m_x.size();
1479 if (!N || !this->
size())
return -100;
1481 const float* xs = &scanPoints->m_x[0];
1482 const float* ys = &scanPoints->m_y[0];
1483 const float* zs = &scanPoints->m_z[0];
1487 double sumSqrDist = 0;
1488 float closest_x, closest_y;
1490 const float max_sqr_err =
1496 const double ccos = cos(takenFrom2D.
phi);
1497 const double csin = sin(takenFrom2D.
phi);
1498 int nPtsForAverage = 0;
1500 for (
size_t i = 0; i < N;
1505 const float xg = takenFrom2D.
x + ccos * xs[i] - csin * ys[i];
1506 const float yg = takenFrom2D.
y + csin * xs[i] + ccos * ys[i];
1510 closest_x, closest_y,
1517 sumSqrDist +=
static_cast<double>(closest_err);
1519 sumSqrDist /= nPtsForAverage;
1527 takenFrom, xs, ys, zs, N);
1535 if (!o.point_cloud.size())
1538 const size_t N = o.point_cloud.size();
1539 if (!N || !this->
size())
return -100;
1541 const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1543 const float* xs = &o.point_cloud.
x[0];
1544 const float* ys = &o.point_cloud.y[0];
1545 const float* zs = &o.point_cloud.z[0];
1548 sensorAbsPose, xs, ys, zs, N);
1555 if (!N || !this->
size())
return -100;
1557 const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1559 auto xs = o.pointcloud->getPointsBufferRef_x();
1560 auto ys = o.pointcloud->getPointsBufferRef_y();
1561 auto zs = o.pointcloud->getPointsBufferRef_z();
1564 sensorAbsPose, &xs[0], &ys[0], &zs[0], N);
1587 if (out_map)
return;
1589 out_map = std::make_shared<CSimplePointsMap>();
1593 *static_cast<const CPointsMap::TInsertionOptions*>(insertOps);
1595 out_map->insertObservation(obs,
nullptr);
1635 pt_has_color =
false;
1646 const size_t nThis = this->
size();
1647 const size_t nOther = anotherMap.
size();
1649 const size_t nTot = nThis + nOther;
1653 for (
size_t i = 0, j = nThis; i < nOther; i++, j++)
1655 m_x[j] = anotherMap.
m_x[i];
1656 m_y[j] = anotherMap.
m_y[i];
1657 m_z[j] = anotherMap.
m_z[i];
1673 const size_t n = mask.size();
1676 for (i = 0, j = 0; i < n; i++)
1698 const size_t N_this =
size();
1699 const size_t N_other = otherMap->
size();
1702 this->
resize(N_this + N_other);
1706 for (src = 0, dst = N_this; src < N_other; src++, dst++)
1730 if (
this == &obj)
return;
1763 robotPose2D =
CPose2D(*robotPose);
1764 robotPose3D = (*robotPose);
1780 bool reallyInsertIt;
1786 reallyInsertIt =
true;
1790 std::vector<bool> checkForDeletion;
1821 const float *xs, *ys, *zs;
1828 for (
size_t i = 0; i < n; i++)
1830 if (checkForDeletion[i])
1838 checkForDeletion[i] =
1874 bool reallyInsertIt;
1880 reallyInsertIt =
true;
1934 this->
size() + o.sensedData.size() * 30);
1936 for (
auto it = o.begin(); it != o.end(); ++it)
1938 const CPose3D sensorPose = robotPose3D +
CPose3D(it->sensorPose);
1939 const double rang = it->sensedDistance;
1941 if (rang <= 0 || rang < o.minSensorDistance ||
1942 rang > o.maxSensorDistance)
1947 const double arc_len = o.sensorConeApperture * rang;
1948 const unsigned int nSteps =
round(1 + arc_len / 0.05);
1949 const double Aa = o.sensorConeApperture / double(nSteps);
1952 for (
double a1 = -aper_2;
a1 < aper_2;
a1 += Aa)
1954 for (
double a2 = -aper_2;
a2 < aper_2;
a2 += Aa)
1956 loc.
x = cos(
a1) * cos(
a2) * rang;
1957 loc.
y = cos(
a1) * sin(
a2) * rang;
1958 loc.
z = sin(
a1) * rang;
1977 if (!o.point_cloud.size())
2037 std::vector<bool>* notFusedPoints)
2041 const CPose2D nullPose(0, 0, 0);
2046 const size_t nOther = otherMap->
size();
2053 params.maxAngularDistForCorrespondence = 0;
2054 params.maxDistForCorrespondence = minDistForFuse;
2059 correspondences,
params, extraResults);
2064 notFusedPoints->clear();
2065 notFusedPoints->reserve(
m_x.size() + nOther);
2066 notFusedPoints->resize(
m_x.size(),
true);
2075 for (
size_t i = 0; i < nOther; i++)
2081 int closestCorr = -1;
2082 float minDist = std::numeric_limits<float>::max();
2083 for (
auto corrsIt = correspondences.begin();
2084 corrsIt != correspondences.end(); ++corrsIt)
2086 if (corrsIt->other_idx == i)
2088 float dist =
square(corrsIt->other_x - corrsIt->this_x) +
2089 square(corrsIt->other_y - corrsIt->this_y) +
2090 square(corrsIt->other_z - corrsIt->this_z);
2094 closestCorr = corrsIt->this_idx;
2099 if (closestCorr != -1)
2106 const float F = 1.0f / (w_a + w_b);
2108 m_x[closestCorr] = F * (w_a * a.
x + w_b * b.
x);
2109 m_y[closestCorr] = F * (w_a * a.
y + w_b * b.
y);
2110 m_z[closestCorr] = F * (w_a * a.
z + w_b * b.
z);
2115 if (notFusedPoints) (*notFusedPoints)[closestCorr] =
false;
2120 if (notFusedPoints) (*notFusedPoints).push_back(
false);
2143 const size_t nOldPtsCount = this->
size();
2145 const size_t nNewPtsCount = nOldPtsCount + nScanPts;
2146 this->
resize(nNewPtsCount);
2148 const float K = 1.0f / 255;
2153 sensorGlobalPose = *robotPose + scan.
sensorPose;
2160 const double m00 = HM(0, 0), m01 = HM(0, 1), m02 = HM(0, 2), m03 = HM(0, 3);
2161 const double m10 = HM(1, 0), m11 = HM(1, 1), m12 = HM(1, 2), m13 = HM(1, 3);
2162 const double m20 = HM(2, 0), m21 = HM(2, 1), m22 = HM(2, 2), m23 = HM(2, 3);
2165 for (
size_t i = 0; i < nScanPts; i++)
2172 const double gx = m00 * lx + m01 * ly + m02 * lz + m03;
2173 const double gy = m10 * lx + m11 * ly + m12 * lz + m13;
2174 const double gz = m20 * lx + m21 * ly + m22 * lz + m23;
2177 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.
float errorSquareAfterTransformation
void closestFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
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 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...
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.
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)
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
#define THROW_EXCEPTION(msg)
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 ...
size_t size(const MATRIXLIKE &m, const int dim)
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 ...
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.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
void loadFromVelodyneScan(const mrpt::obs::CObservationVelodyneScan &scan, const mrpt::poses::CPose3D *robotPose=nullptr)
Like loadFromRangeScan() for Velodyne 3D scans.
mrpt::vision::TStereoCalibParams params
A wrapper of a TPolygon2D class, implementing CSerializable.
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.
virtual void insertPointRGB(float x, float y, float z, [[maybe_unused]] float R, [[maybe_unused]] float G, [[maybe_unused]] float B)
overload (RGB data is ignored in classes without color information)
bool m_boundingBoxIsUpdated
void generatePointCloud(const TGeneratePointCloudParameters ¶ms=TGeneratePointCloudParameters())
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
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...
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.
virtual unsigned int getPointWeight([[maybe_unused]] size_t index) const
Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually st...
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.
#define MRPT_TRY_START
The start of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ...
#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...
float d2f(const double d)
shortcut for static_cast<float>(double)
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.
double phi() const
Get the phi angle of the 2D pose (in radians)
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"))
void extractPoints(const mrpt::math::TPoint3D &corner1, const mrpt::math::TPoint3D &corner2, CPointsMap *outMap, double R=1, double G=1, double B=1)
Extracts the points in the map within the area defined by two corners.
#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 ...
TPoint3D_< double > TPoint3D
Lightweight 3D point.
virtual void setPointWeight([[maybe_unused]] size_t index, [[maybe_unused]] 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.
double x() const
Common members of all points & poses classes.
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".
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.
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) ...
Undefined colormap [New in MRPT 2.0].
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.
return_t square(const num_t x)
Inline function for the square of a number.
#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).
mrpt::vision::TStereoCalibResults out
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 kdtree_mark_as_outdated() const
To be called by child classes when KD tree data changes.
#define ASSERT_ABOVE_(__A, __B)
An RGBA 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: ...
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.
MRPT_TODO("toPointCloud / calibration")
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.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, 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...
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 ...
static 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.
virtual void setPointRGB(size_t index, float x, float y, float z, [[maybe_unused]] float R, [[maybe_unused]] float G, [[maybe_unused]] float B)
overload (RGB data is ignored in classes without color information)
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) ...
Parameters for the determination of matchings between point clouds, etc.
mrpt::aligned_std_vector< float > m_x
The point coordinates.
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
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...
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".
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
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...
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.