Go to the documentation of this file.00001
00002
00003
00004
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
00025 }
00026
00027
00028
00029
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
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
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
00117
00118
00119
00120
00121
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
00127 createChild(i);
00128 getChild(i)->setLogOdds(CLAMPING_THRES_MIN);
00129 }
00130 else if ((child1to4[i*2] == 0) && (child1to4[i*2+1] == 1)) {
00131
00132 createChild(i);
00133 getChild(i)->setLogOdds(CLAMPING_THRES_MAX);
00134 }
00135 else if ((child1to4[i*2] == 1) && (child1to4[i*2+1] == 1)) {
00136
00137 createChild(i);
00138 getChild(i)->setLogOdds(-200.);
00139 }
00140 }
00141 for (unsigned int i=0; i<4; i++) {
00142 if ((child5to8[i*2] == 1) && (child5to8[i*2+1] == 0)) {
00143
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
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
00154 createChild(i+4);
00155 getChild(i+4)->setLogOdds(-200.);
00156 }
00157
00158 }
00159
00160
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 }
00169 }
00170
00171 return s;
00172 }
00173
00174 std::ostream& OcTreeNodePCL::writeBinary(std::ostream &s) const{
00175
00176
00177 std::bitset<8> child1to4;
00178 std::bitset<8> child5to8;
00179
00180
00181
00182
00183
00184
00185
00186
00187
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
00213
00214
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
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 }