int main(int argc, char** argv) {
// default values:
string vrmlFilename = "";
string btFilename = "";
if (argc != 2 || (argc > 1 && strcmp(argv[1], "-h") == 0)){
printUsage(argv[0]);
}
btFilename = std::string(argv[1]);
vrmlFilename = btFilename + ".wrl";
cout << "\nReading OcTree file\n===========================\n";
// TODO: check if file exists and if OcTree read correctly?
OcTree* tree = new OcTree(btFilename);
cout << "\nWriting occupied volumes to VRML\n===========================\n";
std::ofstream outfile (vrmlFilename.c_str());
outfile << "#VRML V2.0 utf8\n#\n";
outfile << "# created from OctoMap file "<<btFilename<< " with bt2vrml\n";
size_t count(0);
for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
if(tree->isNodeOccupied(*it)){
count++;
double size = it.getSize();
outfile << "Transform { translation "
<< it.getX() << " " << it.getY() << " " << it.getZ()
<< " \n children ["
<< " Shape { geometry Box { size "
<< size << " " << size << " " << size << "} } ]\n"
<< "}\n";
}
}
delete tree;
outfile.close();
std::cout << "Finished writing "<< count << " voxels to " << vrmlFilename << std::endl;
return 0;
}
//float xx[70000], yy[70000], zz[70000];
void findProbabilityOfCones3D(Frustum frustum[], int num_poses3D)
{
OcTree* input_tree = retrieve_octree(); //new OcTree(map)
int free = 0;
int occupied = 0;
std::cerr<<"Inside findProbabilityOfCones3D"<< std::endl;
for(OcTree::leaf_iterator it = input_tree->begin_leafs(),
end=input_tree->end_leafs(); it!= end; ++it)
{
if (input_tree->isNodeOccupied(*it))
{
double size = it.getSize();
double x = it.getX();
double y = it.getY();
double z = it.getZ();
//point3d p = it.getCoordinate();
//xx[occupied] = p.x;
//yy[occupied] = p.y;
//zz[occupied] = p.z;
occupied++;
//std::cerr<<"X: "<< x<< std::endl;
//std::cerr<<"Y: "<< y<< std::endl;
//std::cerr<<"Z: "<< z<< std::endl;
for (int i = 0; i < num_poses3D; i++)
{
if (checkInsideCone3D(x, y, z, frustum[i]))
{
frustum[i].probability += 1 + (QSR_WEIGHT * (normal_dist_2d(x, y , QSR_MEAN_1 , QSR_VAR_1 , QSR_MEAN_2 , QSR_VAR_2)));
}
}
}
else
{
free++;
}
}
std::cerr<<"occupied "<< occupied<< std::endl;
std::cerr<<"free "<< free<< std::endl;
}
int compute_value(Frustum frustum, std::vector<unsigned short int> &keys, std::vector<int> &node_values)
{
int value = 0;
int free = 0;
int occupied = 0;
int WEIGHT = 1; // compute weight from QSR model
if (input_tree == NULL)
return 0;
for(OcTree::leaf_iterator it = input_tree->begin_leafs(),
end=input_tree->end_leafs(); it!= end; ++it)
{
if (input_tree->isNodeOccupied(*it))
{
int size = (int) (it.getSize() / input_tree->getResolution());
//ROS_INFO("Size %f", size);
double x = it.getX();
double y = it.getY();
double z = it.getZ();
//std::cerr<< "xyz:" << x << "," << y << "," << z << " hash:" << hash << std::endl;
occupied++;
Vec3 point(x, y, z);
if (frustum.is_inside(point))
{
//ROS_INFO("Node inside frustum");
int node_value = WEIGHT * size;
const OcTreeKey key = it.getKey();
OcTreeKey::KeyHash computeHash;
unsigned short int hash = computeHash(key);
keys.push_back(hash);
node_values.push_back(node_value);
value += node_value;
}
}
else
{
free++;
}
}
//std::cerr<<"occupied "<< occupied<< std::endl;
//std::cerr<<"free "<< free<< std::endl;
return value;
}
int main(int argc, char** argv) {
//##############################################################
string btFilename = "";
unsigned char maxDepth = 16;
// test timing:
timeval start;
timeval stop;
const unsigned char tree_depth(16);
const unsigned int tree_max_val(32768);
double time_it, time_depr;
if (argc <= 1|| argc >3 || strcmp(argv[1], "-h") == 0){
printUsage(argv[0]);
}
btFilename = std::string(argv[1]);
if (argc > 2){
maxDepth = (unsigned char)atoi(argv[2]);
}
maxDepth = std::min((unsigned char)16,maxDepth);
if (maxDepth== 0)
maxDepth = tree_depth;
// iterate over empty tree:
OcTree emptyTree(0.2);
EXPECT_EQ(emptyTree.size(), 0);
EXPECT_EQ(emptyTree.calcNumNodes(), 0);
size_t iteratedNodes = 0;
OcTree::tree_iterator t_it = emptyTree.begin_tree(maxDepth);
OcTree::tree_iterator t_end = emptyTree.end_tree();
EXPECT_TRUE (t_it == t_end);
for( ; t_it != t_end; ++t_it){
iteratedNodes++;
}
EXPECT_EQ(iteratedNodes, 0);
for(OcTree::leaf_iterator l_it = emptyTree.begin_leafs(maxDepth), l_end=emptyTree.end_leafs(); l_it!= l_end; ++l_it){
iteratedNodes++;
}
EXPECT_EQ(iteratedNodes, 0);
cout << "\nReading OcTree file\n===========================\n";
OcTree* tree = new OcTree(btFilename);
if (tree->size()<= 1){
std::cout << "Error reading file, exiting!\n";
return 1;
}
size_t count;
std::list<OcTreeVolume> list_depr;
std::list<OcTreeVolume> list_iterator;
/**
* get number of nodes:
*/
gettimeofday(&start, NULL); // start timer
size_t num_leafs_recurs = tree->getNumLeafNodes();
gettimeofday(&stop, NULL); // stop timer
time_depr = timediff(start, stop);
gettimeofday(&start, NULL); // start timer
size_t num_leafs_it = 0;
for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
num_leafs_it++;
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
std::cout << "Number of leafs: " << num_leafs_it << " / " << num_leafs_recurs << ", times: "
<<time_it << " / " << time_depr << "\n========================\n\n";
/**
* get all occupied leafs
*/
point3d tree_center;
tree_center(0) = tree_center(1) = tree_center(2)
= (float) (((double) tree_max_val) * tree->getResolution());
gettimeofday(&start, NULL); // start timer
getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, true);
gettimeofday(&stop, NULL); // stop timer
time_depr = timediff(start, stop);
gettimeofday(&start, NULL); // start timer
for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){
if(tree->isNodeOccupied(*it))
{
//count ++;
list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
}
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
std::cout << "Occupied lists traversed, times: "
<<time_it << " / " << time_depr << "\n";
compareResults(list_iterator, list_depr);
std::cout << "========================\n\n";
/**
* get all free leafs
*/
list_iterator.clear();
list_depr.clear();
gettimeofday(&start, NULL); // start timer
for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) {
if(!tree->isNodeOccupied(*it))
list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
gettimeofday(&start, NULL); // start timer
getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, false);
gettimeofday(&stop, NULL); // stop timer
time_depr = timediff(start, stop);
std::cout << "Free lists traversed, times: "
<<time_it << " / " << time_depr << "\n";
compareResults(list_iterator, list_depr);
std::cout << "========================\n\n";
/**
* get all volumes
*/
list_iterator.clear();
list_depr.clear();
gettimeofday(&start, NULL); // start timer
getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree->getResolution());
gettimeofday(&stop, NULL); // stop timer
time_depr = timediff(start, stop);
gettimeofday(&start, NULL); // start timers
for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree();
it!= end; ++it){
//count ++;
//std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
list_iterator.sort(OcTreeVolumeSortPredicate);
list_depr.sort(OcTreeVolumeSortPredicate);
std::cout << "All inner lists traversed, times: "
<<time_it << " / " << time_depr << "\n";
compareResults(list_iterator, list_depr);
std::cout << "========================\n\n";
// traverse all leaf nodes, timing:
gettimeofday(&start, NULL); // start timers
count = 0;
for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end();
it!= end; ++it){
// do something:
// std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
count++;
}
gettimeofday(&stop, NULL); // stop timer
time_it = timediff(start, stop);
std::cout << "Time to traverse all leafs at max depth " <<(unsigned int)maxDepth <<" ("<<count<<" nodes): "<< time_it << " s\n\n";
/**
* bounding box tests
*/
//tree->expand();
// test complete tree (should be equal to no bbx)
OcTreeKey bbxMinKey, bbxMaxKey;
double temp_x,temp_y,temp_z;
tree->getMetricMin(temp_x,temp_y,temp_z);
octomap::point3d bbxMin(temp_x,temp_y,temp_z);
tree->getMetricMax(temp_x,temp_y,temp_z);
octomap::point3d bbxMax(temp_x,temp_y,temp_z);
EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
OcTree::leaf_bbx_iterator it_bbx = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey);
EXPECT_TRUE(it_bbx == tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey));
OcTree::leaf_bbx_iterator end_bbx = tree->end_leafs_bbx();
EXPECT_TRUE(end_bbx == tree->end_leafs_bbx());
OcTree::leaf_iterator it = tree->begin_leafs();
EXPECT_TRUE(it == tree->begin_leafs());
OcTree::leaf_iterator end = tree->end_leafs();
EXPECT_TRUE(end == tree->end_leafs());
for( ; it!= end && it_bbx != end_bbx; ++it, ++it_bbx){
EXPECT_TRUE(it == it_bbx);
}
EXPECT_TRUE(it == end && it_bbx == end_bbx);
// now test an actual bounding box:
tree->expand(); // (currently only works properly for expanded tree (no multires)
bbxMin = point3d(-1, -1, - 1);
bbxMax = point3d(3, 2, 1);
EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
typedef unordered_ns::unordered_map<OcTreeKey, double, OcTreeKey::KeyHash> KeyVolumeMap;
KeyVolumeMap bbxVoxels;
count = 0;
for(OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey), end=tree->end_leafs_bbx();
it!= end; ++it)
{
count++;
OcTreeKey currentKey = it.getKey();
// leaf is actually a leaf:
EXPECT_FALSE(it->hasChildren());
// leaf exists in tree:
OcTreeNode* node = tree->search(currentKey);
EXPECT_TRUE(node);
EXPECT_EQ(node, &(*it));
// all leafs are actually in the bbx:
for (unsigned i = 0; i < 3; ++i){
// if (!(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i])){
// std::cout << "Key failed: " << i << " " << currentKey[i] << " "<< bbxMinKey[i] << " "<< bbxMaxKey[i]
// << "size: "<< it.getSize()<< std::endl;
// }
EXPECT_TRUE(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i]);
}
bbxVoxels.insert(std::pair<OcTreeKey,double>(currentKey, it.getSize()));
}
EXPECT_EQ(bbxVoxels.size(), count);
std::cout << "Bounding box traversed ("<< count << " leaf nodes)\n\n";
// compare with manual BBX check on all leafs:
for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
OcTreeKey key = it.getKey();
if ( key[0] >= bbxMinKey[0] && key[0] <= bbxMaxKey[0]
&& key[1] >= bbxMinKey[1] && key[1] <= bbxMaxKey[1]
&& key[2] >= bbxMinKey[2] && key[2] <= bbxMaxKey[2])
{
KeyVolumeMap::iterator bbxIt = bbxVoxels.find(key);
EXPECT_FALSE(bbxIt == bbxVoxels.end());
EXPECT_TRUE(key == bbxIt->first);
EXPECT_EQ(it.getSize(), bbxIt->second);
}
}
// test tree with one node:
OcTree simpleTree(0.01);
simpleTree.updateNode(point3d(10, 10, 10), 5.0f);
for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) {
std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
}
std::cout << "Tests successful\n";
return 0;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
// Usage:
// Constructors/Destructor:
// octree = octomapWrapper(resolution); // constructor: new tree with
// specified resolution
// octree = octomapWrapper(filename); // constructor: load from file
// octomapWrapper(octree); // destructor
//
// Queries:
// results = octomapWrapper(octree, 1, pts) // search
// leaf_nodes = octomapWrapper(octree, 2) // getLeafNodes
//
// Update tree:
// octomapWrapper(octree, 11, pts, occupied) // updateNote(pts, occupied).
// pts is 3-by-n, occupied is 1-by-n logical
//
// General operations:
// octomapWrapper(octree, 21, filename) // save to file
OcTree* tree = NULL;
if (nrhs == 1) {
if (mxIsNumeric(prhs[0])) { // constructor w/ resolution
if (nlhs > 0) {
double resolution = mxGetScalar(prhs[0]);
// mexPrintf("Creating octree w/ resolution %f\n", resolution);
tree = new OcTree(resolution);
plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
}
} else if (mxIsChar(prhs[0])) {
if (nlhs > 0) {
char* filename = mxArrayToString(prhs[0]);
// mexPrintf("Loading octree from %s\n", filename);
tree = new OcTree(filename);
plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
mxFree(filename);
}
} else { // destructor. note: assumes prhs[0] is a DrakeMexPointer (todo:
// could check)
// mexPrintf("Deleting octree\n");
destroyDrakeMexPointer<OcTree*>(prhs[0]);
}
return;
}
tree = (OcTree*)getDrakeMexPointer(prhs[0]);
int COMMAND = (int)mxGetScalar(prhs[1]);
switch (COMMAND) {
case 1: // search
{
mexPrintf("octree search\n");
if (mxGetM(prhs[2]) != 3)
mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
int n = mxGetN(prhs[2]);
double* pts = mxGetPrSafe(prhs[2]);
if (nlhs > 0) {
plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
double* presults = mxGetPrSafe(plhs[0]);
for (int i = 0; i < n; i++) {
OcTreeNode* result =
tree->search(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2]);
if (result == NULL)
presults[i] = -1.0;
else
presults[i] = result->getOccupancy();
}
}
} break;
case 2: // get leaf nodes
{
// mexPrintf("octree get leaf nodes\n");
int N = tree->getNumLeafNodes();
plhs[0] = mxCreateDoubleMatrix(3, N, mxREAL);
double* leaf_xyz = mxGetPrSafe(plhs[0]);
double* leaf_value = NULL, * leaf_size = NULL;
if (nlhs > 1) { // return value
plhs[1] = mxCreateDoubleMatrix(1, N, mxREAL);
leaf_value = mxGetPrSafe(plhs[1]);
}
if (nlhs > 2) { // return size
plhs[2] = mxCreateDoubleMatrix(1, N, mxREAL);
leaf_size = mxGetPrSafe(plhs[2]);
}
for (OcTree::leaf_iterator leaf = tree->begin_leafs(),
end = tree->end_leafs();
leaf != end; ++leaf) {
leaf_xyz[0] = leaf.getX();
leaf_xyz[1] = leaf.getY();
leaf_xyz[2] = leaf.getZ();
leaf_xyz += 3;
if (leaf_value) *leaf_value++ = leaf->getValue();
if (leaf_size) *leaf_size++ = leaf.getSize();
}
} break;
case 11: // add occupied pts
{
// mexPrintf("octree updateNode\n");
if (mxGetM(prhs[2]) != 3)
mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
int n = mxGetN(prhs[2]);
double* pts = mxGetPrSafe(prhs[2]);
mxLogical* occupied = mxGetLogicals(prhs[3]);
for (int i = 0; i < n; i++) {
tree->updateNode(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2],
occupied[i]);
}
} break;
case 12: // insert a scan of endpoints and sensor origin
{
// pointsA should be 3xN, originA is 3x1
double* points = mxGetPrSafe(prhs[2]);
double* originA = mxGetPrSafe(prhs[3]);
int n = mxGetN(prhs[2]);
point3d origin((float)originA[0], (float)originA[1], (float)originA[2]);
Pointcloud pointCloud;
for (int i = 0; i < n; i++) {
point3d point((float)points[3 * i], (float)points[3 * i + 1],
(float)points[3 * i + 2]);
pointCloud.push_back(point);
}
tree->insertPointCloud(pointCloud, origin);
} break;
case 21: // save to file
{
char* filename = mxArrayToString(prhs[2]);
// mexPrintf("writing octree to %s\n", filename);
tree->writeBinary(filename);
mxFree(filename);
} break;
default:
mexErrMsgTxt("octomapWrapper: Unknown command");
}
}
int main(int argc, char** argv) {
cout << endl;
cout << "generating example map" << endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr static_cld(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dynamic_cld(new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("static.pcd",*static_cld)==-1){
std::cout<<"ERror"<<std::endl;
}
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("chair.pcd",*dynamic_cld)==-1){
std::cout<<"ERror"<<std::endl;
}
OcTree st_tree (0.01); // create empty tree with resolution 0.1
OcTree dy_tree (0.01);
octomap::Pointcloud st_cld,dy_cld;
//OccupancyOcTreeBase<OcTreeDataNode<float> > st_occ(0.01);
// insert some measurements of occupied cells
/*
for (int x=-40; x<80; x++) {
for (int y=-10; y<20; y++) {
for (int z=-30; z<20; z++) {
point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
tree.updateNode(endpoint, true); // integrate 'occupied' measurement
}
}
}
// insert some measurements of free cells
for (int x=-30; x<30; x++) {
for (int y=-30; y<30; y++) {
for (int z=-30; z<30; z++) {
point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
tree.updateNode(endpoint, false); // integrate 'free' measurement
}
}
}
*/
for(int i = 0;i<static_cld->size();i++){
// cout<<static_cld->points[i]<<endl;
point3d endpoint((float) static_cld->points[i].x,(float) static_cld->points[i].y,(float) static_cld->points[i].z);
st_cld.push_back(endpoint);
//st_tree.updateNode(endpoint,true);
}
for(int i = 0;i<dynamic_cld->size();i++){
// cout<<static_cld->points[i]<<endl;
point3d endpoint((float) dynamic_cld->points[i].x,(float) dynamic_cld->points[i].y,(float) dynamic_cld->points[i].z);
dy_cld.push_back(endpoint);
//dy_tree.updateNode(endpoint,true);
}
point3d origin(0.0,0.0,0.0);
st_tree.insertPointCloud(st_cld,origin);
st_tree.updateInnerOccupancy();
//st_occ.insertPointCloud(st_cld,origin);
for(OcTree::leaf_iterator it = st_tree.begin_leafs(),
end=st_tree.end_leafs(); it!= end; ++it)
{
//manipulate node, e.g.:
std::cout << "Node center: " << it.getCoordinate() << std::endl;
std::cout << "Node size: " << it.getSize() << std::endl;
std::cout << "Node value: " << it->getValue() << std::endl;
//v=v+(pow(it.getSize(),3));
}
//st_tree.computeUpdate(dy_cld,origin);
//dy_tree.insertPointCloud(dy_cld,origin);
/*
for(leaf_iterator it = st_tree->begin_leafs(),end = st_tree->end_leafs();it!=end;++it ){
std::cout << "Node center: " << it.getCoordinate() << std::endl;
std::cout << "Node size: " << it.getSize() << std::endl;
std::cout << "Node value: " << it->getValue() << std::endl;
}
*/
/*
point3d origin(0.0,0.0,0.0);
//tree.insertPointCloud(static_cld,origin);
cout << endl;
cout << "performing some queries:" << endl;
point3d query (0., 0., 0.);
OcTreeNode* result = tree.search (query);
print_query_info(query, result);
query = point3d(-1.,-1.,-1.);
result = tree.search (query);
print_query_info(query, result);
query = point3d(1.,1.,1.);
result = tree.search (query);
print_query_info(query, result);
cout << endl;
*/
st_tree.writeBinary("static_occ.bt");
// dy_tree.writeBinary("dynamic_tree.bt");
cout << "wrote example file simple_tree.bt" << endl << endl;
cout << "now you can use octovis to visualize: octovis simple_tree.bt" << endl;
cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl << endl;
}
