Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
00104 current_inv_transform = transform.inv();
00105 }
00106
00107
00108 void Pointcloud::transformAbsolute(pose6d transform) {
00109
00110
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 }
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 }
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
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 }