9 #ifndef cpointsmap_crtp_common_H 10 #define cpointsmap_crtp_common_H 22 template <
class Derived>
23 struct loadFromRangeImpl
32 obj.mark_as_modified();
42 if (!
obj.insertionOptions.addToExistingPointsMap)
47 const int sizeRangeScan = rangeScan.
scan.
size();
49 if (!sizeRangeScan)
return;
52 if (
obj.x.size() + sizeRangeScan >
obj.x.capacity())
54 obj.reserve((
size_t)(
obj.x.size() * 1.2f) + 3 * sizeRangeScan);
64 float m00 = lric.
HM.get_unsafe(0, 0);
65 float m01 = lric.
HM.get_unsafe(0, 1);
66 float m03 = lric.
HM.get_unsafe(0, 3);
67 float m10 = lric.
HM.get_unsafe(1, 0);
68 float m11 = lric.
HM.get_unsafe(1, 1);
69 float m13 = lric.
HM.get_unsafe(1, 3);
70 float m20 = lric.
HM.get_unsafe(2, 0);
71 float m21 = lric.
HM.get_unsafe(2, 1);
72 float m23 = lric.
HM.get_unsafe(2, 3);
74 float lx_1, ly_1, lz_1, lx = 0, ly = 0,
86 const bool useMinDist =
87 obj.insertionOptions.minDistBetweenLaserPoints > 0;
88 const float minDistSqrBetweenLaserPoints =
89 square(
obj.insertionOptions.minDistBetweenLaserPoints);
94 bool lastPointWasValid =
true;
95 bool thisIsTheFirst =
true;
96 bool lastPointWasInserted =
false;
104 const size_t nPointsAtStart =
obj.size();
105 size_t nextPtIdx = nPointsAtStart;
108 const size_t expectedMaxSize =
111 (
obj.insertionOptions.also_interpolate ? 3 : 1));
112 obj.x.resize(expectedMaxSize);
113 obj.y.resize(expectedMaxSize);
114 obj.z.resize(expectedMaxSize);
123 sincos_vals =
obj.m_scans_sincos_cache.getSinCosForScan(rangeScan);
126 Eigen::Array<float, Eigen::Dynamic, 1> scan_gx(sizeRangeScan + 3),
127 scan_gy(sizeRangeScan + 3),
134 size_t nPackets = sizeRangeScan / 4;
135 if ((sizeRangeScan & 0x03) != 0) nPackets++;
145 const __m128 m00_4val =
147 const __m128 m01_4val = _mm_set1_ps(m01);
148 const __m128 m03_4val = _mm_set1_ps(m03);
150 const __m128 m10_4val = _mm_set1_ps(m10);
151 const __m128 m11_4val = _mm_set1_ps(m11);
152 const __m128 m13_4val = _mm_set1_ps(m13);
154 const __m128 m20_4val = _mm_set1_ps(m20);
155 const __m128 m21_4val = _mm_set1_ps(m21);
156 const __m128 m23_4val = _mm_set1_ps(m23);
165 const float* ptr_in_scan = &rangeScan.
scan[0];
166 const float* ptr_in_cos = &sincos_vals.
ccos[0];
167 const float* ptr_in_sin = &sincos_vals.
csin[0];
169 float* ptr_out_x = &scan_gx[0];
170 float* ptr_out_y = &scan_gy[0];
171 float* ptr_out_z = &scan_gz[0];
173 for (; nPackets; nPackets--, ptr_in_scan += 4, ptr_in_cos += 4,
174 ptr_in_sin += 4, ptr_out_x += 4, ptr_out_y += 4,
177 const __m128 scan_4vals =
178 _mm_loadu_ps(ptr_in_scan);
181 _mm_mul_ps(scan_4vals, _mm_load_ps(ptr_in_cos));
183 _mm_mul_ps(scan_4vals, _mm_load_ps(ptr_in_sin));
186 ptr_out_x, _mm_add_ps(
187 m03_4val, _mm_add_ps(
188 _mm_mul_ps(xs, m00_4val),
189 _mm_mul_ps(ys, m01_4val))));
191 ptr_out_y, _mm_add_ps(
192 m13_4val, _mm_add_ps(
193 _mm_mul_ps(xs, m10_4val),
194 _mm_mul_ps(ys, m11_4val))));
196 ptr_out_z, _mm_add_ps(
197 m23_4val, _mm_add_ps(
198 _mm_mul_ps(xs, m20_4val),
199 _mm_mul_ps(ys, m21_4val))));
201 #else // MRPT_HAS_SSE2 204 Eigen::Array<float, Eigen::Dynamic, 1> scan_x(sizeRangeScan + 3),
205 scan_y(sizeRangeScan + 3);
208 const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> scan_vals(
209 const_cast<float*>(&rangeScan.
scan[0]), rangeScan.
scan.
size(),
213 const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> ccos(
214 const_cast<float*>(&sincos_vals.
ccos[0]), rangeScan.
scan.
size(),
216 const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> csin(
217 const_cast<float*>(&sincos_vals.
csin[0]), rangeScan.
scan.
size(),
221 scan_x = scan_vals.array() * ccos.array();
222 scan_y = scan_vals.array() * csin.array();
226 scan_gx = m00 * scan_x + m01 * scan_y + m03;
227 scan_gy = m10 * scan_x + m11 * scan_y + m13;
228 scan_gz = m20 * scan_x + m21 * scan_y + m23;
229 #endif // MRPT_HAS_SSE2 232 for (
int i = 0; i < sizeRangeScan; i++)
243 obj, lx, ly, lz, lric);
245 lastPointWasInserted =
false;
248 bool pt_pass_min_dist =
true;
250 if (useMinDist ||
obj.insertionOptions.also_interpolate)
252 if (!lastPointWasValid)
253 pt_pass_min_dist =
false;
259 pt_pass_min_dist = (d2 > minDistSqrBetweenLaserPoints);
263 if (thisIsTheFirst || pt_pass_min_dist)
265 thisIsTheFirst =
false;
269 if (
obj.insertionOptions.also_interpolate && i > 1)
271 float changeInDirection;
272 const float d = std::sqrt(d2);
274 if ((lx != lx_1 || ly != ly_1) &&
275 (lx_1 != lx_2 || ly_1 != ly_2))
276 changeInDirection = atan2(ly - ly_1, lx - lx_1) -
277 atan2(ly_1 - ly_2, lx_1 - lx_2);
279 changeInDirection = 0;
284 .minDistBetweenLaserPoints &&
285 d <
obj.insertionOptions
286 .maxDistForInterpolatePoints &&
287 fabs(changeInDirection) <
DEG2RAD(5))
290 d / (2 * sqrt(minDistSqrBetweenLaserPoints)));
292 for (
int q = 1;
q < nInterpol;
q++)
294 float i_x = lx_1 +
q * (lx - lx_1) / nInterpol;
295 float i_y = ly_1 +
q * (ly - ly_1) / nInterpol;
296 float i_z = lz_1 +
q * (lz - lz_1) / nInterpol;
297 if (!
obj.m_heightfilter_enabled ||
298 (i_z >=
obj.m_heightfilter_z_min &&
299 i_z <=
obj.m_heightfilter_z_max))
301 obj.x.push_back(i_x);
302 obj.y.push_back(i_y);
303 obj.z.push_back(i_z);
314 if (!
obj.m_heightfilter_enabled ||
315 (lz >=
obj.m_heightfilter_z_min &&
316 lz <=
obj.m_heightfilter_z_max))
318 obj.x[nextPtIdx] = lx;
319 obj.y[nextPtIdx] = ly;
320 obj.z[nextPtIdx] = lz;
329 lastPointWasInserted =
true;
344 lastPointWasValid = rangeScan.
validRange[i] != 0;
348 if (lastPointWasValid && !lastPointWasInserted)
350 if (!
obj.m_heightfilter_enabled ||
351 (lz >=
obj.m_heightfilter_z_min &&
352 lz <=
obj.m_heightfilter_z_max))
354 obj.x[nextPtIdx] = lx;
355 obj.y[nextPtIdx] = ly;
356 obj.z[nextPtIdx] = lz;
366 obj.x.resize(nextPtIdx);
367 obj.y.resize(nextPtIdx);
368 obj.z.resize(nextPtIdx);
377 obj.mark_as_modified();
387 if (!
obj.insertionOptions.addToExistingPointsMap)
394 const size_t sizeRangeScan = rangeScan.
points3D_x.size();
397 if (
obj.x.size() + sizeRangeScan >
obj.x.capacity())
398 obj.reserve(
size_t(
obj.x.size() + 1.1 * sizeRangeScan));
405 float m00 = lric.
HM.get_unsafe(0, 0);
406 float m01 = lric.
HM.get_unsafe(0, 1);
407 float m02 = lric.
HM.get_unsafe(0, 2);
408 float m03 = lric.
HM.get_unsafe(0, 3);
409 float m10 = lric.
HM.get_unsafe(1, 0);
410 float m11 = lric.
HM.get_unsafe(1, 1);
411 float m12 = lric.
HM.get_unsafe(1, 2);
412 float m13 = lric.
HM.get_unsafe(1, 3);
413 float m20 = lric.
HM.get_unsafe(2, 0);
414 float m21 = lric.
HM.get_unsafe(2, 1);
415 float m22 = lric.
HM.get_unsafe(2, 2);
416 float m23 = lric.
HM.get_unsafe(2, 3);
418 float lx_1, ly_1, lz_1, lx = 0, ly = 0,
426 float minDistSqrBetweenLaserPoints =
427 square(
obj.insertionOptions.minDistBetweenLaserPoints);
430 if (
obj.insertionOptions.minDistBetweenLaserPoints <= 0)
431 minDistSqrBetweenLaserPoints = -1;
436 bool lastPointWasValid =
true;
437 bool thisIsTheFirst =
true;
438 bool lastPointWasInserted =
false;
443 for (
size_t i = 0; i < sizeRangeScan; i++)
448 obj.insertionOptions.insertInvalidPoints)
464 obj, lx, ly, lz, lric);
466 lastPointWasInserted =
false;
471 if (thisIsTheFirst ||
472 (lastPointWasValid && (d2 > minDistSqrBetweenLaserPoints)))
474 thisIsTheFirst =
false;
484 lastPointWasInserted =
true;
491 lastPointWasValid =
true;
495 lastPointWasValid =
false;
503 if (lastPointWasValid && !lastPointWasInserted)
505 if (lx != 0 || ly != 0 || lz != 0)
double DEG2RAD(const double x)
Degrees to radians.
GLdouble GLdouble GLdouble GLdouble q
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
GLsizei GLsizei GLuint * obj
T square(const T x)
Inline function for the square of a number.
std::vector< float > points3D_z
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation3DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
mrpt::math::CVectorFloat ccos
std::vector< float > points3D_y
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
float scan_x
In internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points b...
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
bool hasPoints3D
true means the field points3D contains valid data.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::CVectorFloat csin
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A pair of vectors with the cos and sin values.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
T square(const T x)
Inline function for the square of a number.
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
int round(const T value)
Returns the closer integer (int) to x.
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters.
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()