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  for (int r = 10; r < 16; r++)
38  for (int c = 10; c <= r; c++) obs.rangeImage(r, c) = r;
39 
40  // Test case:
41  pp.PROJ3D_USE_LUT = (test_case & 1) != 0;
42  pp.USE_SSE2 = (test_case & 2) != 0;
43  pp.takeIntoAccountSensorPoseOnRobot = (test_case & 4) != 0;
44 }
45 
46 TEST(CObservation3DRangeScan, Project3D_noFilter)
47 {
50 
51  for (int i = 0; i < 8; i++) // test all combinations of flags
52  {
54  fillSampleObs(o, pp, i);
55 
57  EXPECT_EQ(o.points3D_x.size(), 21U)
58  << " testcase flags: i=" << i << std::endl;
59  }
60 }
61 
62 TEST(CObservation3DRangeScan, Project3D_filterMinMax1)
63 {
66  fMin(12, 12) = 11.5f;
67  fMax(12, 12) = 12.5f; // pass
68  fMin(14, 14) = 15.5f;
69  fMax(14, 14) = 16.5f; // don't pass
70  // Rest of points: filter=0.0f -> no filtering
71 
74  fp.rangeMask_min = &fMin;
75  fp.rangeMask_max = &fMax;
76 
77  for (int i = 0; i < 16; i++) // test all combinations of flags
78  {
80  fillSampleObs(o, pp, i);
81  fp.rangeCheckBetween = (i & 8) != 0;
82 
84  EXPECT_EQ(o.points3D_x.size(), 20U)
85  << " testcase flags: i=" << i << std::endl;
86  }
87 }
88 
89 TEST(CObservation3DRangeScan, Project3D_filterMinMaxAllBetween)
90 {
93  // Default filter=0.0f -> no filtering
94  for (int r = 10; r < 16; r++)
95  for (int c = 10; c < 16; c++)
96  {
97  fMin(r, c) = r - 0.1f; // All points actually lie in between
98  fMax(r, c) = r + 0.1f;
99  }
100 
103  fp.rangeMask_min = &fMin;
104  fp.rangeMask_max = &fMax;
105 
106  for (int i = 0; i < 16; i++) // test all combinations of flags
107  {
109  fillSampleObs(o, pp, i);
110  fp.rangeCheckBetween = (i & 8) != 0;
111 
113  EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 21U : 0U)
114  << " testcase flags: i=" << i << std::endl;
115  }
116 }
117 
118 TEST(CObservation3DRangeScan, Project3D_filterMinMaxNoneBetween)
119 {
122  // Default filter=0.0f -> no filtering
123  for (int r = 10; r < 16; r++)
124  for (int c = 10; c < 16; c++)
125  {
126  fMin(r, c) = r + 1.1f; // No point lies in between
127  fMax(r, c) = r + 1.2f;
128  }
129 
132  fp.rangeMask_min = &fMin;
133  fp.rangeMask_max = &fMax;
134 
135  for (int i = 0; i < 16; i++) // test all combinations of flags
136  {
138  fillSampleObs(o, pp, i);
139  fp.rangeCheckBetween = (i & 8) != 0;
140 
142  EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 0U : 21U)
143  << " testcase flags: i=" << i << std::endl;
144  }
145 }
146 
147 TEST(CObservation3DRangeScan, Project3D_filterMin)
148 {
150  // Default filter=0.0f -> no filtering
151  for (int r = 10; r < 16; r++)
152  for (int c = 10; c < 16; c++)
153  fMin(r, c) =
154  14.5f; // Only last row of points should pass this filter
155 
158  fp.rangeMask_min = &fMin;
159 
160  for (int i = 0; i < 8; i++) // test all combinations of flags
161  {
163  fillSampleObs(o, pp, i);
164 
166  EXPECT_EQ(o.points3D_x.size(), 6U)
167  << " testcase flags: i=" << i << std::endl;
168  }
169 }
170 
171 TEST(CObservation3DRangeScan, Project3D_filterMax)
172 {
174  // Default filter=0.0f -> no filtering
175  for (int r = 10; r < 16; r++)
176  for (int c = 10; c < 16; c++)
177  fMax(r, c) =
178  11.5f; // Only first 2 rows of points should pass this filter
179 
182  fp.rangeMask_max = &fMax;
183 
184  for (int i = 0; i < 8; i++) // test all combinations of flags
185  {
187  fillSampleObs(o, pp, i);
188 
190  EXPECT_EQ(o.points3D_x.size(), 3U)
191  << " testcase flags: i=" << i << std::endl;
192  }
193 }
194 
195 // We need OPENCV to read the image internal to CObservation3DRangeScan,
196 // so skip this test if built without opencv.
197 #if MRPT_HAS_OPENCV
198 
199 TEST(CObservation3DRangeScan, LoadAndCheckFloorPoints)
200 {
201  const string rawlog_fil =
202  UNITTEST_BASEDIR + string("/tests/test-3d-obs-ground.rawlog");
203  if (!mrpt::system::fileExists(rawlog_fil))
204  {
205  GTEST_FAIL() << "ERROR: test due to missing file: " << rawlog_fil
206  << "\n";
207  return;
208  }
209 
210  // Load sample 3D scan from file:
212  mrpt::io::CFileGZInputStream f(rawlog_fil);
214 
216 
217  // Depth -> 3D points:
220  mrpt::math::CHistogram hist(-0.15, 0.15, 30);
221  std::vector<double> ptsz;
222  std::vector<double> bin_x, bin_count;
223 
224  // decimation=1
225  {
227  obs->project3DPointsFromDepthImageInto(pts, pp);
228 
230  pts.getPointsBufferRef_z(), ptsz);
231  hist.add(ptsz);
232  hist.getHistogram(bin_x, bin_count);
233 
234  /*
235  //for (unsigned int i=0;i<bin_x.size();i++)
236  //std::cout << "Hist[" << i << "] x=" << bin_x[i] << " count= " <<
237  bin_count[i] << std::endl;
238  */
239 
240  EXPECT_LE(bin_count[11], 100);
241  EXPECT_LE(bin_count[12], 1000);
242  EXPECT_GE(bin_count[14], 12000);
243  EXPECT_LE(bin_count[18], 700);
244  EXPECT_LE(bin_count[19], 20);
245  }
246 
247  // decimation=8
248  {
250  pp.decimation = 8;
251  obs->project3DPointsFromDepthImageInto(pts, pp);
252 
254  pts.getPointsBufferRef_z(), ptsz);
255  hist.clear();
256  hist.add(ptsz);
257  hist.getHistogram(bin_x, bin_count);
258 
259  // for (unsigned int i = 0; i < bin_x.size(); i++)
260  // std::cout << "Hist[" << i << "] x=" << bin_x[i]
261  // << " count= " << bin_count[i] << std::endl;
262  /*
263  Hist[11] x=-0.0362069 count= 0
264  Hist[12] x=-0.0258621 count= 7
265  Hist[13] x=-0.0155172 count= 24
266  Hist[14] x=-0.00517241 count= 130
267  Hist[15] x=0.00517241 count= 234
268  Hist[16] x=0.0155172 count= 129
269  Hist[17] x=0.0258621 count= 85
270  Hist[18] x=0.0362069 count= 32
271  Hist[19] x=0.0465517 count= 3
272  */
273  EXPECT_LE(bin_count[11], 2);
274  EXPECT_LE(bin_count[12], 10);
275  EXPECT_GE(bin_count[14], 100);
276  EXPECT_LE(bin_count[18], 40);
277  EXPECT_LE(bin_count[19], 5);
278  }
279 }
280 #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...
mrpt::math::CMatrixF rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
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.
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
const GLubyte * c
Definition: glext.h:6406
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
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
GLsizei const GLchar ** string
Definition: glext.h:4116
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.
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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 ...
T::Ptr getObservationByClass(const 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...
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: 1d7b2c899 Thu Nov 14 18:59:07 2019 +0100 at jue nov 14 19:15:09 CET 2019