int main(int argc, char** argv) {
if (argc != 3)
printUsage(argv[0]);
string inputFilename = argv[1];
string outputFilename = argv[2];
// pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud( new pcl::PointCloud<pcl::PointXYZ>() );
pcl::io::loadPCDFile( inputFilename, *pclcloud );
// data conversion
Pointcloud * cloud = new Pointcloud;
for ( size_t i = 0; i < pclcloud->size(); ++ i ) {
point3d pt(pclcloud->points[i].x, pclcloud->points[i].y, pclcloud->points[i].z);
cloud->push_back( pt );
}
point3d sensor_origin(0,0,0);
OcTree* tree = new OcTree(0.1);
tree->insertPointCloud( cloud, sensor_origin );
tree->writeBinary(outputFilename);
}
int main(int argc, char** argv) {
//##############################################################
OcTree tree (0.05);
tree.enableChangeDetection(true);
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (4.01f,0.01f,0.01f);
tree.insertRay(origin, point_on_surface);
printChanges(tree);
tree.updateNode(point3d(2.01f, 0.01f, 0.01f), 2.0f);
printChanges(tree);
tree.updateNode(point3d(2.01f, 0.01f, 0.01f), -2.0f);
printChanges(tree);
cout << "generating spherical scan at " << origin << " ..." << endl;
for (int i=-100; i<101; i++) {
Pointcloud cloud;
for (int j=-100; j<101; j++) {
point3d rotated = point_on_surface;
rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
cloud.push_back(rotated);
}
// insert in global coordinates:
tree.insertPointCloud(cloud, origin, -1);
}
printChanges(tree);
cout << "done." << endl;
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) {
// default values:
double res = 0.1;
if (argc < 2)
printUsage(argv[0]);
string graphFilename = std::string(argv[1]);
double maxrange = -1;
int max_scan_no = -1;
int skip_scan_eval = 5;
int arg = 1;
while (++arg < argc) {
if (! strcmp(argv[arg], "-i"))
graphFilename = std::string(argv[++arg]);
else if (! strcmp(argv[arg], "-res"))
res = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-m"))
maxrange = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-n"))
max_scan_no = atoi(argv[++arg]);
else {
printUsage(argv[0]);
}
}
cout << "\nReading Graph file\n===========================\n";
ScanGraph* graph = new ScanGraph();
if (!graph->readBinary(graphFilename))
exit(2);
size_t num_points_in_graph = 0;
if (max_scan_no > 0) {
num_points_in_graph = graph->getNumPoints(max_scan_no-1);
cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
}
else {
num_points_in_graph = graph->getNumPoints();
cout << "\n Data points in graph: " << num_points_in_graph << endl;
}
cout << "\nCreating tree\n===========================\n";
OcTree* tree = new OcTree(res);
size_t numScans = graph->size();
unsigned int currentScan = 1;
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
if (currentScan % skip_scan_eval != 0){
if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
else cout << "("<<currentScan << "/" << numScans << ") " << flush;
tree->insertPointCloud(**scan_it, maxrange);
} else
cout << "(SKIP) " << flush;
if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
break;
currentScan++;
}
tree->expand();
cout << "\nEvaluating scans\n===========================\n";
currentScan = 1;
size_t num_points = 0;
size_t num_voxels_correct = 0;
size_t num_voxels_wrong = 0;
size_t num_voxels_unknown = 0;
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
if (currentScan % skip_scan_eval == 0){
if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
else cout << "("<<currentScan << "/" << numScans << ") " << flush;
pose6d frame_origin = (*scan_it)->pose;
point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
// transform pointcloud:
Pointcloud scan (*(*scan_it)->scan);
scan.transform(frame_origin);
point3d origin = frame_origin.transform(sensor_origin);
KeySet free_cells, occupied_cells;
tree->computeUpdate(scan, origin, free_cells, occupied_cells, maxrange);
num_points += scan.size();
// count free cells
for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
OcTreeNode* n = tree->search(*it);
if (n){
if (tree->isNodeOccupied(n))
num_voxels_wrong++;
else
num_voxels_correct++;
} else
num_voxels_unknown++;
} // count occupied cells
for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
OcTreeNode* n = tree->search(*it);
if (n){
if (tree->isNodeOccupied(n))
num_voxels_correct++;
else
num_voxels_wrong++;
} else
num_voxels_unknown++;
}
}
if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
break;
currentScan++;
}
cout << "\nFinished evaluating " << num_points <<"/"<< num_points_in_graph << " points.\n"
<<"Voxels correct: "<<num_voxels_correct<<" #wrong: " <<num_voxels_wrong << " #unknown: " <<num_voxels_unknown
<<". % correct: "<< num_voxels_correct/double(num_voxels_correct+num_voxels_wrong)<<"\n\n";
delete graph;
delete tree;
return 0;
}
int main(int argc, char** argv) {
if (argc != 2){
std::cerr << "Error: you need to specify a test as argument" << std::endl;
return 1; // exit 1 means failure
}
std::string test_name (argv[1]);
// ------------------------------------------------------------
if (test_name == "MathVector") {
// test constructors
Vector3* twos = new Vector3();
Vector3* ones = new Vector3(1,1,1);
for (int i=0;i<3;i++) {
(*twos)(i) = 2;
}
// test basic operations
Vector3 subtraction = *twos - *ones;
Vector3 addition = *twos + *ones;
Vector3 multiplication = *twos * 2.;
for (int i=0;i<3;i++) {
EXPECT_FLOAT_EQ (subtraction(i), 1.);
EXPECT_FLOAT_EQ (addition(i), 3.);
EXPECT_FLOAT_EQ (multiplication(i), 4.);
}
// copy constructor
Vector3 rotation = *ones;
// rotation
rotation.rotate_IP (M_PI, 1., 0.1);
EXPECT_FLOAT_EQ (rotation.x(), 1.2750367);
EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513));
EXPECT_FLOAT_EQ (rotation.z(), 0.30116868);
// ------------------------------------------------------------
} else if (test_name == "MathPose") {
// constructors
Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. );
Pose6D b;
Vector3 trans(1.0f, 0.1f, 0.1f);
Quaternion rot(0.0f, 0.1f, (float) M_PI/4.);
Pose6D c(trans, rot);
// comparator
EXPECT_TRUE ( a == c);
// toEuler
EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.);
// transform
Vector3 t = c.transform (trans);
EXPECT_FLOAT_EQ (t.x() , 1.6399229);
EXPECT_FLOAT_EQ (t.y() , 0.8813442);
EXPECT_FLOAT_EQ (t.z() , 0.099667005);
// inverse transform
Pose6D c_inv = c.inv();
Vector3 t2 = c_inv.transform (t);
EXPECT_FLOAT_EQ (t2.x() , trans.x());
EXPECT_FLOAT_EQ (t2.y() , trans.y());
EXPECT_FLOAT_EQ (t2.z() , trans.z());
// ------------------------------------------------------------
} else if (test_name == "InsertRay") {
double p = 0.5;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
p = 0.1;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
p = 0.99;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
float l = 0;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
l = -4;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
l = 2;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
OcTree tree (0.05);
tree.setProbHit(0.7);
tree.setProbMiss(0.4);
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (2.01f,0.01f,0.01f);
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface));
point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
}
point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
}
EXPECT_TRUE (tree.writeBinary("sphere_rays.bt"));
EXPECT_EQ ((int) tree.size(), 50615);
// ------------------------------------------------------------
// ray casting is now in "test_raycasting.cpp"
// ------------------------------------------------------------
// insert scan test
// insert graph node test
// write graph test
} else if (test_name == "InsertScan") {
Pointcloud* measurement = new Pointcloud();
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (2.01f, 0.01f, 0.01f);
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
point3d p = origin+point_on_surface;
measurement->push_back(p);
point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
}
point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
}
OcTree tree (0.05);
tree.insertPointCloud(*measurement, origin);
EXPECT_EQ (tree.size(), 53959);
ScanGraph* graph = new ScanGraph();
Pose6D node_pose (origin.x(), origin.y(), origin.z(),0.0f,0.0f,0.0f);
graph->addNode(measurement, node_pose);
EXPECT_TRUE (graph->writeBinary("test.graph"));
delete graph;
// ------------------------------------------------------------
// graph read file test
} else if (test_name == "ReadGraph") {
// not really meaningful, see better test in "test_scans.cpp"
ScanGraph graph;
EXPECT_TRUE (graph.readBinary("test.graph"));
// ------------------------------------------------------------
} else if (test_name == "StampedTree") {
OcTreeStamped stamped_tree (0.05);
// fill tree
for (int x=-20; x<20; x++)
for (int y=-20; y<20; y++)
for (int z=-20; z<20; z++) {
point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
stamped_tree.updateNode(p, true); // integrate 'occupied' measurement
}
// test if update times set
point3d query (0.1f, 0.1f, 0.1f);
OcTreeNodeStamped* result = stamped_tree.search (query);
EXPECT_TRUE (result);
unsigned int tree_time = stamped_tree.getLastUpdateTime();
unsigned int node_time = result->getTimestamp();
std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl;
EXPECT_TRUE (tree_time > 0);
#ifdef _WIN32
Sleep(1000);
#else
sleep(1);
#endif
stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update
std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
EXPECT_EQ (node_time, result->getTimestamp()); // node time updated?
point3d query2 = point3d (0.1f, 0.1f, 0.3f);
stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement
OcTreeNodeStamped* result2 = stamped_tree.search (query2);
EXPECT_TRUE (result2);
result = stamped_tree.search (query);
EXPECT_TRUE (result);
std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp()
<< "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl;
EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated
EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime());
// ------------------------------------------------------------
} else if (test_name == "OcTreeKey") {
OcTree tree (0.05);
point3d p(0.0,0.0,0.0);
OcTreeKey key;
tree.coordToKeyChecked(p, key);
point3d p_inv = tree.keyToCoord(key);
EXPECT_FLOAT_EQ (0.025, p_inv.x());
EXPECT_FLOAT_EQ (0.025, p_inv.y());
EXPECT_FLOAT_EQ (0.025, p_inv.z());
// ------------------------------------------------------------
} else {
std::cerr << "Invalid test name specified: " << test_name << std::endl;
return 1;
}
std::cerr << "Test successful.\n";
return 0;
}
int main(int argc, char** argv) {
// default values:
double res = 0.1;
string graphFilename = "";
string treeFilename = "";
double maxrange = -1;
int max_scan_no = -1;
bool detailedLog = false;
bool simpleUpdate = false;
bool discretize = false;
unsigned char compression = 1;
// get default sensor model values:
OcTree emptyTree(0.1);
double clampingMin = emptyTree.getClampingThresMin();
double clampingMax = emptyTree.getClampingThresMax();
double probMiss = emptyTree.getProbMiss();
double probHit = emptyTree.getProbHit();
timeval start;
timeval stop;
int arg = 0;
while (++arg < argc) {
if (! strcmp(argv[arg], "-i"))
graphFilename = std::string(argv[++arg]);
else if (!strcmp(argv[arg], "-o"))
treeFilename = std::string(argv[++arg]);
else if (! strcmp(argv[arg], "-res") && argc-arg < 2)
printUsage(argv[0]);
else if (! strcmp(argv[arg], "-res"))
res = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-log"))
detailedLog = true;
else if (! strcmp(argv[arg], "-simple"))
simpleUpdate = true;
else if (! strcmp(argv[arg], "-discretize"))
discretize = true;
else if (! strcmp(argv[arg], "-compress"))
OCTOMAP_WARNING("Argument -compress no longer has an effect, incremental pruning is done during each insertion.\n");
else if (! strcmp(argv[arg], "-compressML"))
compression = 2;
else if (! strcmp(argv[arg], "-m"))
maxrange = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-n"))
max_scan_no = atoi(argv[++arg]);
else if (! strcmp(argv[arg], "-clamping") && (argc-arg < 3))
printUsage(argv[0]);
else if (! strcmp(argv[arg], "-clamping")){
clampingMin = atof(argv[++arg]);
clampingMax = atof(argv[++arg]);
}
else if (! strcmp(argv[arg], "-sensor") && (argc-arg < 3))
printUsage(argv[0]);
else if (! strcmp(argv[arg], "-sensor")){
probMiss = atof(argv[++arg]);
probHit = atof(argv[++arg]);
}
else {
printUsage(argv[0]);
}
}
if (graphFilename == "" || treeFilename == "")
printUsage(argv[0]);
// verify input:
if (res <= 0.0){
OCTOMAP_ERROR("Resolution must be positive");
exit(1);
}
if (clampingMin >= clampingMax || clampingMin < 0.0 || clampingMax > 1.0){
OCTOMAP_ERROR("Error in clamping values: 0.0 <= [%f] < [%f] <= 1.0\n", clampingMin, clampingMax);
exit(1);
}
if (probMiss >= probHit || probMiss < 0.0 || probHit > 1.0){
OCTOMAP_ERROR("Error in sensor model (hit/miss prob.): 0.0 <= [%f] < [%f] <= 1.0\n", probMiss, probHit);
exit(1);
}
std::string treeFilenameOT = treeFilename + ".ot";
std::string treeFilenameMLOT = treeFilename + "_ml.ot";
cout << "\nReading Graph file\n===========================\n";
ScanGraph* graph = new ScanGraph();
if (!graph->readBinary(graphFilename))
exit(2);
unsigned int num_points_in_graph = 0;
if (max_scan_no > 0) {
num_points_in_graph = graph->getNumPoints(max_scan_no-1);
cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
}
else {
num_points_in_graph = graph->getNumPoints();
cout << "\n Data points in graph: " << num_points_in_graph << endl;
}
// transform pointclouds first, so we can directly operate on them later
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
pose6d frame_origin = (*scan_it)->pose;
point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
(*scan_it)->scan->transform(frame_origin);
point3d transformed_sensor_origin = frame_origin.transform(sensor_origin);
(*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion());
}
std::ofstream logfile;
if (detailedLog){
logfile.open((treeFilename+".log").c_str());
logfile << "# Memory of processing " << graphFilename << " over time\n";
logfile << "# Resolution: "<< res <<"; compression: " << int(compression) << "; scan endpoints: "<< num_points_in_graph << std::endl;
logfile << "# [scan number] [bytes octree] [bytes full 3D grid]\n";
}
cout << "\nCreating tree\n===========================\n";
OcTree* tree = new OcTree(res);
tree->setClampingThresMin(clampingMin);
tree->setClampingThresMax(clampingMax);
tree->setProbHit(probHit);
tree->setProbMiss(probMiss);
gettimeofday(&start, NULL); // start timer
unsigned int numScans = graph->size();
unsigned int currentScan = 1;
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
else cout << "("<<currentScan << "/" << numScans << ") " << flush;
if (simpleUpdate)
tree->insertPointCloudRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange);
else
tree->insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange, false, discretize);
if (compression == 2){
tree->toMaxLikelihood();
tree->prune();
}
if (detailedLog)
logfile << currentScan << " " << tree->memoryUsage() << " " << tree->memoryFullGrid() << "\n";
if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
break;
currentScan++;
}
gettimeofday(&stop, NULL); // stop timer
double time_to_insert = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
// get rid of graph in mem before doing anything fancy with tree (=> memory)
delete graph;
if (logfile.is_open())
logfile.close();
cout << "\nDone building tree.\n\n";
cout << "time to insert scans: " << time_to_insert << " sec" << endl;
cout << "time to insert 100.000 points took: " << time_to_insert/ ((double) num_points_in_graph / 100000) << " sec (avg)" << endl << endl;
std::cout << "Pruned tree (lossless compression)\n" << "===========================\n";
outputStatistics(tree);
tree->write(treeFilenameOT);
std::cout << "Pruned max-likelihood tree (lossy compression)\n" << "===========================\n";
tree->toMaxLikelihood();
tree->prune();
outputStatistics(tree);
cout << "\nWriting tree files\n===========================\n";
tree->write(treeFilenameMLOT);
std::cout << "Full Octree (pruned) written to "<< treeFilenameOT << std::endl;
std::cout << "Full Octree (max.likelihood, pruned) written to "<< treeFilenameMLOT << std::endl;
tree->writeBinary(treeFilename);
std::cout << "Bonsai tree written to "<< treeFilename << std::endl;
cout << endl;
delete tree;
exit(0);
}