MRPT  2.0.4
CObservation3DRangeScan_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
11 #include <mrpt/config.h>
15 #include <mrpt/math/CHistogram.h>
17 #include <mrpt/obs/CSensoryFrame.h>
19 #include <mrpt/system/filesystem.h>
20 #include <test_mrpt_common.h>
21 
22 using namespace mrpt;
23 using namespace std;
24 
25 // We need OPENCV to read the image internal to CObservation3DRangeScan,
26 // and to build the unprojected points LUTs, so skip tests if we don't have
27 // opencv.
28 #if MRPT_HAS_OPENCV
29 
30 constexpr unsigned int TEST_RANGEIMG_WIDTH = 32;
31 constexpr unsigned int TEST_RANGEIMG_HEIGHT = 24;
32 
33 constexpr float SECOND_LAYER_CONSTANT_RANGE = 50.0f;
34 
37  mrpt::obs::T3DPointsProjectionParams& pp, int test_case)
38 {
39  obs.hasRangeImage = true;
40 
41  // Create a second depth layer:
42  obs.rangeImageOtherLayers.clear();
43  mrpt::math::CMatrix_u16& ri_2nd = obs.rangeImageOtherLayers["LATEST"];
44 
46 
47  obs.rangeImage.setZero();
48 
49  obs.rangeUnits = 1e-3f;
50 
51  for (unsigned int r = 10; r < 16; r++)
52  for (unsigned int c = 10; c <= r; c++)
53  obs.rangeImage(r, c) = static_cast<uint16_t>(r / obs.rangeUnits);
54 
55  ri_2nd.fill(
56  static_cast<uint16_t>(SECOND_LAYER_CONSTANT_RANGE / obs.rangeUnits));
57 
64 
65  // Test case:
66  pp.USE_SSE2 = (test_case & 1) != 0;
67  pp.takeIntoAccountSensorPoseOnRobot = (test_case & 2) != 0;
68 }
69 
70 TEST(CObservation3DRangeScan, Project3D_noFilter)
71 {
74 
75  for (int i = 0; i < 4; i++) // test all combinations of flags
76  {
78  fillSampleObs(o, pp, i);
79 
80  o.unprojectInto(o, pp, fp);
81  EXPECT_EQ(o.points3D_x.size(), 21U)
82  << " testcase flags: i=" << i << std::endl;
83  }
84 }
85 
86 TEST(CObservation3DRangeScan, Project3D_filterMinMax1)
87 {
90  fMin(12, 12) = 11.5f;
91  fMax(12, 12) = 12.5f; // pass
92  fMin(14, 14) = 15.5f;
93  fMax(14, 14) = 16.5f; // don't pass
94  // Rest of points: filter=0.0f -> no filtering
95 
98  fp.rangeMask_min = &fMin;
99  fp.rangeMask_max = &fMax;
100 
101  for (int i = 0; i < 8; i++) // test all combinations of flags
102  {
104  fillSampleObs(o, pp, i);
105  fp.rangeCheckBetween = (i & 4) != 0;
106 
107  o.unprojectInto(o, pp, fp);
108  EXPECT_EQ(o.points3D_x.size(), 20U)
109  << " testcase flags: i=" << i << std::endl;
110  }
111 }
112 
113 TEST(CObservation3DRangeScan, Project3D_additionalLayers)
114 {
117 
118  pp.layer = "LATEST";
119 
120  for (int i = 0; i < 4; i++) // test all combinations of flags
121  {
123  fillSampleObs(o, pp, i);
124 
125  o.unprojectInto(o, pp, fp);
126  EXPECT_EQ(
128  << " testcase flags: i=" << i << std::endl;
129  }
130 }
131 
132 TEST(CObservation3DRangeScan, Project3D_filterMinMaxAllBetween)
133 {
136  // Default filter=0.0f -> no filtering
137  for (int r = 10; r < 16; r++)
138  for (int c = 10; c < 16; c++)
139  {
140  fMin(r, c) = r - 0.1f; // All points actually lie in between
141  fMax(r, c) = r + 0.1f;
142  }
143 
146  fp.rangeMask_min = &fMin;
147  fp.rangeMask_max = &fMax;
148 
149  for (int i = 0; i < 16; i++) // test all combinations of flags
150  {
152  fillSampleObs(o, pp, i);
153  fp.rangeCheckBetween = (i & 8) != 0;
154 
155  o.unprojectInto(o, pp, fp);
156  EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 21U : 0U)
157  << " testcase flags: i=" << i << std::endl;
158  }
159 }
160 
161 TEST(CObservation3DRangeScan, Project3D_filterMinMaxNoneBetween)
162 {
165  // Default filter=0.0f -> no filtering
166  for (int r = 10; r < 16; r++)
167  for (int c = 10; c < 16; c++)
168  {
169  fMin(r, c) = r + 1.1f; // No point lies in between
170  fMax(r, c) = r + 1.2f;
171  }
172 
175  fp.rangeMask_min = &fMin;
176  fp.rangeMask_max = &fMax;
177 
178  for (int i = 0; i < 16; i++) // test all combinations of flags
179  {
181  fillSampleObs(o, pp, i);
182  fp.rangeCheckBetween = (i & 8) != 0;
183 
184  o.unprojectInto(o, pp, fp);
185  EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 0U : 21U)
186  << " testcase flags: i=" << i << std::endl;
187  }
188 }
189 
190 TEST(CObservation3DRangeScan, Project3D_filterMin)
191 {
193  // Default filter=0.0f -> no filtering
194  for (int r = 10; r < 16; r++)
195  for (int c = 10; c < 16; c++)
196  fMin(r, c) =
197  14.5f; // Only last row of points should pass this filter
198 
201  fp.rangeMask_min = &fMin;
202 
203  for (int i = 0; i < 8; i++) // test all combinations of flags
204  {
206  fillSampleObs(o, pp, i);
207 
208  o.unprojectInto(o, pp, fp);
209  EXPECT_EQ(o.points3D_x.size(), 6U)
210  << " testcase flags: i=" << i << std::endl;
211  }
212 }
213 
214 TEST(CObservation3DRangeScan, Project3D_filterMax)
215 {
217  // Default filter=0.0f -> no filtering
218  for (int r = 10; r < 16; r++)
219  for (int c = 10; c < 16; c++)
220  fMax(r, c) =
221  11.5f; // Only first 2 rows of points should pass this filter
222 
225  fp.rangeMask_max = &fMax;
226 
227  for (int i = 0; i < 8; i++) // test all combinations of flags
228  {
230  fillSampleObs(o, pp, i);
231 
232  o.unprojectInto(o, pp, fp);
233  EXPECT_EQ(o.points3D_x.size(), 3U)
234  << " testcase flags: i=" << i << std::endl;
235  }
236 }
237 
238 TEST(CObservation3DRangeScan, LoadAndCheckFloorPoints)
239 {
240  const string rawlog_fil =
241  UNITTEST_BASEDIR + string("/tests/test-3d-obs-ground.rawlog");
242  if (!mrpt::system::fileExists(rawlog_fil))
243  {
244  GTEST_FAIL() << "ERROR: test due to missing file: " << rawlog_fil
245  << "\n";
246  return;
247  }
248 
249  // Load sample 3D scan from file:
251  mrpt::io::CFileGZInputStream f(rawlog_fil);
253 
255 
256  // Depth -> 3D points:
259  mrpt::math::CHistogram hist(-0.15, 0.15, 30);
260  std::vector<double> ptsz;
261  std::vector<double> bin_x, bin_count;
262 
263  // decimation=1
264  {
266  obs->unprojectInto(pts, pp);
267 
269  pts.getPointsBufferRef_z(), ptsz);
270  hist.add(ptsz);
271  hist.getHistogram(bin_x, bin_count);
272 
273  /*
274  //for (unsigned int i=0;i<bin_x.size();i++)
275  //std::cout << "Hist[" << i << "] x=" << bin_x[i] << " count= " <<
276  bin_count[i] << std::endl;
277  */
278 
279  EXPECT_LE(bin_count[11], 100);
280  EXPECT_LE(bin_count[12], 1000);
281  EXPECT_GE(bin_count[14], 12000);
282  EXPECT_LE(bin_count[18], 700);
283  EXPECT_LE(bin_count[19], 20);
284  }
285 
286  // decimation=8
287  {
289  pp.decimation = 8;
290  obs->unprojectInto(pts, pp);
291 
293  pts.getPointsBufferRef_z(), ptsz);
294  hist.clear();
295  hist.add(ptsz);
296  hist.getHistogram(bin_x, bin_count);
297 
298  // for (unsigned int i = 0; i < bin_x.size(); i++)
299  // std::cout << "Hist[" << i << "] x=" << bin_x[i]
300  // << " count= " << bin_count[i] << std::endl;
301  /*
302  Hist[11] x=-0.0362069 count= 0
303  Hist[12] x=-0.0258621 count= 7
304  Hist[13] x=-0.0155172 count= 24
305  Hist[14] x=-0.00517241 count= 130
306  Hist[15] x=0.00517241 count= 234
307  Hist[16] x=0.0155172 count= 129
308  Hist[17] x=0.0258621 count= 85
309  Hist[18] x=0.0362069 count= 32
310  Hist[19] x=0.0465517 count= 3
311  */
312  EXPECT_LE(bin_count[11], 2);
313  EXPECT_LE(bin_count[12], 10);
314  EXPECT_GE(bin_count[14], 100);
315  EXPECT_LE(bin_count[18], 40);
316  EXPECT_LE(bin_count[19], 5);
317  }
318 }
319 
320 TEST(CObservation3DRangeScan, SyntheticRange)
321 {
322  for (int i = 0; i < 4; i++) // test all combinations of flags
323  {
326  fillSampleObs(o, pp, i);
327  const float R = 15.0f;
328  o.rangeImage.fill(static_cast<uint16_t>(R / o.rangeUnits));
329 
330  // Ranges:
331  o.range_is_depth = false;
332 
333  // x y z yaw pitch roll
334  o.sensorPose = mrpt::poses::CPose3D::FromString("[1 2 3 0 0 0]");
335 
337  o.unprojectInto(pts, pp);
338 
340 
341  for (size_t j = 0; j < pts.size(); j++)
342  {
343  float px, py, pz;
344  pts.getPoint(j, px, py, pz);
345 
348  o.sensorPose.inverseComposePoint(px, py, pz, l.x, l.y, l.z);
349  else
350  l = mrpt::math::TPoint3D(px, py, pz);
351 
352  EXPECT_NEAR(l.norm(), R, 2e-3);
353  }
354  }
355 }
356 
357 TEST(CObservation3DRangeScan, SyntheticDepth)
358 {
359  for (int i = 0; i < 4; i++) // test all combinations of flags
360  {
363  fillSampleObs(o, pp, i);
364  const float R = 15.0f;
365  o.rangeImage.fill(static_cast<uint16_t>(R / o.rangeUnits));
366 
367  // depth:
368  o.range_is_depth = true;
369 
370  // x y z yaw pitch roll
371  o.sensorPose = mrpt::poses::CPose3D::FromString("[1 2 3 0 0 0]");
372 
374  o.unprojectInto(pts, pp);
375 
377 
378  for (size_t j = 0; j < pts.size(); j++)
379  {
380  float px, py, pz;
381  pts.getPoint(j, px, py, pz);
382 
385  o.sensorPose.inverseComposePoint(px, py, pz, l.x, l.y, l.z);
386  else
387  l = mrpt::math::TPoint3D(px, py, pz);
388 
389  EXPECT_NEAR(l.x, R, 2e-3);
390  }
391  }
392 }
393 #endif
void fillSampleObs(mrpt::obs::CObservation3DRangeScan &obs, mrpt::obs::T3DPointsProjectionParams &pp, int test_case)
uint32_t nrows
Definition: TCamera.h:40
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:198
This class provides an easy way of computing histograms for unidimensional real valued variables...
Definition: CHistogram.h:33
std::map< std::string, mrpt::math::CMatrix_u16 > rangeImageOtherLayers
Additional layer range/depth images.
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
void unprojectInto(POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams=T3DPointsProjectionParams(), const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Unprojects the RGB+D image pair into a 3D point cloud (with color if the target map supports it) and ...
static CPose3D FromString(const std::string &s)
Definition: CPose3D.h:644
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:175
void copy_container_typecasting(const src_container &src, dst_container &trg)
Copy all the elements in a container (vector, deque, list) into a different one performing the approp...
void fill(const Scalar &val)
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, 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) const
Computes the 3D point L such as .
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:177
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
T::Ptr getObservationByClass(size_t ith=0) const
Returns the i&#39;th observation of a given class (or of a descendant class), or nullptr if there is no s...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
Used in CObservation3DRangeScan::unprojectInto()
TEST(CObservation3DRangeScan, Project3D_noFilter)
const mrpt::math::CMatrixF * rangeMask_max
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:173
const mrpt::aligned_std_vector< float > & getPointsBufferRef_z() const
Provides a direct access to a read-only reference of the internal point buffer.
Definition: CPointsMap.h:576
void add(const double x)
Add an element to the histogram.
Definition: CHistogram.cpp:42
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
bool hasRangeImage
true means the field rangeImage contains valid data
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)...
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
constexpr float SECOND_LAYER_CONSTANT_RANGE
const mrpt::math::CMatrixF * rangeMask_min
(Default: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
void getHistogram(std::vector< double > &x, std::vector< double > &hits) const
Returns the list of bin centers & hit counts.
Definition: CHistogram.cpp:77
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:171
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
constexpr unsigned int TEST_RANGEIMG_WIDTH
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
const float R
Used in CObservation3DRangeScan::unprojectInto()
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
void rangeImage_setSize(const int HEIGHT, const int WIDTH)
Similar to calling "rangeImage.setSize(H,W)" but this method provides memory pooling to speed-up the ...
void clear()
Clear the histogram:
Definition: CHistogram.cpp:33
Transparently opens a compressed "gz" file and reads uncompressed data from it.
constexpr unsigned int TEST_RANGEIMG_HEIGHT
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:459
std::string layer
If empty, the main rangeImage layer will be unprojected.
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
bool rangeCheckBetween
Only used if both rangeMask_min and rangeMask_max are present.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020