test_bbx.cpp
Go to the documentation of this file.
1 
2 #include <octomap/octomap.h>
3 #include "testing.h"
4 
5 using namespace octomap;
6 
7 int main(int /*argc*/, char** /*argv*/) {
8  const float resolution = 0.2f;
9  OcTree tree(resolution);
10 
11  // Set up the bounding box.
12  const float bbx_limit = 10.1f;
13  point3d bbx_min(-bbx_limit, -bbx_limit, -bbx_limit);
14  point3d bbx_max(bbx_limit, bbx_limit, bbx_limit);
15  tree.setBBXMin(bbx_min);
16  tree.setBBXMax(bbx_max);
17  tree.useBBXLimit(true);
18 
19  // Set up the point cloud. Offset the origin by half resolution to use the
20  // center of a voxel.
21  Pointcloud cloud;
22  const point3d origin(resolution / 2.f, 5.f + resolution / 2.f, resolution / 2.f);
23  const float maxrange = 8.f;
24  // The first point is inside the bounding box and max range.
25  const point3d point0 = origin + point3d(5.f, 0.f, 0.f);
26  cloud.push_back(point0);
27  // The second point is outside the bounding box but within max range.
28  const point3d point1 = origin + point3d(0.f, 7.f, 0.f);
29  cloud.push_back(point1);
30  // The third point is inside the bounding box but outside max range.
31  const point3d point2 = origin + point3d(0.f, 0.f, 9.f);
32  cloud.push_back(point2);
33  tree.insertPointCloud(cloud, origin, maxrange);
34 
35  // Check the point cloud insertion using ray casting.
36  tree.setOccupancyThres(0.5f);
37  point3d end_point;
38 
39  // Searching in the x-direction from the origin finds the first point.
40  bool ray_cast_ret = tree.castRay(origin, point3d(1.f, 0.f, 0.f), end_point);
41  EXPECT_TRUE(ray_cast_ret);
42  const float eps = 1e-3;
43  EXPECT_NEAR(end_point.x(), point0.x(), eps);
44  EXPECT_NEAR(end_point.y(), point0.y(), eps);
45  EXPECT_NEAR(end_point.z(), point0.z(), eps);
46 
47  // Searching in the y-direction from the origin terminates just outside the
48  // bounding box.
49  ray_cast_ret = tree.castRay(origin, point3d(0.f, 1.f, 0.f), end_point);
50  EXPECT_FALSE(ray_cast_ret);
51  EXPECT_NEAR(end_point.x(), point1.x(), eps);
52  EXPECT_NEAR(end_point.y(), bbx_limit + resolution, eps);
53  EXPECT_NEAR(end_point.z(), point1.z(), eps);
54 
55  // Searching in the z-direction from the origin terminates at the max range.
56  ray_cast_ret = tree.castRay(origin, point3d(0.f, 0.f, 1.f), end_point);
57  EXPECT_FALSE(ray_cast_ret);
58  EXPECT_NEAR(end_point.x(), point2.x(), eps);
59  EXPECT_NEAR(end_point.y(), point2.y(), eps);
60  EXPECT_NEAR(end_point.z(), origin.z() + maxrange, eps);
61  return 0;
62 }
octomap::OccupancyOcTreeBase::castRay
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
octomap::Pointcloud
Definition: Pointcloud.h:47
octomap::AbstractOccupancyOcTree::setOccupancyThres
void setOccupancyThres(double prob)
sets the threshold for occupancy (sensor model)
Definition: AbstractOccupancyOcTree.h:188
octomap::Pointcloud::push_back
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
testing.h
main
int main(int, char **)
Definition: test_bbx.cpp:7
EXPECT_TRUE
#define EXPECT_TRUE(args)
Definition: testing.h:6
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomap::OcTree
Definition: OcTree.h:49
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
Definition: testing.h:27
octomap::OccupancyOcTreeBase::setBBXMax
void setBBXMax(const point3d &max)
sets the maximum for a query bounding box to use
octomap.h
octomath::Vector3::y
float & y()
Definition: Vector3.h:136
octomap::OccupancyOcTreeBase::setBBXMin
void setBBXMin(const point3d &min)
sets the minimum for a query bounding box to use
octomap
octomap::OccupancyOcTreeBase::useBBXLimit
void useBBXLimit(bool enable)
use or ignore BBX limit (default: ignore)
Definition: OccupancyOcTreeBase.h:334
octomath::Vector3::z
float & z()
Definition: Vector3.h:141
octomap::point3d
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
octomath::Vector3::x
float & x()
Definition: Vector3.h:131
EXPECT_FALSE
#define EXPECT_FALSE(args)
Definition: testing.h:11
octomap::OccupancyOcTreeBase::insertPointCloud
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Tue Dec 12 2023 03:39:40