MRPT  1.9.9
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-2019, 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 #define TEST_RANGEIMG_WIDTH 32
26 #define TEST_RANGEIMG_HEIGHT 24
27 
30  mrpt::obs::T3DPointsProjectionParams& pp, int test_case)
31 {
32  obs.hasRangeImage = true;
34 
35  obs.rangeImage.setZero();
36 
37  obs.rangeUnits = 1e-3f;
38 
39  for (int r = 10; r < 16; r++)
40  for (int c = 10; c <= r; c++) obs.rangeImage(r, c) = r / obs.rangeUnits;
41 
42  // Test case:
43  pp.PROJ3D_USE_LUT = (test_case & 1) != 0;
44  pp.USE_SSE2 = (test_case & 2) != 0;
45  pp.takeIntoAccountSensorPoseOnRobot = (test_case & 4) != 0;
46 }
47 
48 TEST(CObservation3DRangeScan, Project3D_noFilter)
49 {
52 
53  for (int i = 0; i < 8; i++) // test all combinations of flags
54  {
56  fillSampleObs(o, pp, i);
57 
59  EXPECT_EQ(o.points3D_x.size(), 21U)
60  << " testcase flags: i=" << i << std::endl;
61  }
62 }
63 
64 TEST(CObservation3DRangeScan, Project3D_filterMinMax1)
65 {
68  fMin(12, 12) = 11.5f;
69  fMax(12, 12) = 12.5f; // pass
70  fMin(14, 14) = 15.5f;
71  fMax(14, 14) = 16.5f; // don't pass
72  // Rest of points: filter=0.0f -> no filtering
73 
76  fp.rangeMask_min = &fMin;
77  fp.rangeMask_max = &fMax;
78 
79  for (int i = 0; i < 16; i++) // test all combinations of flags
80  {
82  fillSampleObs(o, pp, i);
83  fp.rangeCheckBetween = (i & 8) != 0;
84 
86  EXPECT_EQ(o.points3D_x.size(), 20U)
87  << " testcase flags: i=" << i << std::endl;
88  }
89 }
90 
91 TEST(CObservation3DRangeScan, Project3D_filterMinMaxAllBetween)
92 {
95  // Default filter=0.0f -> no filtering
96  for (int r = 10; r < 16; r++)
97  for (int c = 10; c < 16; c++)
98  {
99  fMin(r, c) = r - 0.1f; // All points actually lie in between
100  fMax(r, c) = r + 0.1f;
101  }
102 
105  fp.rangeMask_min = &fMin;
106  fp.rangeMask_max = &fMax;
107 
108  for (int i = 0; i < 16; i++) // test all combinations of flags
109  {
111  fillSampleObs(o, pp, i);
112  fp.rangeCheckBetween = (i & 8) != 0;
113 
115  EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 21U : 0U)
116  << " testcase flags: i=" << i << std::endl;
117  }
118 }
119 
120 TEST(CObservation3DRangeScan, Project3D_filterMinMaxNoneBetween)
121 {
124  // Default filter=0.0f -> no filtering
125  for (int r = 10; r < 16; r++)
126  for (int c = 10; c < 16; c++)
127  {
128  fMin(r, c) = r + 1.1f; // No point lies in between
129  fMax(r, c) = r + 1.2f;
130  }
131 
134  fp.rangeMask_min = &fMin;
135  fp.rangeMask_max = &fMax;
136 
137  for (int i = 0; i < 16; i++) // test all combinations of flags
138  {
140  fillSampleObs(o, pp, i);
141  fp.rangeCheckBetween = (i & 8) != 0;
142 
144  EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 0U : 21U)
145  << " testcase flags: i=" << i << std::endl;
146  }
147 }
148 
149 TEST(CObservation3DRangeScan, Project3D_filterMin)
150 {
152  // Default filter=0.0f -> no filtering
153  for (int r = 10; r < 16; r++)
154  for (int c = 10; c < 16; c++)
155  fMin(r, c) =
156  14.5f; // Only last row of points should pass this filter
157 
160  fp.rangeMask_min = &fMin;
161 
162  for (int i = 0; i < 8; i++) // test all combinations of flags
163  {
165  fillSampleObs(o, pp, i);
166 
168  EXPECT_EQ(o.points3D_x.size(), 6U)
169  << " testcase flags: i=" << i << std::endl;
170  }
171 }
172 
173 TEST(CObservation3DRangeScan, Project3D_filterMax)
174 {
176  // Default filter=0.0f -> no filtering
177  for (int r = 10; r < 16; r++)
178  for (int c = 10; c < 16; c++)
179  fMax(r, c) =
180  11.5f; // Only first 2 rows of points should pass this filter
181 
184  fp.rangeMask_max = &fMax;
185 
186  for (int i = 0; i < 8; i++) // test all combinations of flags
187  {
189  fillSampleObs(o, pp, i);
190 
192  EXPECT_EQ(o.points3D_x.size(), 3U)
193  << " testcase flags: i=" << i << std::endl;
194  }
195 }
196 
197 // We need OPENCV to read the image internal to CObservation3DRangeScan,
198 // so skip this test if built without opencv.
199 #if MRPT_HAS_OPENCV
200 
201 TEST(CObservation3DRangeScan, LoadAndCheckFloorPoints)
202 {
203  const string rawlog_fil =
204  UNITTEST_BASEDIR + string("/tests/test-3d-obs-ground.rawlog");
205  if (!mrpt::system::fileExists(rawlog_fil))
206  {
207  GTEST_FAIL() << "ERROR: test due to missing file: " << rawlog_fil
208  << "\n";
209  return;
210  }
211 
212  // Load sample 3D scan from file:
214  mrpt::io::CFileGZInputStream f(rawlog_fil);
216 
218 
219  // Depth -> 3D points:
222  mrpt::math::CHistogram hist(-0.15, 0.15, 30);
223  std::vector<double> ptsz;
224  std::vector<double> bin_x, bin_count;
225 
226  // decimation=1
227  {
229  obs->project3DPointsFromDepthImageInto(pts, pp);
230 
232  pts.getPointsBufferRef_z(), ptsz);
233  hist.add(ptsz);
234  hist.getHistogram(bin_x, bin_count);
235 
236  /*
237  //for (unsigned int i=0;i<bin_x.size();i++)
238  //std::cout << "Hist[" << i << "] x=" << bin_x[i] << " count= " <<
239  bin_count[i] << std::endl;
240  */
241 
242  EXPECT_LE(bin_count[11], 100);
243  EXPECT_LE(bin_count[12], 1000);
244  EXPECT_GE(bin_count[14], 12000);
245  EXPECT_LE(bin_count[18], 700);
246  EXPECT_LE(bin_count[19], 20);
247  }
248 
249  // decimation=8
250  {
252  pp.decimation = 8;
253  obs->project3DPointsFromDepthImageInto(pts, pp);
254 
256  pts.getPointsBufferRef_z(), ptsz);
257  hist.clear();
258  hist.add(ptsz);
259  hist.getHistogram(bin_x, bin_count);
260 
261  // for (unsigned int i = 0; i < bin_x.size(); i++)
262  // std::cout << "Hist[" << i << "] x=" << bin_x[i]
263  // << " count= " << bin_count[i] << std::endl;
264  /*
265  Hist[11] x=-0.0362069 count= 0
266  Hist[12] x=-0.0258621 count= 7
267  Hist[13] x=-0.0155172 count= 24
268  Hist[14] x=-0.00517241 count= 130
269  Hist[15] x=0.00517241 count= 234
270  Hist[16] x=0.0155172 count= 129
271  Hist[17] x=0.0258621 count= 85
272  Hist[18] x=0.0362069 count= 32
273  Hist[19] x=0.0465517 count= 3
274  */
275  EXPECT_LE(bin_count[11], 2);
276  EXPECT_LE(bin_count[12], 10);
277  EXPECT_GE(bin_count[14], 100);
278  EXPECT_LE(bin_count[18], 40);
279  EXPECT_LE(bin_count[19], 5);
280  }
281 }
282 #endif
void fillSampleObs(mrpt::obs::CObservation3DRangeScan &obs, mrpt::obs::T3DPointsProjectionParams &pp, int test_case)
#define TEST_RANGEIMG_WIDTH
This class provides an easy way of computing histograms for unidimensional real valued variables...
Definition: CHistogram.h:33
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...
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
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.
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::project3DPointsFromDepthImageInto()
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:591
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:562
#define TEST_RANGEIMG_HEIGHT
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) ...
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.
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.
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
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 project3DPointsFromDepthImageInto(POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams, const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Project the RGB+D images into a 3D point cloud (with color if the target map supports it) and optiona...
void clear()
Clear the histogram:
Definition: CHistogram.cpp:33
Transparently opens a compressed "gz" file and reads uncompressed data from it.
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
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 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020