OcTreeNodePCL.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 <bitset>
00008 #include <cassert>
00009 
00010 #include "pcl_to_octree/octree/OcTreeNodePCL.h"
00011 
00012 namespace octomap 
00013 {
00014 
00015 
00016 OcTreeNodePCL::OcTreeNodePCL()
00017   : OcTreeNode() 
00018 {
00019   setLabel(-1);
00020 }
00021 
00022 OcTreeNodePCL::~OcTreeNodePCL()
00023 {
00024   // children are deleted in parent destructor
00025 }
00026 
00027 
00028 // ============================================================
00029 // =  children          =======================================
00030 // ============================================================
00031 
00032 bool OcTreeNodePCL::createChild(unsigned int i) 
00033 {
00034   if (itsChildren == NULL) 
00035   {
00036     allocChildren();
00037   }
00038   itsChildren[i] = new OcTreeNodePCL();
00039   return true;
00040 }
00041 
00042 
00043 OcTreeNodePCL* OcTreeNodePCL::getChild(unsigned int i) 
00044 {
00045   assert((i < 8) && (itsChildren != NULL));
00046   assert(itsChildren[i] != NULL);
00047   return (OcTreeNodePCL*) itsChildren[i];
00048 }
00049 
00050 const OcTreeNodePCL* OcTreeNodePCL::getChild(unsigned int i) const
00051 {
00052   assert((i < 8) && (itsChildren != NULL));
00053   assert(itsChildren[i] != NULL);
00054   return (const OcTreeNodePCL*) itsChildren[i];
00055 }
00056 
00057 
00058 // ============================================================
00059 // =  data              =======================================
00060 // ============================================================
00061 
00062 void OcTreeNodePCL::setLabel(int l) 
00063 {
00064   label = l;
00065 }
00066 
00067 int OcTreeNodePCL::getLabel() const
00068 {
00069   return (label);
00070 }
00071 
00072 void OcTreeNodePCL::setCentroid(point3d c) 
00073 {
00074   centroid = c;
00075 }
00076 
00077 point3d OcTreeNodePCL::getCentroid() const
00078 {
00079   return (centroid);
00080 }
00081 
00082 void OcTreeNodePCL::setResolution(double res) 
00083 {
00084   resolution = res;
00085 }
00086 
00087 double OcTreeNodePCL::getResolution() const
00088 {
00089   return (resolution);
00090 }
00091 
00092 void OcTreeNodePCL::set3DPointInliers(int inlier_index) 
00093 {
00094   inliers.push_back(inlier_index);
00095 }
00096 
00097 std::vector<int> OcTreeNodePCL::get3DPointInliers()
00098 {
00099   return (inliers);
00100 }
00101 
00102 // ============================================================
00103   // =  file I/O          =======================================
00104   // ============================================================
00105 
00106   std::istream& OcTreeNodePCL::readBinary(std::istream &s) {
00107 
00108     char child1to4_char;
00109     char child5to8_char;
00110     s.read((char*)&child1to4_char, sizeof(char));
00111     s.read((char*)&child5to8_char, sizeof(char));
00112 
00113     std::bitset<8> child1to4 ((unsigned long) child1to4_char);
00114     std::bitset<8> child5to8 ((unsigned long) child5to8_char);
00115 
00116     //     std::cout << "read:  "
00117     //        << child1to4.to_string<char,std::char_traits<char>,std::allocator<char> >() << " "
00118     //        << child5to8.to_string<char,std::char_traits<char>,std::allocator<char> >() << std::endl;
00119 
00120 
00121     // inner nodes default to occupied
00122     this->setLogOdds(CLAMPING_THRES_MAX);
00123 
00124     for (unsigned int i=0; i<4; i++) {
00125       if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 0)) {
00126         // child is free leaf
00127         createChild(i);
00128         getChild(i)->setLogOdds(CLAMPING_THRES_MIN);
00129       }
00130       else if ((child1to4[i*2] == 0) && (child1to4[i*2+1] == 1)) {
00131         // child is occupied leaf
00132         createChild(i);
00133         getChild(i)->setLogOdds(CLAMPING_THRES_MAX);
00134       }
00135       else if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 1)) {
00136         // child has children
00137         createChild(i);
00138         getChild(i)->setLogOdds(-200.); // child is unkown, we leave it uninitialized
00139       }
00140     }
00141     for (unsigned int i=0; i<4; i++) {
00142       if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 0)) {
00143         // child is free leaf
00144         createChild(i+4);
00145         getChild(i+4)->setLogOdds(CLAMPING_THRES_MIN);
00146       }
00147       else if ((child5to8[i*2] == 0) && (child5to8[i*2+1] == 1)) {
00148         // child is occupied leaf
00149         createChild(i+4);
00150         getChild(i+4)->setLogOdds(CLAMPING_THRES_MAX);
00151       }
00152       else if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 1)) {
00153         // child has children
00154         createChild(i+4);
00155         getChild(i+4)->setLogOdds(-200.); // set occupancy when all children have been read
00156       }
00157       // child is unkown, we leave it uninitialized
00158     }
00159 
00160     // read children's children and set the label
00161     for (unsigned int i=0; i<8; i++) {
00162       if (this->childExists(i)) {
00163         OcTreeNodePCL* child = this->getChild(i);
00164         if (fabs(child->getLogOdds()+200.)<1e-3) {
00165           child->readBinary(s);
00166           child->setLogOdds(child->getMaxChildLogOdds());
00167         }
00168       } // end if child exists
00169     } // end for children
00170 
00171     return s;
00172   }
00173 
00174   std::ostream& OcTreeNodePCL::writeBinary(std::ostream &s) const{
00175 
00176     // 2 bits for each children, 8 children per node -> 16 bits
00177     std::bitset<8> child1to4;
00178     std::bitset<8> child5to8;
00179 
00180     // 10 : child is free node
00181     // 01 : child is occupied node
00182     // 00 : child is unkown node
00183     // 11 : child has children
00184 
00185 
00186     // speedup: only set bits to 1, rest is init with 0 anyway,
00187     //          can be one logic expression per bit
00188 
00189     for (unsigned int i=0; i<4; i++) {
00190       if (childExists(i)) {
00191         const OcTreeNodePCL* child = this->getChild(i);
00192         if      (child->hasChildren())  { child1to4[i*2] = 1; child1to4[i*2+1] = 1; }
00193         else if (child->isOccupied())   { child1to4[i*2] = 0; child1to4[i*2+1] = 1; }
00194         else                            { child1to4[i*2] = 1; child1to4[i*2+1] = 0; }
00195       }
00196       else {
00197         child1to4[i*2] = 0; child1to4[i*2+1] = 0;
00198       }
00199     }
00200 
00201     for (unsigned int i=0; i<4; i++) {
00202       if (childExists(i+4)) {
00203         const OcTreeNodePCL* child = this->getChild(i+4);
00204         if      (child->hasChildren())  { child5to8[i*2] = 1; child5to8[i*2+1] = 1; }
00205         else if (child->isOccupied())   { child5to8[i*2] = 0; child5to8[i*2+1] = 1; }
00206         else                            { child5to8[i*2] = 1; child5to8[i*2+1] = 0; }
00207       }
00208       else {
00209         child5to8[i*2] = 0; child5to8[i*2+1] = 0;
00210       }
00211     }
00212     //     std::cout << "wrote: "
00213     //        << child1to4.to_string<char,std::char_traits<char>,std::allocator<char> >() << " "
00214     //        << child5to8.to_string<char,std::char_traits<char>,std::allocator<char> >() << std::endl;
00215 
00216     char child1to4_char = (char) child1to4.to_ulong();
00217     char child5to8_char = (char) child5to8.to_ulong();
00218 
00219     s.write((char*)&child1to4_char, sizeof(char));
00220     s.write((char*)&child5to8_char, sizeof(char));
00221 
00222     // write children's children
00223     for (unsigned int i=0; i<8; i++) {
00224       if (childExists(i)) {
00225         const OcTreeNodePCL* child = this->getChild(i);
00226         if (child->hasChildren()) {
00227           child->writeBinary(s);
00228         }
00229       }
00230     }
00231 
00232     return s;
00233   }
00234 
00235 
00236 } // end namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_to_octree
Author(s): Hozefa Indorewala, Dejan Pangercic
autogenerated on Sun Oct 6 2013 12:03:28