31 void CObservation2DRangeScan::serializeTo(
35 out << aperture << rightToLeft << maxRange << sensorPose;
36 uint32_t N = m_scan.size();
41 out.WriteBufferFixEndianness(&m_scan[0], N);
42 out.WriteBuffer(&m_validRange[0],
sizeof(m_validRange[0]) * N);
52 out << hasIntensity();
53 if (hasIntensity())
out.WriteBufferFixEndianness(&m_intensity[0], N);
56 void CObservation2DRangeScan::truncateByDistanceAndAngle(
57 float min_distance,
float max_angle,
float min_height,
float max_height,
63 const auto nPts = m_scan.
size();
65 auto itValid = m_validRange.begin();
66 for (
auto itScan = m_scan.begin(); itScan != m_scan.end();
67 itScan++, itValid++, k++)
69 const auto ang = std::abs(k * aperture / nPts - aperture * 0.5f);
70 float x = (*itScan) * cos(ang);
72 if (min_height != 0 || max_height != 0)
74 ASSERT_(max_height > min_height);
75 if (*itScan < min_distance || ang > max_angle ||
76 x > h - min_height || x < h - max_height)
79 else if (*itScan < min_distance || ang > max_angle)
84 void CObservation2DRangeScan::serializeFrom(
95 in >> aperture >> rightToLeft >> maxRange >> sensorPose >>
108 &m_validRange[0],
sizeof(m_validRange[0]) * N);
113 for (i = 0; i < N; i++) m_validRange[i] = m_scan[i] < maxRange;
146 in >> aperture >> rightToLeft >> maxRange >> sensorPose;
156 in.
ReadBuffer(&m_validRange[0],
sizeof(m_validRange[0]) * N);
176 setScanHasIntensity(hasIntensity);
177 if (hasIntensity && N)
199 mxArray* CObservation2DRangeScan::writeToMatlab()
const
202 const char* fields[] = {
"class",
216 mexplus::MxArray obs_struct(
217 mexplus::MxArray::Struct(
sizeof(fields) /
sizeof(fields[0]), fields));
219 obs_struct.set(
"class", this->GetRuntimeClass()->className);
222 obs_struct.set(
"sensorLabel", this->sensorLabel);
224 obs_struct.set(
"scan", this->m_scan);
228 obs_struct.set(
"validRange", this->m_validRange);
229 obs_struct.set(
"intensity", this->m_intensity);
230 obs_struct.set(
"aperture", this->aperture);
231 obs_struct.set(
"rightToLeft", this->rightToLeft);
232 obs_struct.set(
"maxRange", this->maxRange);
233 obs_struct.set(
"stdError", this->stdError);
234 obs_struct.set(
"beamAperture", this->beamAperture);
235 obs_struct.set(
"deltaPitch", this->deltaPitch);
236 obs_struct.set(
"pose", this->sensorPose);
238 return obs_struct.release();
247 bool CObservation2DRangeScan::isPlanarScan(
const double tolerance)
const
249 return sensorPose.isHorizontal(tolerance);
252 bool CObservation2DRangeScan::hasIntensity()
const {
return m_has_intensity; }
253 void CObservation2DRangeScan::setScanHasIntensity(
bool setHasIntensityFlag)
255 m_has_intensity = setHasIntensityFlag;
261 void CObservation2DRangeScan::filterByExclusionAreas(
264 if (areas.empty())
return;
269 const size_t sizeRangeScan = m_scan.size();
273 if (!sizeRangeScan)
return;
277 Ang = -0.5f * aperture;
278 dA = aperture / (sizeRangeScan - 1);
282 Ang = +0.5f * aperture;
283 dA = -aperture / (sizeRangeScan - 1);
286 auto valid_it = m_validRange.begin();
287 for (
auto scan_it = m_scan.begin(); scan_it != m_scan.end();
288 scan_it++, valid_it++)
297 const auto Lx = *scan_it * cos(Ang);
298 const auto Ly = *scan_it * sin(Ang);
303 this->sensorPose.composePoint(Lx, Ly, 0, Gx, Gy, Gz);
306 for (
const auto& area : areas)
308 if (area.first.PointIntoPolygon(Gx, Gy) &&
309 (Gz >= area.second.first && Gz <= area.second.second))
323 void CObservation2DRangeScan::filterByExclusionAreas(
324 const std::vector<mrpt::math::CPolygon>& areas)
326 if (areas.empty())
return;
329 for (
const auto& area : areas)
331 TListExclusionAreasWithRanges::value_type dat;
333 dat.second.first = -std::numeric_limits<double>::max();
334 dat.second.second = std::numeric_limits<double>::max();
338 filterByExclusionAreas(lst);
344 void CObservation2DRangeScan::filterByExclusionAngles(
345 const std::vector<std::pair<double, double>>& angles)
347 if (angles.empty())
return;
352 const size_t sizeRangeScan = m_scan.size();
356 if (!sizeRangeScan)
return;
360 Ang = -0.5 * aperture;
361 dA = aperture / (sizeRangeScan - 1);
365 Ang = +0.5 * aperture;
366 dA = -aperture / (sizeRangeScan - 1);
370 for (
const auto& angle : angles)
377 if (ap_idx_ini < 0) ap_idx_ini = 0;
378 if (ap_idx_end < 0) ap_idx_end = 0;
380 if (ap_idx_ini >
static_cast<int>(sizeRangeScan))
381 ap_idx_ini = sizeRangeScan - 1;
382 if (ap_idx_end >
static_cast<int>(sizeRangeScan))
383 ap_idx_end = sizeRangeScan - 1;
385 const size_t idx_ini = ap_idx_ini;
386 const size_t idx_end = ap_idx_end;
388 if (idx_end >= idx_ini)
390 for (
size_t i = idx_ini; i <= idx_end; i++) m_validRange[i] =
false;
394 for (
size_t i = 0; i < idx_end; i++) m_validRange[i] =
false;
396 for (
size_t i = idx_ini; i < sizeRangeScan; i++)
397 m_validRange[i] =
false;
425 void CObservation2DRangeScan::internal_buildAuxPointsMap(
426 const void* options)
const
429 throw std::runtime_error(
430 "[CObservation2DRangeScan::buildAuxPointsMap] ERROR: This function "
431 "needs linking against mrpt-maps.\n");
433 (*ptr_internal_build_points_map_from_scan2D)(*
this, m_cachedMap, options);
439 p.
nRays = m_scan.size();
453 void CObservation2DRangeScan::getDescriptionAsText(std::ostream& o)
const
455 CObservation::getDescriptionAsText(o);
456 o <<
"Homogeneous matrix for the sensor's 3D pose, relative to robot "
458 o << sensorPose.getHomogeneousMatrixVal<
CMatrixDouble44>() << sensorPose
462 "Samples direction: %s\n",
463 (rightToLeft) ?
"Right->Left" :
"Left->Right");
464 o <<
format(
"Points in the scan: %u\n", m_scan.size());
465 o <<
format(
"Estimated sensor 'sigma': %f\n", stdError);
467 "Increment in pitch during the scan: %f deg\n",
RAD2DEG(deltaPitch));
470 for (i = 0; i < m_scan.size(); i++)
471 if (!m_validRange[i]) inval++;
472 o <<
format(
"Invalid points in the scan: %u\n", (
unsigned)inval);
474 o <<
format(
"Sensor maximum range: %.02f m\n", maxRange);
476 "Sensor field-of-view (\"aperture\"): %.01f deg\n",
RAD2DEG(aperture));
478 o <<
"Raw scan values: [";
479 for (i = 0; i < m_scan.size(); i++) o <<
format(
"%.03f ", m_scan[i]);
482 o <<
"Raw valid-scan values: [";
483 for (i = 0; i < m_validRange.size(); i++)
484 o <<
format(
"%u ", m_validRange[i] ? 1 : 0);
489 o <<
"Raw intensity values: [";
490 for (i = 0; i < m_intensity.size(); i++)
491 o <<
format(
"%d ", m_intensity[i]);
496 const float& CObservation2DRangeScan::getScanRange(
const size_t i)
const
501 float& CObservation2DRangeScan::getScanRange(
const size_t i)
507 void CObservation2DRangeScan::setScanRange(
const size_t i,
const float val)
513 const int32_t& CObservation2DRangeScan::getScanIntensity(
const size_t i)
const
516 return m_intensity[i];
518 int32_t& CObservation2DRangeScan::getScanIntensity(
const size_t i)
521 return m_intensity[i];
523 void CObservation2DRangeScan::setScanIntensity(
const size_t i,
const int val)
526 m_intensity[i] =
val;
529 bool CObservation2DRangeScan::getScanRangeValidity(
const size_t i)
const
532 return m_validRange[i] != 0;
534 void CObservation2DRangeScan::setScanRangeValidity(
535 const size_t i,
const bool val)
538 m_validRange[i] =
val ? 1 : 0;
541 void CObservation2DRangeScan::resizeScan(
const size_t len)
544 m_intensity.resize(len);
545 m_validRange.resize(len);
548 void CObservation2DRangeScan::resizeScanAndAssign(
549 const size_t len,
const float rangeVal,
const bool rangeValidity,
550 const int32_t rangeIntensity)
552 m_scan.assign(len, rangeVal);
553 m_validRange.assign(len, rangeValidity);
554 m_intensity.assign(len, rangeIntensity);
557 size_t CObservation2DRangeScan::getScanSize()
const {
return m_scan.size(); }
558 void CObservation2DRangeScan::loadFromVectors(
559 size_t nRays,
const float* scanRanges,
const char* scanValidity)
564 for (
size_t i = 0; i < nRays; i++)
566 m_scan[i] = scanRanges[i];
567 m_validRange[i] = scanValidity[i];