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
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 std::string filename(argv[1]);
00053 MapCollection<MapNode<OcTree> > collection(filename);
00054 EXPECT_TRUE(collection.size() > 0);
00055
00056
00057 collection.write("writeout.txt");
00058
00059
00060
00061
00062
00063
00064
00065
00066
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 }