test_mapcollection.cpp
Go to the documentation of this file.
00001 
00002 #include <stdio.h>
00003 #include <octomap/MapCollection.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 OcTree* generateSphereTree(point3d origin, float radius){
00012         OcTree* tree = new OcTree(0.05);
00013 
00014         point3d point_on_surface = origin;
00015         point_on_surface.x() += radius;
00016         for (int i=0; i<360; i++) {
00017                 for (int j=0; j<360; j++) {
00018                         if (!tree->insertRay(origin, origin+point_on_surface)) {
00019                                 cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl;
00020                         }
00021                         point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
00022                 }
00023                 point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
00024         }
00025         return tree;
00026 }
00027 
00028 int main(int argc, char** argv) {
00029 
00030   // //Generate a MapCollection
00031   // MapCollection<MapNode<OcTree> > coll;
00032   // OcTree* tree1 = generateSphereTree(point3d(0.01f,0.01f,0.01f), 2.0f);
00033   // MapNode<OcTree>* mn1 = new MapNode<OcTree>(tree1, pose6d(0.0,0.0,0.0,0.0,0.0,0.0));
00034   // mn1->setId("nodeone");
00035   // coll.addNode(mn1);
00036   // OcTree* tree2 = generateSphereTree(point3d(0.01f,0.01f,0.01f), 3.5f);
00037   // MapNode<OcTree>* mn2 = new MapNode<OcTree>(tree2, pose6d(3.0,7.0,10.0,0.0,0.0,0.0));
00038   // mn2->setId("nodetwo");
00039   // coll.addNode(mn2);
00040 
00041   // //Write the MapCollection
00042   // coll.write("tempcollection.txt");
00043 
00044   // //Read it back in
00045   // MapCollection<MapNode<OcTree> > coll_read("tempcollection.txt");
00046 
00047   //TODO do some ray operations
00048   //TODO do some isOccupied operations
00049   //TODO do some comparisons between original and re-read MaCollection
00050 
00051   //Read MapCollection from command line
00052   std::string filename(argv[1]);
00053   MapCollection<MapNode<OcTree> > collection(filename);
00054   EXPECT_TRUE(collection.size() > 0);
00055 
00056   //Write it to file
00057   collection.write("writeout.txt");
00058   //Write pointcloud to file
00059   //    collection.writePointcloud("test.vrml");
00060 
00061   //TODO delete temporary files?
00062   //tempcollection.txt
00063   //nodeone.bt
00064   //nodetwo.bt
00065   //writeout.txt
00066   //test.vrml
00067 
00068   std::vector<point3d> query;
00069   query.push_back(point3d(0,0,0));
00070   query.push_back(point3d(2,0,0));
00071   query.push_back(point3d(2,0,2));
00072   query.push_back(point3d(1.99,0,0));
00073   query.push_back(point3d(0,0,3));
00074   query.push_back(point3d(3,7,13.5));
00075   query.push_back(point3d(0,-1,-1));
00076 
00077   for (std::vector<point3d>::iterator it = query.begin(); it != query.end(); ++it) {
00078     point3d& q = *it;
00079     if (collection.isOccupied(q))
00080       printf("q (%0.2f, %0.2f, %0.2f) is occupied\n", q.x(), q.y(), q.z());
00081     else 
00082       printf("q (%0.2f, %0.2f, %0.2f) is NOT occupied\n", q.x(), q.y(), q.z());
00083     printf("in fact, it has an occupancy probability of %0.2f\n", collection.getOccupancy(q));
00084   }
00085 
00086   point3d ray_origin (0,0,10);
00087   point3d ray_direction (0,0,-10);
00088   point3d ray_end (100,100,100);
00089   
00090   if (collection.castRay(ray_origin, ray_direction, ray_end,true)) {
00091     printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
00092            ray_origin.x(), ray_origin.y(), ray_origin.z(),
00093            ray_direction.x(), ray_direction.y(), ray_direction.z(),
00094            ray_end.x(), ray_end.y(), ray_end.z());
00095   }
00096   else {
00097     printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
00098            ray_origin.x(), ray_origin.y(), ray_origin.z(),
00099            ray_direction.x(), ray_direction.y(), ray_direction.z());
00100   }
00101 
00102 
00103   printf("\n\n");
00104 
00105   ray_origin = point3d(0,0,-10);
00106   ray_direction = point3d(0,0,10);
00107   
00108   if (collection.castRay(ray_origin, ray_direction, ray_end,true)) {
00109     printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
00110            ray_origin.x(), ray_origin.y(), ray_origin.z(),
00111            ray_direction.x(), ray_direction.y(), ray_direction.z(),
00112            ray_end.x(), ray_end.y(), ray_end.z());
00113   }
00114   else {
00115     printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
00116            ray_origin.x(), ray_origin.y(), ray_origin.z(),
00117            ray_direction.x(), ray_direction.y(), ray_direction.z());
00118   }
00119 
00120   printf("\n\n");
00121 
00122   //....
00123 
00124   ray_origin = point3d(0,-1.5,-3);
00125   ray_direction = point3d(0,0,1);
00126   
00127   if (collection.castRay(ray_origin, ray_direction, ray_end,true, 5)) {
00128     printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
00129            ray_origin.x(), ray_origin.y(), ray_origin.z(),
00130            ray_direction.x(), ray_direction.y(), ray_direction.z(),
00131            ray_end.x(), ray_end.y(), ray_end.z());
00132   }
00133   else {
00134     printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
00135            ray_origin.x(), ray_origin.y(), ray_origin.z(),
00136            ray_direction.x(), ray_direction.y(), ray_direction.z());
00137   }
00138 
00139 
00140   return 0;
00141 }


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