Main MRPT website > C++ reference for MRPT 1.5.7
CObservation3DRangeScan_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
11 #include <mrpt/config.h>
13 #include <mrpt/math/CHistogram.h>
15 #include <mrpt/obs/CSensoryFrame.h>
16 #include <mrpt/system/filesystem.h>
19 
20 // Defined in tests/test_main.cpp
21 namespace mrpt {
22 namespace utils {
24 }
25 } // namespace mrpt
26 
27 using namespace mrpt;
28 using namespace std;
29 
30 #define TEST_RANGEIMG_WIDTH 32
31 #define TEST_RANGEIMG_HEIGHT 24
32 
34  mrpt::obs::T3DPointsProjectionParams &pp, int test_case) {
35  obs.hasRangeImage = true;
37 
38  obs.rangeImage.setZero();
39 
40  for (int r = 10; r < 16; r++)
41  for (int c = 10; c <= r; c++)
42  obs.rangeImage(r, c) = r;
43 
44  // Test case:
45  pp.PROJ3D_USE_LUT = (test_case & 1) != 0;
46  pp.USE_SSE2 = (test_case & 2) != 0;
47  pp.takeIntoAccountSensorPoseOnRobot = (test_case & 4) != 0;
48 }
49 
50 TEST(CObservation3DRangeScan, Project3D_noFilter) {
53 
54  for (int i = 0; i < 8; i++) // test all combinations of flags
55  {
57  fillSampleObs(o, pp, i);
58 
60  EXPECT_EQ(o.points3D_x.size(), 21U)
61  << " testcase flags: i=" << i << std::endl;
62  }
63 }
64 
65 TEST(CObservation3DRangeScan, Project3D_filterMinMax1) {
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) {
94  // Default filter=0.0f -> no filtering
95  for (int r = 10; r < 16; r++)
96  for (int c = 10; c < 16; c++) {
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) {
121  // Default filter=0.0f -> no filtering
122  for (int r = 10; r < 16; r++)
123  for (int c = 10; c < 16; c++) {
124  fMin(r, c) = r + 1.1f; // No point lies in between
125  fMax(r, c) = r + 1.2f;
126  }
127 
130  fp.rangeMask_min = &fMin;
131  fp.rangeMask_max = &fMax;
132 
133  for (int i = 0; i < 16; i++) // test all combinations of flags
134  {
136  fillSampleObs(o, pp, i);
137  fp.rangeCheckBetween = (i & 8) != 0;
138 
140  EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 0U : 21U)
141  << " testcase flags: i=" << i << std::endl;
142  }
143 }
144 
145 TEST(CObservation3DRangeScan, Project3D_filterMin) {
147  // Default filter=0.0f -> no filtering
148  for (int r = 10; r < 16; r++)
149  for (int c = 10; c < 16; c++)
150  fMin(r, c) = 14.5f; // Only last row of points should pass this filter
151 
154  fp.rangeMask_min = &fMin;
155 
156  for (int i = 0; i < 8; i++) // test all combinations of flags
157  {
159  fillSampleObs(o, pp, i);
160 
162  EXPECT_EQ(o.points3D_x.size(), 6U)
163  << " testcase flags: i=" << i << std::endl;
164  }
165 }
166 
167 TEST(CObservation3DRangeScan, Project3D_filterMax) {
169  // Default filter=0.0f -> no filtering
170  for (int r = 10; r < 16; r++)
171  for (int c = 10; c < 16; c++)
172  fMax(r, c) = 11.5f; // Only first 2 rows of points should pass this filter
173 
176  fp.rangeMask_max = &fMax;
177 
178  for (int i = 0; i < 8; i++) // test all combinations of flags
179  {
181  fillSampleObs(o, pp, i);
182 
184  EXPECT_EQ(o.points3D_x.size(), 3U)
185  << " testcase flags: i=" << i << std::endl;
186  }
187 }
188 
189 // We need OPENCV to read the image internal to CObservation3DRangeScan,
190 // so skip this test if built without opencv.
191 #if MRPT_HAS_OPENCV
192 
193 TEST(CObservation3DRangeScan, LoadAndCheckFloorPoints) {
194  const string rawlog_fil = mrpt::utils::MRPT_GLOBAL_UNITTEST_SRC_DIR +
195  string("/tests/test-3d-obs-ground.rawlog");
196  if (!mrpt::system::fileExists(rawlog_fil)) {
197  GTEST_FAIL() << "ERROR: test due to missing file: " << rawlog_fil << "\n";
198  return;
199  }
200 
201  // Load sample 3D scan from file:
203  mrpt::utils::CFileGZInputStream f(rawlog_fil);
204  f >> sf;
205 
207 
208  // Depth -> 3D points:
211  obs->project3DPointsFromDepthImageInto(pts, pp);
212  // rotate to account for the sensor pose:
213  pts.changeCoordinatesReference(obs->sensorPose);
214 
215  mrpt::math::CHistogram hist(-0.15, 0.15, 30);
216  std::vector<double> ptsz;
218  pts.getPointsBufferRef_z(), ptsz);
219  hist.add(ptsz);
220 
221  std::vector<double> bin_x, bin_count;
222  hist.getHistogram(bin_x, bin_count);
223 
224  /*
225  Hist[0] x=-0.15 count= 0
226  Hist[1] x=-0.139655 count= 0
227  Hist[2] x=-0.12931 count= 0
228  Hist[3] x=-0.118966 count= 0
229  Hist[4] x=-0.108621 count= 0
230  Hist[5] x=-0.0982759 count= 0
231  Hist[6] x=-0.087931 count= 0
232  Hist[7] x=-0.0775862 count= 0
233  Hist[8] x=-0.0672414 count= 0
234  Hist[9] x=-0.0568966 count= 0
235  Hist[10] x=-0.0465517 count= 1
236  Hist[11] x=-0.0362069 count= 47
237  Hist[12] x=-0.0258621 count= 942
238  Hist[13] x=-0.0155172 count= 5593
239  Hist[14] x=-0.00517241 count= 12903
240  Hist[15] x=0.00517241 count= 9116
241  Hist[16] x=0.0155172 count= 4666
242  Hist[17] x=0.0258621 count= 2677
243  Hist[18] x=0.0362069 count= 595
244  Hist[19] x=0.0465517 count= 4
245  Hist[20] x=0.0568966 count= 0
246  Hist[21] x=0.0672414 count= 0
247  Hist[22] x=0.0775862 count= 0
248  Hist[23] x=0.087931 count= 0
249  Hist[24] x=0.0982759 count= 0
250  Hist[25] x=0.108621 count= 0
251  Hist[26] x=0.118966 count= 0
252  Hist[27] x=0.12931 count= 0
253  Hist[28] x=0.139655 count= 0
254  Hist[29] x=0.15 count= 0
255  //for (unsigned int i=0;i<bin_x.size();i++)
256  //std::cout << "Hist[" << i << "] x=" << bin_x[i] << " count= " <<
257  bin_count[i] << std::endl;
258  */
259 
260  EXPECT_LE(bin_count[11], 100);
261  EXPECT_LE(bin_count[12], 1000);
262  EXPECT_GE(bin_count[14], 12000);
263  EXPECT_LE(bin_count[18], 700);
264  EXPECT_LE(bin_count[19], 20);
265 }
266 #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:35
std::string MRPT_GLOBAL_UNITTEST_SRC_DIR
bool BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
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.
const mrpt::math::CMatrix * rangeMask_min
(Default: NULL) If provided, each data range will be tested to be greater-than (rangeMask_min) or les...
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)
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
#define TEST_RANGEIMG_HEIGHT
const GLubyte * c
Definition: glext.h:5590
void add(const double x)
Add an element to the histogram.
Definition: CHistogram.cpp:42
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
bool hasRangeImage
true means the field rangeImage contains valid data
void getHistogram(std::vector< double > &x, std::vector< double > &hits) const
Returns the list of bin centers & hit counts.
Definition: CHistogram.cpp:78
GLsizei const GLchar ** string
Definition: glext.h:3919
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
const mrpt::math::CMatrix * rangeMask_max
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
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...
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
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 NULL if there is no such...
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 takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated. Otherwise, points are transformed with sensorPose. Furthermore, if provided, those coordinates are transformed with robotPoseInTheWorld
bool rangeCheckBetween
Only used if both rangeMask_min and rangeMask_max are present.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019