Pointcloud.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 /* According to c++ standard including this header has no practical effect
00035  * but it can be used to determine the c++ standard library implementation.
00036  */ 
00037 #include <ciso646>
00038 
00039 #if defined(_MSC_VER) || defined(_LIBCPP_VERSION)
00040   #include <algorithm>
00041 #else
00042   #include <ext/algorithm>
00043 #endif
00044 #include <fstream>
00045 #include <math.h>
00046 
00047 #include <octomap/Pointcloud.h>
00048 
00049 namespace octomap {
00050 
00051 
00052   Pointcloud::Pointcloud() {
00053 
00054   }
00055 
00056   Pointcloud::~Pointcloud() {
00057     this->clear();
00058   }
00059 
00060   void Pointcloud::clear() {
00061 
00062     // delete the points
00063     if (points.size()) {
00064       points.clear();
00065     }
00066   }
00067 
00068 
00069   Pointcloud::Pointcloud(const Pointcloud& other) {
00070     for (Pointcloud::const_iterator it = other.begin(); it != other.end(); it++) {
00071       points.push_back(point3d(*it));
00072     }
00073   }
00074 
00075   Pointcloud::Pointcloud(Pointcloud* other) {
00076     for (Pointcloud::const_iterator it = other->begin(); it != other->end(); it++) {
00077       points.push_back(point3d(*it));
00078     }
00079   }
00080 
00081 
00082   void Pointcloud::push_back(const Pointcloud& other)   {
00083     for (Pointcloud::const_iterator it = other.begin(); it != other.end(); it++) {
00084       points.push_back(point3d(*it));
00085     }
00086   }
00087 
00088   point3d Pointcloud::getPoint(unsigned int i) const{
00089     if (i < points.size())
00090       return points[i];
00091     else {
00092       OCTOMAP_WARNING("Pointcloud::getPoint index out of range!\n");
00093       return points.back();
00094     }
00095   }
00096 
00097   void Pointcloud::transform(octomath::Pose6D transform) {
00098 
00099     for (unsigned int i=0; i<points.size(); i++) {
00100       points[i] = transform.transform(points[i]);
00101     }
00102 
00103    // FIXME: not correct for multiple transforms
00104     current_inv_transform = transform.inv();
00105   }
00106 
00107 
00108   void Pointcloud::transformAbsolute(pose6d transform) {
00109 
00110     // undo previous transform, then apply current transform
00111     pose6d transf = current_inv_transform * transform;
00112 
00113     for (unsigned int i=0; i<points.size(); i++) {
00114       points[i] = transf.transform(points[i]);
00115     }
00116 
00117     current_inv_transform = transform.inv();
00118   }
00119 
00120 
00121   void Pointcloud::rotate(double roll, double pitch, double yaw) {
00122 
00123     for (unsigned int i=0; i<points.size(); i++) {
00124       points[i].rotate_IP(roll, pitch, yaw);
00125     }
00126   }
00127 
00128 
00129   void Pointcloud::calcBBX(point3d& lowerBound, point3d& upperBound) const {
00130     float min_x, min_y, min_z;
00131     float max_x, max_y, max_z;
00132     min_x = min_y = min_z = 1e6;
00133     max_x = max_y = max_z = -1e6;
00134 
00135     float x,y,z;
00136 
00137     for (Pointcloud::const_iterator it=begin(); it!=end(); it++) {
00138 
00139       x = (*it)(0);
00140       y = (*it)(1);
00141       z = (*it)(2);
00142 
00143       if (x < min_x) min_x = x;
00144       if (y < min_y) min_y = y;
00145       if (z < min_z) min_z = z;
00146 
00147       if (x > max_x) max_x = x;
00148       if (y > max_y) max_y = y;
00149       if (z > max_z) max_z = z;
00150     }
00151 
00152     lowerBound(0) = min_x; lowerBound(1) = min_y; lowerBound(2) = min_z;
00153     upperBound(0) = max_x; upperBound(1) = max_y; upperBound(2) = max_z;
00154   }
00155 
00156 
00157   void Pointcloud::crop(point3d lowerBound, point3d upperBound) {
00158 
00159     Pointcloud result;
00160 
00161     float min_x, min_y, min_z;
00162     float max_x, max_y, max_z;
00163     float x,y,z;
00164 
00165     min_x = lowerBound(0); min_y = lowerBound(1); min_z = lowerBound(2);
00166     max_x = upperBound(0); max_y = upperBound(1); max_z = upperBound(2);
00167 
00168     for (Pointcloud::const_iterator it=begin(); it!=end(); it++) {
00169       x = (*it)(0);
00170       y = (*it)(1);
00171       z = (*it)(2);
00172 
00173       if ( (x >= min_x) &&
00174            (y >= min_y) &&
00175            (z >= min_z) &&
00176            (x <= max_x) &&
00177            (y <= max_y) &&
00178            (z <= max_z) ) {
00179         result.push_back (x,y,z);
00180       }
00181     } // end for points
00182 
00183     this->clear();
00184     this->push_back(result);
00185 
00186   }
00187 
00188 
00189   void Pointcloud::minDist(double thres) {
00190     Pointcloud result;
00191 
00192     float x,y,z;
00193     for (Pointcloud::const_iterator it=begin(); it!=end(); it++) {
00194       x = (*it)(0);
00195       y = (*it)(1);
00196       z = (*it)(2);
00197       double dist = sqrt(x*x+y*y+z*z);
00198       if ( dist > thres ) result.push_back (x,y,z);
00199     } // end for points
00200     this->clear();
00201     this->push_back(result);
00202   }
00203 
00204 
00205   void Pointcloud::subSampleRandom(unsigned int num_samples, Pointcloud& sample_cloud) {
00206     point3d_collection samples;
00207     // visual studio does not support random_sample_n and neither does libc++
00208   #if defined(_MSC_VER) || defined(_LIBCPP_VERSION)
00209     samples.reserve(this->size());
00210     samples.insert(samples.end(), this->begin(), this->end());
00211     std::random_shuffle(samples.begin(), samples.end());
00212     samples.resize(num_samples);
00213   #else
00214     random_sample_n(begin(), end(), std::back_insert_iterator<point3d_collection>(samples), num_samples);
00215     for (unsigned int i=0; i<samples.size(); i++) {
00216       sample_cloud.push_back(samples[i]);
00217     }
00218   #endif
00219   }
00220 
00221 
00222   void Pointcloud::writeVrml(std::string filename){
00223 
00224     std::ofstream outfile (filename.c_str());
00225 
00226     outfile << "#VRML V2.0 utf8" << std::endl;
00227     outfile << "Transform {" << std::endl;
00228     outfile << "translation 0 0 0" << std::endl;
00229     outfile << "rotation 0 0 0 0" << std::endl;
00230     outfile << "  children [" << std::endl;
00231     outfile << "     Shape{" << std::endl;
00232     outfile << "  geometry PointSet {" << std::endl;
00233     outfile << "      coord Coordinate {" << std::endl;
00234     outfile << "          point [" << std::endl;
00235 
00236     OCTOMAP_DEBUG_STR("PointCloud::writeVrml writing "
00237               << points.size() << " points to " 
00238               << filename.c_str() <<  ".");
00239 
00240     for (unsigned int i = 0; i < (points.size()); i++){
00241       outfile << "\t\t" << (points[i])(0) 
00242               << " " <<    (points[i])(1) 
00243               <<  " " <<   (points[i])(2) 
00244               << "\n";
00245     }
00246 
00247     outfile << "                 ]" << std::endl;
00248     outfile << "      }" << std::endl;
00249     outfile << "    color Color{" << std::endl;
00250     outfile << "              color [" << std::endl;
00251 
00252     for (unsigned int i = 0; i < points.size(); i++){
00253       outfile << "\t\t 1.0 1.0 1.0 \n";
00254     }
00255 
00256     outfile << "                 ]" << std::endl;
00257     outfile << "      }" << std::endl;
00258 
00259     outfile << "   }" << std::endl;
00260     outfile << "     }" << std::endl;
00261 
00262 
00263     outfile << "  ]" << std::endl;
00264     outfile << "}" << std::endl;
00265 
00266 
00267   }
00268 
00269   std::istream& Pointcloud::read(std::istream &s){
00270     while (!s.eof()){
00271       point3d p;
00272       for (unsigned int i=0; i<3; i++){
00273         s >> p(i);
00274       }
00275       if (!s.fail()){
00276         this->push_back(p);
00277       } else {
00278         break;
00279       }
00280     }
00281 
00282     return s;
00283   }
00284 
00285   std::istream& Pointcloud::readBinary(std::istream &s) {
00286 
00287     unsigned int pc_size = 0;
00288     s.read((char*)&pc_size, sizeof(pc_size));
00289     OCTOMAP_DEBUG("Reading %d points from binary file...", pc_size);
00290 
00291     if (pc_size > 0) {
00292       this->points.reserve(pc_size);
00293       point3d p;
00294       for (unsigned int i=0; i<pc_size; i++) {
00295         p.readBinary(s);
00296         if (!s.fail()) {
00297           this->push_back(p);
00298         }
00299         else {
00300           OCTOMAP_ERROR("Pointcloud::readBinary: ERROR.\n" );
00301           break;
00302         }
00303       }
00304     }
00305     OCTOMAP_DEBUG("done.\n");
00306 
00307     return s;
00308   }
00309 
00310 
00311   std::ostream& Pointcloud::writeBinary(std::ostream &s) const {
00312 
00313     unsigned int pc_size = this->size();
00314     OCTOMAP_DEBUG("Writing %d points to binary file...", pc_size);
00315     s.write((char*)&pc_size, sizeof(pc_size));
00316 
00317     for (Pointcloud::const_iterator it = this->begin(); it != this->end(); it++) {
00318       it->writeBinary(s);
00319     }
00320     OCTOMAP_DEBUG("done.\n");
00321 
00322     return s;
00323   }
00324 
00325 } // end namespace


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14