43 cout <<
"occupancy probability at " << query <<
":\t " << node->
getOccupancy() << endl;
46 cout <<
"occupancy probability at " << query <<
":\t is unknown" << endl;
49 int main(
int argc,
char** argv) {
52 cout <<
"generating example map" << endl;
59 for (
int x=-20; x<20; x++) {
60 for (
int y=-20; y<20; y++) {
61 for (
int z=-20; z<20; z++) {
62 point3d endpoint ((
float) x*0.05f, (
float) y*0.05f, (
float) z*0.05f);
70 for (
int x=-30; x<30; x++) {
71 for (
int y=-30; y<30; y++) {
72 for (
int z=-30; z<30; z++) {
73 point3d endpoint ((
float) x*0.02f-1.0f, (
float) y*0.02f-1.0f, (
float) z*0.02f-1.0f);
80 cout <<
"performing some queries:" << endl;
87 result = tree.
search (query);
91 result = tree.
search (query);
97 cout <<
"wrote example file simple_tree.bt" << endl << endl;
98 cout <<
"now you can use octovis to visualize: octovis simple_tree.bt" << endl;
99 cout <<
"Hint: hit 'F'-key in viewer to see the freespace" << endl << endl;
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
NODE * search(double x, double y, double z, unsigned int depth=0) const
void print_query_info(point3d query, OcTreeNode *node)
bool writeBinary(const std::string &filename)
double getOccupancy() const
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)