MRPT  2.0.4
COccupancyGridMap3D_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>
15 #include <mrpt/obs/CSensoryFrame.h>
18 #include <mrpt/system/filesystem.h>
19 #include <test_mrpt_common.h>
20 
21 TEST(COccupancyGridMap3DTests, insert2DScan)
22 {
25 
26  // Insert the scan in the grid map and check expected values:
27  {
29  grid.insertObservation(scan1);
30 
31  // A cell in front of the laser should have a high "freeness"
32  EXPECT_GT(grid.getFreenessByPos(0.5, 0, 0), 0.53f);
33  }
34 }
35 
36 // We need OPENCV to read the image internal to CObservation3DRangeScan,
37 // so skip this test if built without opencv.
38 #if MRPT_HAS_OPENCV
39 
40 TEST(COccupancyGridMap3DTests, insertScan3D)
41 {
42  using namespace std::string_literals;
43  const auto fil =
44  mrpt::UNITTEST_BASEDIR + "/tests/test-3d-obs-ground.rawlog"s;
45  if (!mrpt::system::fileExists(fil))
46  {
47  GTEST_FAIL() << "ERROR: test due to missing file: " << fil << "\n";
48  return;
49  }
50 
51  // Load sample 3D scan from file:
55 
57  ASSERT_(obs);
58 
59  {
61  grid.insertObservation(*obs);
62 
63  // A cell in front of the laser should have a high "freeness"
64  EXPECT_GT(grid.getFreenessByPos(0.2f, 0.2f, 0.1f), 0.53f);
65  }
66 }
67 
68 #endif
EXPECT_GT(out.final_iters, 10UL)
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
void example2DRangeScan(mrpt::obs::CObservation2DRangeScan &s, int i=0)
Example 2D lidar scans (form a venerable SICK LMS200).
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...
TEST(COccupancyGridMap3DTests, insert2DScan)
float getFreenessByPos(float x, float y, float z) const
Read the real valued [0,1] contents of a voxel, given its coordinates.
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
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
A 3D occupancy grid map with a regular, even distribution of voxels.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Transparently opens a compressed "gz" file and reads uncompressed data from it.
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020