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>();
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,