test_raycasting.cpp
Go to the documentation of this file.
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   //  point3d origin (10.01, 10.01, 10.02);
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         // hit unknown
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   // fill a cube with "free", end is "occupied":
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   // fill some "floor":
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   // hit the corner:
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   // hit bottom:
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   // hit boundary of unknown:
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   // hit boundary of octree:
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   // test maxrange:
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 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14