OcTreePCL.cpp
Go to the documentation of this file.
00001 /*
00002  * Based on the original code by Kai M. Wurm and Armin Hornung
00003  * (http://octomap.sourceforge.net)
00004  * Author: Hozefa Indorewala
00005  */
00006 
00007 #include <cassert>
00008 #include <fstream>
00009 #include <stdlib.h>
00010 
00011 #include "pcl_to_octree/octree/OcTreePCL.h"
00012 #include "octomap/CountingOcTree.h"
00013 
00014 
00015 // switch to true to disable uniform sampling of scans (will "eat" the floor)
00016 #define NO_UNIFORM_SAMPLING false
00017 
00018 
00019 namespace octomap {
00020 
00021 OcTreePCL::OcTreePCL(double _resolution)
00022   : OccupancyOcTreeBase<OcTreeNodePCL> (_resolution)  
00023 {
00024   itsRoot = new OcTreeNodePCL();
00025   tree_size++;
00026 }
00027 
00028 
00029 void OcTreePCL::insertScan(const ScanNode& scan, double maxrange, bool pruning) {
00030   if (scan.scan->size()< 1)
00031     return;
00032 
00033   if (NO_UNIFORM_SAMPLING){
00034     std::cerr << "Warning: Uniform sampling of scan is disabled!\n";
00035 
00036     pose6d scan_pose (scan.pose);
00037 
00038     // integrate beams
00039     octomap::point3d origin (scan_pose.x(), scan_pose.y(), scan_pose.z());
00040     octomap::point3d p;
00041 
00042     for (octomap::Pointcloud::iterator point_it = scan.scan->begin(); 
00043          point_it != scan.scan->end(); point_it++) {
00044       p = scan_pose.transform(*point_it);
00045       this->insertRay(origin, p, maxrange);
00046     } // end for all points
00047   } 
00048   else {
00049     this->insertScanUniform(scan, maxrange);
00050   }
00051 
00052   if (pruning)
00053     this->prune();
00054 }
00055 
00056 void OcTreePCL::toMaxLikelihood() {
00057 
00058   // convert bottom up
00059   for (unsigned int depth=tree_depth; depth>0; depth--) {
00060     toMaxLikelihoodRecurs(this->itsRoot, 0, depth);
00061   }
00062 
00063   // convert root
00064   itsRoot->toMaxLikelihood();
00065 }
00066   
00067   
00068 // -- Information  ---------------------------------  
00069 
00070 unsigned int OcTreePCL::memoryUsage() const{
00071   unsigned int node_size = sizeof(OcTreeNodePCL);
00072   std::list<OcTreeVolume> leafs;
00073   this->getLeafNodes(leafs);
00074   unsigned int inner_nodes = tree_size - leafs.size();
00075   return node_size * tree_size + inner_nodes * sizeof(OcTreeNodePCL*[8]);
00076 }
00077 
00078   
00079 void OcTreePCL::calcNumThresholdedNodes(unsigned int& num_thresholded, 
00080                                         unsigned int& num_other) const {
00081   num_thresholded = 0;
00082   num_other = 0;
00083   calcNumThresholdedNodesRecurs(itsRoot, num_thresholded, num_other);
00084 }
00085 
00086 
00087 void OcTreePCL::readBinary(const std::string& filename){
00088   std::ifstream binary_infile( filename.c_str(), std::ios_base::binary);
00089   if (!binary_infile.is_open()){
00090     std::cerr << "ERROR: Filestream to "<< filename << " not open, nothing read.\n";
00091     return;
00092   } else {
00093     readBinary(binary_infile);
00094     binary_infile.close();
00095   }
00096 }
00097 
00098 
00099 // -- I/O  -----------------------------------------
00100 
00101 std::istream& OcTreePCL::readBinary(std::istream &s) {
00102     
00103   if (!s.good()){
00104     std::cerr << "Warning: Input filestream not \"good\" in OcTree::readBinary\n";
00105   }
00106 
00107   int tree_type = -1;
00108   s.read((char*)&tree_type, sizeof(tree_type));
00109   if (tree_type == OcTreePCL::TREETYPE){
00110 
00111     this->tree_size = 0;
00112     sizeChanged = true;
00113 
00114     // clear tree if there are nodes
00115     if (itsRoot->hasChildren()) {
00116       delete itsRoot;
00117       itsRoot = new OcTreeNodePCL();
00118     }
00119 
00120     double tree_resolution;
00121     s.read((char*)&tree_resolution, sizeof(tree_resolution));
00122 
00123     this->setResolution(tree_resolution);
00124 
00125     unsigned int tree_read_size = 0;
00126     s.read((char*)&tree_read_size, sizeof(tree_read_size));
00127     std::cout << "Reading "
00128               << tree_read_size
00129               << " nodes from bonsai tree file..." <<std::flush;
00130 
00131     itsRoot->readBinary(s);
00132 
00133     tree_size = calcNumNodes();  // compute number of nodes
00134 
00135     std::cout << " done.\n";
00136   } else if (tree_type == OcTreePCL::TREETYPE+1){
00137     this->read(s);
00138   } else{
00139     std::cerr << "Binary file does not contain an OcTree!\n";
00140   }
00141 
00142   return s;
00143 }
00144 
00145 
00146 void OcTreePCL::writeBinary(const std::string& filename){
00147   std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
00148 
00149   if (!binary_outfile.is_open()){
00150     std::cerr << "ERROR: Filestream to "<< filename << " not open, nothing written.\n";
00151     return;
00152   } else {
00153     writeBinary(binary_outfile);
00154     binary_outfile.close();
00155   }
00156 }
00157 
00158 void OcTreePCL::writeBinaryConst(const std::string& filename) const{
00159   std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
00160 
00161   if (!binary_outfile.is_open()){
00162     std::cerr << "ERROR: Filestream to "<< filename << " not open, nothing written.\n";
00163     return;
00164   } 
00165   else {
00166     writeBinaryConst(binary_outfile);
00167     binary_outfile.close();
00168   }
00169 }
00170 
00171 std::ostream& OcTreePCL::writeBinary(std::ostream &s){
00172 
00173   // format:    treetype | resolution | num nodes | [binary nodes]
00174 
00175   this->toMaxLikelihood();
00176   this->prune();
00177 
00178   return writeBinaryConst(s);
00179 }
00180 
00181 std::ostream& OcTreePCL::writeBinaryConst(std::ostream &s) const{
00182 
00183   // format:    treetype | resolution | num nodes | [binary nodes]
00184 
00185   unsigned int tree_type = OcTreePCL::TREETYPE;
00186   s.write((char*)&tree_type, sizeof(tree_type));
00187 
00188   double tree_resolution = resolution;
00189   s.write((char*)&tree_resolution, sizeof(tree_resolution));
00190 
00191   unsigned int tree_write_size = this->size(); 
00192   fprintf(stderr, "writing %d nodes to output stream...", tree_write_size); fflush(stderr);
00193   s.write((char*)&tree_write_size, sizeof(tree_write_size));
00194 
00195   itsRoot->writeBinary(s);
00196 
00197   fprintf(stderr, " done.\n");
00198 
00199   return s;
00200 }
00201 
00202 
00203 // --  protected  --------------------------------------------
00204 
00205 void OcTreePCL::insertScanUniform(const ScanNode& scan, double maxrange) {
00206     
00207   octomap::pose6d  scan_pose (scan.pose);
00208   octomap::point3d origin (scan_pose.x(), scan_pose.y(), scan_pose.z());
00209 
00210 
00211   // preprocess data  --------------------------
00212 
00213   octomap::point3d p;
00214 
00215   CountingOcTree free_tree    (this->getResolution());
00216   CountingOcTree occupied_tree(this->getResolution());
00217   octomap::KeyRay ray;
00218 
00219   for (octomap::Pointcloud::iterator point_it = scan.scan->begin(); point_it != scan.scan->end(); point_it++) {
00220 
00221     p = scan_pose.transform(*point_it);
00222 
00223     bool is_maxrange = false;
00224     if ( (maxrange > 0.0) && ((p - origin).norm2() > maxrange) ) is_maxrange = true;
00225 
00226     if (!is_maxrange) {
00227       // free cells
00228       if (this->computeRayKeys(origin, p, ray)){
00229         for(octomap::KeyRay::iterator it=ray.begin(); it != ray.end(); it++) {
00230           free_tree.updateNode(*it);
00231         }
00232       }
00233       // occupied cells
00234       occupied_tree.updateNode(p);
00235     } // end if NOT maxrange
00236 
00237     else {
00238       point3d direction = (p - origin).unit();
00239       point3d new_end = origin + direction * maxrange;
00240       if (this->computeRayKeys(origin, new_end, ray)){
00241         for(octomap::KeyRay::iterator it=ray.begin(); it != ray.end(); it++) {
00242           free_tree.updateNode(*it);
00243         }
00244       }
00245     } // end if maxrange
00246 
00247 
00248   } // end for all points
00249 
00250   std::list<OcTreeVolume> free_cells;
00251   free_tree.getLeafNodes(free_cells);
00252 
00253   std::list<OcTreeVolume> occupied_cells;
00254   occupied_tree.getLeafNodes(occupied_cells);
00255 
00256 
00257   // delete free cells if cell is also measured occupied
00258   for (std::list<OcTreeVolume>::iterator cellit = free_cells.begin(); cellit != free_cells.end();){
00259     if ( occupied_tree.search(cellit->first) ) {
00260       cellit = free_cells.erase(cellit);
00261     }
00262     else {
00263       cellit++;
00264     }
00265   } // end for
00266 
00267 
00268     // insert data into tree  -----------------------
00269   for (std::list<OcTreeVolume>::iterator it = free_cells.begin(); it != free_cells.end(); it++) {
00270     updateNode(it->first, false);
00271   }
00272   for (std::list<OcTreeVolume>::iterator it = occupied_cells.begin(); it != occupied_cells.end(); it++) {
00273     updateNode(it->first, true);
00274   }
00275 
00276   //    unsigned int num_thres = 0;
00277   //    unsigned int num_other = 0;
00278   //    calcNumThresholdedNodes(num_thres, num_other);
00279   //    std::cout << "Inserted scan, total num of thresholded nodes: "<< num_thres << ", num of other nodes: "<< num_other << std::endl;
00280 
00281 }
00282   
00283 void OcTreePCL::toMaxLikelihoodRecurs(OcTreeNodePCL* node, unsigned int depth,
00284                                       unsigned int max_depth) {
00285 
00286   if (depth < max_depth) {
00287     for (unsigned int i=0; i<8; i++) {
00288       if (node->childExists(i)) {
00289         toMaxLikelihoodRecurs(node->getChild(i), depth+1, max_depth);
00290       }
00291     }
00292   }
00293 
00294   else { // max level reached
00295     node->toMaxLikelihood();
00296   }
00297 }
00298   
00299 void OcTreePCL::calcNumThresholdedNodesRecurs (OcTreeNodePCL* node,
00300                                                unsigned int& num_thresholded, 
00301                                                unsigned int& num_other) const { 
00302   assert(node != NULL);
00303 
00304   for (unsigned int i=0; i<8; i++) {
00305     if (node->childExists(i)) {
00306       OcTreeNodePCL* child_node = node->getChild(i);
00307       if (child_node->atThreshold()) num_thresholded++;
00308       else num_other++;
00309       calcNumThresholdedNodesRecurs(child_node, num_thresholded, num_other);
00310     } // end if child
00311   } // end for children
00312 }
00313 
00314 
00315 void OcTreePCL::insertOcTreeNodePCL(octomap::OcTreeNodePCL * ocn)
00316 {
00317   this->octree_node_list.push_back(ocn);
00318 }
00319 
00320 octomap::OcTreeNodePCL * OcTreePCL::getOcTreeNodePCL(point3d c) const
00321 {
00322   
00323   for (int i = 0; i < this->octree_node_list.size(); i++)
00324   {
00325 //     std::cerr << "cx: " << this->octree_node_list[i]->getCentroid().x() << " " << c.x() << std::endl; 
00326 //     std::cerr << "cy: " << this->octree_node_list[i]->getCentroid().y() << " " << c.y() << std::endl; 
00327 //     std::cerr << "cz: " << this->octree_node_list[i]->getCentroid().z() << " " << c.z() << std::endl; 
00328     if (this->octree_node_list[i]->getCentroid().x() == c.x() &&  
00329         this->octree_node_list[i]->getCentroid().y() == c.y() &&
00330         this->octree_node_list[i]->getCentroid().z() == c.z())
00331       return this->octree_node_list[i];
00332   }
00333   return NULL;
00334 }
00335 
00336 } // namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_to_octree
Author(s): Hozefa Indorewala, Dejan Pangercic
autogenerated on Thu May 23 2013 08:28:50