14 point3d point_on_surface = origin;
15 point_on_surface.
x() += radius;
16 for (
int i=0; i<360; i++) {
17 for (
int j=0; j<360; j++) {
18 if (!tree->
insertRay(origin, origin+point_on_surface)) {
19 cout <<
"ERROR while inserting ray from " << origin <<
" to " << point_on_surface << endl;
28 int main(
int argc,
char** argv) {
52 std::string filename(argv[1]);
57 collection.
write(
"writeout.txt");
68 std::vector<point3d> query;
69 query.push_back(
point3d(0,0,0));
70 query.push_back(
point3d(2,0,0));
71 query.push_back(
point3d(2,0,2));
72 query.push_back(
point3d(1.99f,0.0f,0.0f));
73 query.push_back(
point3d(0,0,3));
74 query.push_back(
point3d(3,7,13.5));
75 query.push_back(
point3d(0,-1,-1));
77 for (std::vector<point3d>::iterator it = query.begin(); it != query.end(); ++it) {
80 printf(
"q (%0.2f, %0.2f, %0.2f) is occupied\n", q.
x(), q.
y(), q.
z());
82 printf(
"q (%0.2f, %0.2f, %0.2f) is NOT occupied\n", q.
x(), q.
y(), q.
z());
83 printf(
"in fact, it has an occupancy probability of %0.2f\n", collection.
getOccupancy(q));
87 point3d ray_direction (0,0,-10);
90 if (collection.
castRay(ray_origin, ray_direction, ray_end,
true)) {
91 printf(
"ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
92 ray_origin.x(), ray_origin.y(), ray_origin.z(),
93 ray_direction.
x(), ray_direction.
y(), ray_direction.
z(),
94 ray_end.
x(), ray_end.
y(), ray_end.
z());
97 printf(
"ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
98 ray_origin.x(), ray_origin.y(), ray_origin.z(),
99 ray_direction.
x(), ray_direction.
y(), ray_direction.
z());
106 ray_direction =
point3d(0,0,10);
108 if (collection.
castRay(ray_origin, ray_direction, ray_end,
true)) {
109 printf(
"ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
110 ray_origin.x(), ray_origin.y(), ray_origin.z(),
111 ray_direction.
x(), ray_direction.
y(), ray_direction.
z(),
112 ray_end.
x(), ray_end.
y(), ray_end.
z());
115 printf(
"ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
116 ray_origin.x(), ray_origin.y(), ray_origin.z(),
117 ray_direction.
x(), ray_direction.
y(), ray_direction.
z());
124 ray_origin =
point3d(0,-1.5,-3);
125 ray_direction =
point3d(0,0,1);
127 if (collection.
castRay(ray_origin, ray_direction, ray_end,
true, 5)) {
128 printf(
"ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
129 ray_origin.x(), ray_origin.y(), ray_origin.z(),
130 ray_direction.
x(), ray_direction.
y(), ray_direction.
z(),
131 ray_end.
x(), ray_end.
y(), ray_end.
z());
134 printf(
"ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
135 ray_origin.x(), ray_origin.y(), ray_origin.z(),
136 ray_direction.
x(), ray_direction.
y(), ray_direction.
z());
bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Vector3 & rotate_IP(double roll, double pitch, double yaw)
bool isOccupied(const point3d &p) const
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
double getOccupancy(const point3d &p)
This class represents a three-dimensional vector.
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
int main(int argc, char **argv)
OcTree * generateSphereTree(point3d origin, float radius)
bool write(std::string filename)
#define EXPECT_TRUE(args)