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 tempCorrs.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 tempCorrs.resize(tempCorrs.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.");
528 correspondences.swap(tempCorrs);
534 extraResults.sumSqrDist =
535 _sumSqrDist /
static_cast<double>(_sumSqrCount);
537 extraResults.sumSqrDist = 0;
540 extraResults.correspondencesRatio =
params.decimation_other_map_points *
541 nOtherMapPointsWithCorrespondence /
550 void CPointsMap::changeCoordinatesReference(
const CPose2D& newBase)
552 const size_t N = m_x.size();
554 const CPose3D newBase3D(newBase);
556 for (
size_t i = 0; i < N; i++)
558 m_x[i], m_y[i], m_z[i],
559 m_x[i], m_y[i], m_z[i]
568 void CPointsMap::changeCoordinatesReference(
const CPose3D& newBase)
570 const size_t N = m_x.size();
572 for (
size_t i = 0; i < N; i++)
574 m_x[i], m_y[i], m_z[i],
575 m_x[i], m_y[i], m_z[i]
584 void CPointsMap::changeCoordinatesReference(
588 changeCoordinatesReference(newBase);
594 bool CPointsMap::isEmpty()
const {
return m_x.empty(); }
598 CPointsMap::TInsertionOptions::TInsertionOptions()
599 : horizontalTolerance(0.05_deg)
608 const int8_t version = 0;
611 out << minDistBetweenLaserPoints << addToExistingPointsMap
612 << also_interpolate << disableDeletion << fuseWithExisting
613 << isPlanarMap << horizontalTolerance << maxDistForInterpolatePoints
614 << insertInvalidPoints;
626 in >> minDistBetweenLaserPoints >> addToExistingPointsMap >>
627 also_interpolate >> disableDeletion >> fuseWithExisting >>
628 isPlanarMap >> horizontalTolerance >>
629 maxDistForInterpolatePoints >> insertInvalidPoints;
644 const int8_t version = 0;
646 out << sigma_dist << max_corr_distance << decimation;
658 in >> sigma_dist >> max_corr_distance >> decimation;
669 const int8_t version = 0;
683 in >> point_size >> this->color;
694 out <<
"\n----------- [CPointsMap::TInsertionOptions] ------------ \n\n";
713 out <<
"\n----------- [CPointsMap::TLikelihoodOptions] ------------ \n\n";
722 out <<
"\n----------- [CPointsMap::TRenderOptions] ------------ \n\n";
780 obj->loadFromPointsMap(
this);
783 obj->enableColorFromZ(
false);
789 obj->loadFromPointsMap(
this);
793 obj->recolorizeByCoordinate(
827 float maxDistSq = 0, d;
828 for (
auto X =
m_x.begin(), Y =
m_y.begin(), Z =
m_z.begin();
829 X !=
m_x.end(); ++X, ++Y, ++Z)
832 maxDistSq = max(d, maxDistSq);
845 vector<float>& xs, vector<float>& ys,
size_t decimation)
const
851 xs = vector<float>(
m_x.begin(),
m_x.end());
852 ys = vector<float>(
m_y.begin(),
m_y.end());
856 size_t N =
m_x.size() / decimation;
861 auto X =
m_x.begin();
862 auto Y =
m_y.begin();
863 for (
auto oX = xs.begin(), oY = ys.begin(); oX != xs.end();
864 X += decimation, Y += decimation, ++oX, ++oY)
877 float x0,
float y0)
const
885 float x1, y1, x2, y2, d1, d2;
896 if (d12 > 0.20f * 0.20f || d12 < 0.03f * 0.03f)
902 double interp_x, interp_y;
920 float& min_x,
float& max_x,
float& min_y,
float& max_y,
float& min_z,
925 const size_t nPoints =
m_x.size();
940 size_t nPackets = nPoints / 4;
943 __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
944 __m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
945 __m128 y_mins = x_mins, y_maxs = x_maxs;
946 __m128 z_mins = x_mins, z_maxs = x_maxs;
948 const float* ptr_in_x = &
m_x[0];
949 const float* ptr_in_y = &
m_y[0];
950 const float* ptr_in_z = &
m_z[0];
953 nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_in_z += 4)
955 const __m128 xs = _mm_loadu_ps(ptr_in_x);
956 x_mins = _mm_min_ps(x_mins, xs);
957 x_maxs = _mm_max_ps(x_maxs, xs);
959 const __m128 ys = _mm_loadu_ps(ptr_in_y);
960 y_mins = _mm_min_ps(y_mins, ys);
961 y_maxs = _mm_max_ps(y_maxs, ys);
963 const __m128 zs = _mm_loadu_ps(ptr_in_z);
964 z_mins = _mm_min_ps(z_mins, zs);
965 z_maxs = _mm_max_ps(z_maxs, zs);
969 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
float temp_nums[4];
971 _mm_store_ps(temp_nums, x_mins);
973 min(min(temp_nums[0], temp_nums[1]),
974 min(temp_nums[2], temp_nums[3]));
975 _mm_store_ps(temp_nums, y_mins);
977 min(min(temp_nums[0], temp_nums[1]),
978 min(temp_nums[2], temp_nums[3]));
979 _mm_store_ps(temp_nums, z_mins);
981 min(min(temp_nums[0], temp_nums[1]),
982 min(temp_nums[2], temp_nums[3]));
983 _mm_store_ps(temp_nums, x_maxs);
985 max(max(temp_nums[0], temp_nums[1]),
986 max(temp_nums[2], temp_nums[3]));
987 _mm_store_ps(temp_nums, y_maxs);
989 max(max(temp_nums[0], temp_nums[1]),
990 max(temp_nums[2], temp_nums[3]));
991 _mm_store_ps(temp_nums, z_maxs);
993 max(max(temp_nums[0], temp_nums[1]),
994 max(temp_nums[2], temp_nums[3]));
997 for (
size_t k = 0; k < nPoints % 4; k++)
1011 (std::numeric_limits<float>::max)();
1014 -(std::numeric_limits<float>::max)();
1016 for (
auto xi =
m_x.begin(), yi =
m_y.begin(), zi =
m_z.begin();
1017 xi !=
m_x.end(); xi++, yi++, zi++)
1054 params.offset_other_map_points,
params.decimation_other_map_points);
1057 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
1059 const size_t nLocalPoints = otherMap->
size();
1060 const size_t nGlobalPoints = this->
size();
1061 float _sumSqrDist = 0;
1062 size_t _sumSqrCount = 0;
1063 size_t nOtherMapPointsWithCorrespondence =
1066 float local_x_min = std::numeric_limits<float>::max(),
1067 local_x_max = -std::numeric_limits<float>::max();
1068 float local_y_min = std::numeric_limits<float>::max(),
1069 local_y_max = -std::numeric_limits<float>::max();
1070 float local_z_min = std::numeric_limits<float>::max(),
1071 local_z_max = -std::numeric_limits<float>::max();
1073 double maxDistForCorrespondenceSquared;
1076 correspondences.clear();
1077 correspondences.reserve(nLocalPoints);
1080 tempCorrs.reserve(nLocalPoints);
1083 if (!nGlobalPoints || !nLocalPoints)
return;
1087 vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
1088 z_locals(nLocalPoints);
1090 for (
unsigned int localIdx =
params.offset_other_map_points;
1091 localIdx < nLocalPoints;
1092 localIdx +=
params.decimation_other_map_points)
1094 float x_local, y_local, z_local;
1096 otherMap->m_x[localIdx], otherMap->m_y[localIdx],
1097 otherMap->m_z[localIdx], x_local, y_local, z_local);
1099 x_locals[localIdx] = x_local;
1100 y_locals[localIdx] = y_local;
1101 z_locals[localIdx] = z_local;
1104 local_x_min = min(local_x_min, x_local);
1105 local_x_max = max(local_x_max, x_local);
1106 local_y_min = min(local_y_min, y_local);
1107 local_y_max = max(local_y_max, y_local);
1108 local_z_min = min(local_z_min, z_local);
1109 local_z_max = max(local_z_max, z_local);
1113 float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1116 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1121 if (local_x_min > global_x_max || local_x_max < global_x_min ||
1122 local_y_min > global_y_max || local_y_max < global_y_min)
1127 for (
unsigned int localIdx =
params.offset_other_map_points;
1128 localIdx < nLocalPoints;
1129 localIdx +=
params.decimation_other_map_points)
1132 const float x_local = x_locals[localIdx];
1133 const float y_local = y_locals[localIdx];
1134 const float z_local = z_locals[localIdx];
1142 float tentativ_err_sq;
1144 x_local, y_local, z_local,
1149 maxDistForCorrespondenceSquared =
square(
1150 params.maxAngularDistForCorrespondence *
1151 params.angularDistPivotPoint.distanceTo(
1152 TPoint3D(x_local, y_local, z_local)) +
1153 params.maxDistForCorrespondence);
1156 if (tentativ_err_sq < maxDistForCorrespondenceSquared)
1159 tempCorrs.resize(tempCorrs.size() + 1);
1169 p.
other_x = otherMap->m_x[localIdx];
1170 p.
other_y = otherMap->m_y[localIdx];
1171 p.
other_z = otherMap->m_z[localIdx];
1176 nOtherMapPointsWithCorrespondence++;
1190 if (
params.onlyUniqueRobust)
1193 params.onlyKeepTheClosest,
1194 "ERROR: onlyKeepTheClosest must be also set to true when "
1195 "onlyUniqueRobust=true.");
1200 correspondences = std::move(tempCorrs);
1205 extraResults.sumSqrDist =
1206 (_sumSqrCount) ? _sumSqrDist /
static_cast<double>(_sumSqrCount) : 0;
1207 extraResults.correspondencesRatio =
params.decimation_other_map_points *
1208 nOtherMapPointsWithCorrespondence /
1218 const TPoint2D& center,
const double radius,
const double zmin,
1222 for (
size_t k = 0; k <
m_x.size(); k++)
1224 if ((
m_z[k] <= zmax &&
m_z[k] >= zmin) &&
1236 double R,
double G,
double B)
1239 double minX, maxX, minY, maxY, minZ, maxZ;
1240 minX = min(corner1.
x, corner2.
x);
1241 maxX = max(corner1.
x, corner2.
x);
1242 minY = min(corner1.
y, corner2.
y);
1243 maxY = max(corner1.
y, corner2.
y);
1244 minZ = min(corner1.
z, corner2.
z);
1245 maxZ = max(corner1.
z, corner2.
z);
1246 for (
size_t k = 0; k <
m_x.size(); k++)
1248 if ((
m_x[k] >= minX &&
m_x[k] <= maxX) &&
1249 (
m_y[k] >= minY &&
m_y[k] <= maxY) &&
1250 (
m_z[k] >= minZ &&
m_z[k] <= maxZ))
1260 [[maybe_unused]]
const CPose3D& otherMapPose,
1262 float& correspondencesRatio)
1266 const auto* otherMap =
static_cast<const CPointsMap*
>(otherMap2);
1268 const size_t nLocalPoints = otherMap->
size();
1269 const size_t nGlobalPoints = this->
size();
1270 size_t nOtherMapPointsWithCorrespondence =
1274 correspondences.clear();
1275 correspondences.reserve(nLocalPoints);
1276 correspondencesRatio = 0;
1280 tempCorrs.reserve(nLocalPoints);
1283 if (!nGlobalPoints)
return;
1286 if (!nLocalPoints)
return;
1289 float local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1291 otherMap->boundingBox(
1292 local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1296 float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1299 global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1304 if (local_x_min > global_x_max || local_x_max < global_x_min ||
1305 local_y_min > global_y_max || local_y_max < global_y_min)
1309 std::vector<std::vector<size_t>> vIdx;
1313 std::vector<float> outX, outY, outZ, tentativeErrSq;
1314 std::vector<size_t> outIdx;
1315 for (
unsigned int localIdx = 0; localIdx < nLocalPoints; ++localIdx)
1318 const float x_local = otherMap->m_x[localIdx];
1319 const float y_local = otherMap->m_y[localIdx];
1320 const float z_local = otherMap->m_z[localIdx];
1328 x_local, y_local, z_local,
1336 const float mX = (outX[0] + outX[1] + outX[2]) / 3.0;
1337 const float mY = (outY[0] + outY[1] + outY[2]) / 3.0;
1338 const float mZ = (outZ[0] + outZ[1] + outZ[2]) / 3.0;
1344 if (distanceForThisPoint < maxDistForCorrespondence)
1347 tempCorrs.resize(tempCorrs.size() + 1);
1351 p.
this_idx = nOtherMapPointsWithCorrespondence++;
1359 p.
other_x = otherMap->m_x[localIdx];
1360 p.
other_y = otherMap->m_y[localIdx];
1361 p.
other_z = otherMap->m_z[localIdx];
1366 std::sort(outIdx.begin(), outIdx.end());
1367 vIdx.push_back(outIdx);
1376 std::map<size_t, std::map<size_t, std::map<size_t, pair<size_t, float>>>>
1378 TMatchingPairList::iterator it;
1379 for (it = tempCorrs.begin(); it != tempCorrs.end(); ++it)
1381 const size_t i0 = vIdx[it->this_idx][0];
1382 const size_t i1 = vIdx[it->this_idx][1];
1383 const size_t i2 = vIdx[it->this_idx][2];
1385 if (best.find(i0) != best.end() &&
1386 best[i0].find(i1) != best[i0].end() &&
1387 best[i0][i1].find(i2) !=
1391 if (best[i0][i1][i2].second > it->errorSquareAfterTransformation)
1393 best[i0][i1][i2].first = it->this_idx;
1394 best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1399 best[i0][i1][i2].first = it->this_idx;
1400 best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1404 for (it = tempCorrs.begin(); it != tempCorrs.end(); ++it)
1406 const size_t i0 = vIdx[it->this_idx][0];
1407 const size_t i1 = vIdx[it->this_idx][1];
1408 const size_t i2 = vIdx[it->this_idx][2];
1410 if (best[i0][i1][i2].first == it->this_idx)
1411 correspondences.push_back(*it);
1415 correspondencesRatio =
1416 nOtherMapPointsWithCorrespondence /
d2f(nLocalPoints);
1423 const float* zs,
const std::size_t num_pts)
1427 float closest_x, closest_y, closest_z;
1430 double sumSqrDist = 0;
1432 std::size_t nPtsForAverage = 0;
1433 for (std::size_t i = 0; i < num_pts;
1439 pc_in_map.
composePoint(xs[i], ys[i], zs[i], xg, yg, zg);
1443 closest_x, closest_y,
1451 sumSqrDist +=
static_cast<double>(closest_err);
1453 if (nPtsForAverage) sumSqrDist /= nPtsForAverage;
1476 const size_t N = scanPoints->
m_x.size();
1477 if (!N || !this->
size())
return -100;
1479 const float* xs = &scanPoints->m_x[0];
1480 const float* ys = &scanPoints->m_y[0];
1481 const float* zs = &scanPoints->m_z[0];
1485 double sumSqrDist = 0;
1486 float closest_x, closest_y;
1488 const float max_sqr_err =
1494 const double ccos = cos(takenFrom2D.
phi);
1495 const double csin = sin(takenFrom2D.
phi);
1496 int nPtsForAverage = 0;
1498 for (
size_t i = 0; i < N;
1503 const float xg = takenFrom2D.
x + ccos * xs[i] - csin * ys[i];
1504 const float yg = takenFrom2D.
y + csin * xs[i] + ccos * ys[i];
1508 closest_x, closest_y,
1515 sumSqrDist +=
static_cast<double>(closest_err);
1517 sumSqrDist /= nPtsForAverage;
1525 takenFrom, xs, ys, zs, N);
1533 if (!o.point_cloud.size())
1536 const size_t N = o.point_cloud.size();
1537 if (!N || !this->
size())
return -100;
1539 const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1541 const float* xs = &o.point_cloud.
x[0];
1542 const float* ys = &o.point_cloud.y[0];
1543 const float* zs = &o.point_cloud.z[0];
1546 sensorAbsPose, xs, ys, zs, N);
1553 if (!N || !this->
size())
return -100;
1555 const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1557 auto xs = o.pointcloud->getPointsBufferRef_x();
1558 auto ys = o.pointcloud->getPointsBufferRef_y();
1559 auto zs = o.pointcloud->getPointsBufferRef_z();
1562 sensorAbsPose, &xs[0], &ys[0], &zs[0], N);
1585 if (out_map)
return;
1587 out_map = std::make_shared<CSimplePointsMap>();
1593 out_map->insertObservation(obs,
nullptr);
1633 pt_has_color =
false;
1644 const size_t nThis = this->
size();
1645 const size_t nOther = anotherMap.
size();
1647 const size_t nTot = nThis + nOther;
1651 for (
size_t i = 0, j = nThis; i < nOther; i++, j++)
1653 m_x[j] = anotherMap.
m_x[i];
1654 m_y[j] = anotherMap.
m_y[i];
1655 m_z[j] = anotherMap.
m_z[i];
1671 const size_t n = mask.size();
1674 for (i = 0, j = 0; i < n; i++)
1696 const size_t N_this =
size();
1697 const size_t N_other = otherMap->
size();
1700 this->
resize(N_this + N_other);
1704 for (src = 0, dst = N_this; src < N_other; src++, dst++)
1728 if (
this == &obj)
return;
1761 robotPose2D =
CPose2D(*robotPose);
1762 robotPose3D = (*robotPose);
1778 bool reallyInsertIt;
1784 reallyInsertIt =
true;
1788 std::vector<bool> checkForDeletion;
1819 const float *xs, *ys, *zs;
1826 for (
size_t i = 0; i < n; i++)
1828 if (checkForDeletion[i])
1836 checkForDeletion[i] =
1872 bool reallyInsertIt;
1878 reallyInsertIt =
true;
1932 this->
size() + o.sensedData.size() * 30);
1934 for (
auto it = o.begin(); it != o.end(); ++it)
1936 const CPose3D sensorPose = robotPose3D +
CPose3D(it->sensorPose);
1937 const double rang = it->sensedDistance;
1939 if (rang <= 0 || rang < o.minSensorDistance ||
1940 rang > o.maxSensorDistance)
1945 const double arc_len = o.sensorConeApperture * rang;
1946 const unsigned int nSteps =
round(1 + arc_len / 0.05);
1947 const double Aa = o.sensorConeApperture / double(nSteps);
1950 for (
double a1 = -aper_2;
a1 < aper_2;
a1 += Aa)
1952 for (
double a2 = -aper_2;
a2 < aper_2;
a2 += Aa)
1954 loc.
x = cos(
a1) * cos(
a2) * rang;
1955 loc.
y = cos(
a1) * sin(
a2) * rang;
1956 loc.
z = sin(
a1) * rang;
1975 if (!o.point_cloud.size())
2035 std::vector<bool>* notFusedPoints)
2039 const CPose2D nullPose(0, 0, 0);
2044 const size_t nOther = otherMap->
size();
2051 params.maxAngularDistForCorrespondence = 0;
2052 params.maxDistForCorrespondence = minDistForFuse;
2057 correspondences,
params, extraResults);
2062 notFusedPoints->clear();
2063 notFusedPoints->reserve(
m_x.size() + nOther);
2064 notFusedPoints->resize(
m_x.size(),
true);
2073 for (
size_t i = 0; i < nOther; i++)
2079 int closestCorr = -1;
2080 float minDist = std::numeric_limits<float>::max();
2081 for (
auto corrsIt = correspondences.begin();
2082 corrsIt != correspondences.end(); ++corrsIt)
2084 if (corrsIt->other_idx == i)
2086 float dist =
square(corrsIt->other_x - corrsIt->this_x) +
2087 square(corrsIt->other_y - corrsIt->this_y) +
2088 square(corrsIt->other_z - corrsIt->this_z);
2092 closestCorr = corrsIt->this_idx;
2097 if (closestCorr != -1)
2104 const float F = 1.0f / (w_a + w_b);
2106 m_x[closestCorr] = F * (w_a * a.
x + w_b * b.
x);
2107 m_y[closestCorr] = F * (w_a * a.
y + w_b * b.
y);
2108 m_z[closestCorr] = F * (w_a * a.
z + w_b * b.
z);
2113 if (notFusedPoints) (*notFusedPoints)[closestCorr] =
false;
2118 if (notFusedPoints) (*notFusedPoints).push_back(
false);
2141 const size_t nOldPtsCount = this->
size();
2143 const size_t nNewPtsCount = nOldPtsCount + nScanPts;
2144 this->
resize(nNewPtsCount);
2146 const float K = 1.0f / 255;
2151 sensorGlobalPose = *robotPose + scan.
sensorPose;
2158 const double m00 = HM(0, 0), m01 = HM(0, 1), m02 = HM(0, 2), m03 = HM(0, 3);
2159 const double m10 = HM(1, 0), m11 = HM(1, 1), m12 = HM(1, 2), m13 = HM(1, 3);
2160 const double m20 = HM(2, 0), m21 = HM(2, 1), m22 = HM(2, 2), m23 = HM(2, 3);
2163 for (
size_t i = 0; i < nScanPts; i++)
2170 const double gx = m00 * lx + m01 * ly + m02 * lz + m03;
2171 const double gy = m10 * lx + m11 * ly + m12 * lz + m13;
2172 const double gz = m20 * lx + m21 * ly + m22 * lz + m23;
2175 nOldPtsCount + i, gx, gy, gz,