00001
00002 #include <stdio.h>
00003 #include <octomap/octomap.h>
00004 #include <octomap/math/Utils.h>
00005 #include "testing.h"
00006
00007 using namespace std;
00008 using namespace octomap;
00009 using namespace octomath;
00010
00011
00012
00013 int main(int argc, char** argv) {
00014
00015
00016
00017 OcTree tree (0.05);
00018
00019
00020
00021
00022 point3d origin (0.01f, 0.01f, 0.02f);
00023 point3d point_on_surface (2.01f, 0.01f, 0.01f);
00024
00025 cout << "Generating sphere at " << origin << " ..." << endl;
00026
00027 unsigned sphere_beams = 500;
00028 double angle = 2.0*M_PI/double(sphere_beams);
00029 Pointcloud p;
00030 for (unsigned i=0; i<sphere_beams; i++) {
00031 for (unsigned j=0; j<sphere_beams; j++) {
00032 p.push_back(origin+point_on_surface);
00033 point_on_surface.rotate_IP (0,0,angle);
00034 }
00035 point_on_surface.rotate_IP (0,angle,0);
00036 }
00037 tree.insertPointCloud(p, origin);
00038
00039
00040 cout << "Writing to sphere.bt..." << endl;
00041 EXPECT_TRUE(tree.writeBinary("sphere.bt"));
00042
00043
00044
00045 cout << "Casting rays in sphere ..." << endl;
00046
00047 OcTree sampled_surface (0.05);
00048
00049 point3d direction = point3d (1.0,0.0,0.0);
00050 point3d obstacle(0,0,0);
00051
00052 unsigned int hit (0);
00053 unsigned int miss (0);
00054 unsigned int unknown (0);
00055 double mean_dist(0);
00056
00057 for (unsigned i=0; i<sphere_beams; i++) {
00058 for (unsigned j=0; j<sphere_beams; j++) {
00059 if (!tree.castRay(origin, direction, obstacle, false, 3.)) {
00060
00061 if (!tree.search(obstacle))
00062 unknown++;
00063 else
00064 miss++;
00065 }
00066 else {
00067 hit++;
00068 mean_dist += (obstacle - origin).norm();
00069 sampled_surface.updateNode(obstacle, true);
00070 }
00071 direction.rotate_IP (0,0,angle);
00072 }
00073 direction.rotate_IP (0,angle,0);
00074 }
00075
00076 cout << "Writing sampled_surface.bt" << endl;
00077 EXPECT_TRUE(sampled_surface.writeBinary("sampled_surface.bt"));
00078
00079 mean_dist /= (double) hit;
00080 std::cout << " hits / misses / unknown: " << hit << " / " << miss << " / " << unknown << std::endl;
00081 std::cout << " mean obstacle dist: " << mean_dist << std::endl;
00082 EXPECT_NEAR(mean_dist, 2., 0.05);
00083 EXPECT_EQ(hit, (sphere_beams*sphere_beams));
00084 EXPECT_EQ(miss, 0);
00085 EXPECT_EQ(unknown, 0);
00086
00087
00088
00089
00090 cout << "generating single rays..." << endl;
00091 OcTree single_beams(0.03333);
00092 int num_beams = 17;
00093 float beamLength = 10.0f;
00094 point3d single_origin (1.0f, 0.45f, 0.45f);
00095 point3d single_origin_top (1.0f, 0.45f, 1.0);
00096 point3d single_endpoint(beamLength, 0.0f, 0.0f);
00097
00098
00099 for (int i=0; i<num_beams; i++) {
00100 for (int j=0; j<num_beams; j++) {
00101 if (!single_beams.insertRay(single_origin, single_origin+single_endpoint)) {
00102 cout << "ERROR while inserting ray from " << single_origin << " to " << single_endpoint << endl;
00103 }
00104 single_endpoint.rotate_IP (0,0,DEG2RAD(360.0/num_beams));
00105 }
00106 single_endpoint.rotate_IP (0,DEG2RAD(360.0/num_beams),0);
00107 }
00108
00109
00110 cout << "done." << endl;
00111 cout << "writing to beams.bt..." << endl;
00112 EXPECT_TRUE(single_beams.writeBinary("beams.bt"));
00113
00114
00116 double res = 0.1;
00117 double res_2 = res/2.0;
00118 OcTree cubeTree(res);
00119
00120 for (float x=-0.95; x <= 1.0; x+=res){
00121 for (float y=-0.95; y <= 1.0; y+=res){
00122 for (float z=-0.95; z <= 1.0; z+=res){
00123 if (x < 0.9){
00124 EXPECT_TRUE(cubeTree.updateNode(point3d(x,y,z), false));
00125 } else{
00126 EXPECT_TRUE(cubeTree.updateNode(point3d(x,y,z), true));
00127 }
00128 }
00129 }
00130 }
00131
00132
00133 EXPECT_TRUE(cubeTree.updateNode(res_2,res_2,-res_2, true));
00134 EXPECT_TRUE(cubeTree.updateNode(3*res_2,res_2,-res_2, true));
00135 EXPECT_TRUE(cubeTree.updateNode(-res_2,res_2,-res_2, true));
00136 EXPECT_TRUE(cubeTree.updateNode(-3*res_2,res_2,-res_2, true));
00137
00138 cubeTree.writeBinary("raycasting_cube.bt");
00139 origin = point3d(0.0f, 0.0f, 0.0f);
00140 point3d end;
00141
00142 direction = point3d(0.95f, 0.95f, 0.95f);
00143 EXPECT_TRUE(cubeTree.castRay(origin, direction, end, false));
00144 EXPECT_TRUE(cubeTree.isNodeOccupied(cubeTree.search(end)));
00145 std::cout << "Hit occupied voxel: " << end << std::endl;
00146 direction = point3d(1.0, 0.0, 0.0);
00147 EXPECT_TRUE(cubeTree.castRay(origin, direction, end, false));
00148 EXPECT_TRUE(cubeTree.isNodeOccupied(cubeTree.search(end)));
00149 std::cout << "Hit occupied voxel: " << end << std::endl;
00150 EXPECT_NEAR(1.0, (origin - end).norm(), res_2);
00151
00152
00153 origin = point3d(res_2, res_2, 0.5f);
00154 direction = point3d(0.0, 0.0, -1.0f);
00155 EXPECT_TRUE(cubeTree.castRay(origin, direction, end, false));
00156 EXPECT_TRUE(cubeTree.isNodeOccupied(cubeTree.search(end)));
00157 std::cout << "Hit voxel: " << end << std::endl;
00158 EXPECT_FLOAT_EQ(origin(0), end(0));
00159 EXPECT_FLOAT_EQ(origin(1), end(1));
00160 EXPECT_FLOAT_EQ(-res_2, end(2));
00161
00162
00163
00164 origin = point3d(0.0f, 0.0f, 0.0f);
00165 direction = point3d(0.0f, 1.0f, 0.0f);
00166 EXPECT_FALSE(cubeTree.castRay(origin, direction, end, false));
00167 EXPECT_FALSE(cubeTree.search(end));
00168 std::cout << "Boundary unknown hit: " << end << std::endl;
00169
00170
00171 EXPECT_FALSE(cubeTree.castRay(origin, direction, end, true));
00172 EXPECT_FALSE(cubeTree.search(end));
00173 EXPECT_FLOAT_EQ(end.x(), res_2);
00174 EXPECT_FLOAT_EQ(end.y(), float(32768*res-res_2));
00175 EXPECT_FLOAT_EQ(end.z(), res_2);
00176 direction = point3d(-1.0f, 0.0f, 0.0f);
00177 EXPECT_FALSE(cubeTree.castRay(origin, direction, end, true));
00178 EXPECT_FALSE(cubeTree.search(end));
00179 EXPECT_FLOAT_EQ(end.x(), float(-32767*res-res_2));
00180 EXPECT_FLOAT_EQ(end.y(), res_2);
00181 EXPECT_FLOAT_EQ(end.z(), res_2);
00182
00183
00184 EXPECT_FALSE(cubeTree.castRay(origin, direction, end, true, 0.9));
00185 std::cout << "Max range endpoint: " << end << std::endl;
00186 OcTreeNode* endPt = cubeTree.search(end);
00187 EXPECT_TRUE(endPt);
00188 EXPECT_FALSE(cubeTree.isNodeOccupied(endPt));
00189 double dist = (origin - end).norm();
00190 EXPECT_NEAR(0.9, dist, res);
00191
00192
00193 std::cout << "Test successful\n";
00194 return 0;
00195 }