13 int main(
int argc,
char** argv) {
22 point3d origin (0.01f, 0.01f, 0.02f);
23 point3d point_on_surface (2.01f, 0.01f, 0.01f);
25 cout <<
"Generating sphere at " << origin <<
" ..." << endl;
27 unsigned sphere_beams = 500;
28 double angle = 2.0*
M_PI/double(sphere_beams);
30 for (
unsigned i=0; i<sphere_beams; i++) {
31 for (
unsigned j=0; j<sphere_beams; j++) {
40 cout <<
"Writing to sphere.bt..." << endl;
45 cout <<
"Casting rays in sphere ..." << endl;
47 OcTree sampled_surface (0.05);
53 unsigned int miss (0);
54 unsigned int unknown (0);
57 for (
unsigned i=0; i<sphere_beams; i++) {
58 for (
unsigned j=0; j<sphere_beams; j++) {
59 if (!tree.
castRay(origin, direction, obstacle,
false, 3.)) {
61 if (!tree.
search(obstacle))
68 mean_dist += (obstacle - origin).norm();
76 cout <<
"Writing sampled_surface.bt" << endl;
79 mean_dist /= (double) hit;
80 std::cout <<
" hits / misses / unknown: " << hit <<
" / " << miss <<
" / " << unknown << std::endl;
81 std::cout <<
" mean obstacle dist: " << mean_dist << std::endl;
83 EXPECT_EQ(hit, (sphere_beams*sphere_beams));
90 cout <<
"generating single rays..." << endl;
91 OcTree single_beams(0.03333);
93 float beamLength = 10.0f;
94 point3d single_origin (1.0f, 0.45f, 0.45f);
95 point3d single_origin_top (1.0f, 0.45f, 1.0);
96 point3d single_endpoint(beamLength, 0.0f, 0.0f);
99 for (
int i=0; i<num_beams; i++) {
100 for (
int j=0; j<num_beams; j++) {
101 if (!single_beams.
insertRay(single_origin, single_origin+single_endpoint)) {
102 cout <<
"ERROR while inserting ray from " << single_origin <<
" to " << single_endpoint << endl;
110 cout <<
"done." << endl;
111 cout <<
"writing to beams.bt..." << endl;
117 double res_2 = res/2.0;
120 for (
float x=-0.95f; x <= 1.0f; x+=float(res)){
121 for (
float y=-0.95f; y <= 1.0f; y+= float(res)){
122 for (
float z=-0.95f; z <= 1.0f; z+= float(res)){
139 origin =
point3d(0.0f, 0.0f, 0.0f);
142 direction =
point3d(0.95f, 0.95f, 0.95f);
145 std::cout <<
"Hit occupied voxel: " << end << std::endl;
146 direction =
point3d(1.0, 0.0, 0.0);
149 std::cout <<
"Hit occupied voxel: " << end << std::endl;
153 origin =
point3d(
float(res_2),
float(res_2), 0.5f);
154 direction =
point3d(0.0, 0.0, -1.0f);
157 std::cout <<
"Hit voxel: " << end << std::endl;
164 origin =
point3d(0.0f, 0.0f, 0.0f);
165 direction =
point3d(0.0f, 1.0f, 0.0f);
168 std::cout <<
"Boundary unknown hit: " << end << std::endl;
176 direction =
point3d(-1.0f, 0.0f, 0.0f);
185 std::cout <<
"Max range endpoint: " << end << std::endl;
189 double dist = (origin - end).norm();
193 std::cout <<
"Test successful\n";
int main(int argc, char **argv)
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
NODE * search(double x, double y, double z, unsigned int depth=0) const
void push_back(float x, float y, float z)
#define EXPECT_NEAR(a, b, prec)
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
Vector3 & rotate_IP(double roll, double pitch, double yaw)
#define EXPECT_FALSE(args)
#define EXPECT_FLOAT_EQ(a, b)
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
bool writeBinary(const std::string &filename)
This class represents a three-dimensional vector.
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
#define EXPECT_TRUE(args)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree's parameter for "occupancy" ...