pointcloud_model_generator.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  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
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_footstep_planner/pointcloud_model_generator.h"
00037 #include <boost/random.hpp>
00038 
00039 namespace jsk_footstep_planner
00040 {
00041   void PointCloudModelGenerator::generate(
00042     const std::string& model_name,
00043     pcl::PointCloud<PointT>& output,
00044     double hole_rate)
00045   {
00046     output.points.clear();
00047     if (model_name == "flat") {
00048       flat(output, hole_rate);
00049     }
00050     else if (model_name == "stairs") {
00051       stairs(output, hole_rate);
00052     }
00053     else if (model_name == "hills") {
00054       hills(output, hole_rate);
00055     }
00056     else if (model_name == "gaussian") {
00057       gaussian(output, hole_rate);
00058     }
00059     else if (model_name == "flat_pole") {
00060       flatPole(output, hole_rate);
00061     }
00062   }
00063 
00064   void PointCloudModelGenerator::flat(pcl::PointCloud<PointT>& output, double hole_rate)
00065   {
00066     boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
00067     boost::uniform_real<> dst( 0, 100 );
00068     boost::variate_generator<
00069       boost::mt19937&, boost::uniform_real<>
00070       > rand( gen, dst );
00071 
00072     for (double y = -4; y < 4; y = y + 0.01) {
00073       for (double x = -4; x < 4; x = x + 0.01) {
00074         if (rand() >= hole_rate) {
00075           pcl::PointNormal p;
00076           p.x = x;
00077           p.y = y;
00078           output.points.push_back(p);
00079         }
00080       }
00081     }
00082   }
00083 
00084   void PointCloudModelGenerator::addPole(pcl::PointCloud<PointT>& output,
00085                                          const Eigen::Vector3f& center,
00086                                          const double width,
00087                                          const double height)
00088   {
00089     double y0 = center[1] + width / 2.0;
00090     double y1 = center[1] - width / 2.0;
00091     double x0 = center[0] + width / 2.0;
00092     double x1 = center[0] - width / 2.0;
00093     for (double x = x0; x > x1; x = x - 0.01) {
00094       for (double z = 0; z < height; z = z + 0.01) {
00095         pcl::PointNormal p;
00096         p.x = x;
00097         p.y = y0;
00098         p.z = z;
00099         output.points.push_back(p);
00100       }
00101     }
00102     for (double y = y0; y > y1; y = y - 0.01) {
00103       for (double z = 0; z < height; z = z + 0.01) {
00104         pcl::PointNormal p;
00105         p.x = x1;
00106         p.y = y;
00107         p.z = z;
00108         output.points.push_back(p);
00109       }
00110     }
00111     for (double x = x0; x > x1; x = x - 0.01) {
00112       for (double z = 0; z < height; z = z + 0.01) {
00113         pcl::PointNormal p;
00114         p.x = x;
00115         p.y = y1;
00116         p.z = z;
00117         output.points.push_back(p);
00118       }
00119     }
00120     for (double y = y0; y > y1; y = y - 0.01) {
00121       for (double z = 0; z < height; z = z + 0.01) {
00122         pcl::PointNormal p;
00123         p.x = x0;
00124         p.y = y;
00125         p.z = z;
00126         output.points.push_back(p);
00127       }
00128     }
00129   }
00130   
00131   void PointCloudModelGenerator::flatPole(pcl::PointCloud<PointT>& output, double hole_rate)
00132   {
00133     boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
00134     boost::uniform_real<> dst( 0, 100 );
00135     boost::variate_generator<
00136       boost::mt19937&, boost::uniform_real<>
00137       > rand( gen, dst );
00138 
00139     for (double y = -4; y < 4; y = y + 0.01) {
00140       for (double x = -4; x < 4; x = x + 0.01) {
00141         if (rand() >= hole_rate) {
00142           pcl::PointNormal p;
00143           p.x = x;
00144           p.y = y;
00145           output.points.push_back(p);
00146         }
00147       }
00148     }
00149     for (double y = -4; y < 4; y = y + 2.0) {
00150       for (double x = -4; x < 4; x = x + 2.0) {
00151         if (x != 0.0 || y != 0.0) {
00152           addPole(output, Eigen::Vector3f(x, y, 0), 0.2, 2.0);
00153         }
00154       }
00155     }
00156   }
00157 
00158   void PointCloudModelGenerator::hills(pcl::PointCloud<PointT>& output, double hole_rate)
00159   {
00160     boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
00161     boost::uniform_real<> dst( 0, 100 );
00162     boost::variate_generator<
00163       boost::mt19937&, boost::uniform_real<>
00164       > rand( gen, dst );
00165 
00166     const double height = 0.1;
00167     for (double y = -4; y < 4; y = y + 0.01) {
00168       for (double x = -4; x < 4; x = x + 0.01) {
00169         if (rand() >= hole_rate) {
00170           pcl::PointNormal p;
00171           p.x = x;
00172           p.y = y;
00173           p.z = height * sin(x * 2) * sin(y * 2);
00174           output.points.push_back(p);
00175         }
00176       }
00177     }
00178   }
00179 
00180   void PointCloudModelGenerator::gaussian(pcl::PointCloud<PointT>& output, double hole_rate)
00181   {
00182     boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
00183     boost::uniform_real<> dst( 0, 100 );
00184     boost::variate_generator<
00185       boost::mt19937&, boost::uniform_real<>
00186       > rand( gen, dst );
00187     const double height = 1.0;
00188     const double sigma = 0.3;
00189     for (double y = -4; y < 4; y = y + 0.01) {
00190       for (double x = -4; x < 4; x = x + 0.01) {
00191         if (rand() >= hole_rate) {
00192           pcl::PointNormal p;
00193           p.x = x;
00194           p.y = y;
00195           //p.z = height * sin(x * 2) * sin(y * 2);
00196           p.z = height * exp(-x*x / (2 * sigma * 2)) * exp(-y*y / (2 * sigma * 2));
00197           output.points.push_back(p);
00198         }
00199       }
00200     }
00201   }
00202 
00203   void PointCloudModelGenerator::stairs(pcl::PointCloud<PointT>& output, double hole_rate)
00204   {
00205     boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
00206     boost::uniform_real<> dst( 0, 100 );
00207     boost::variate_generator<
00208       boost::mt19937&, boost::uniform_real<>
00209       > rand( gen, dst );
00210 
00211     for (double y = -4; y < 4; y = y + 0.01) {
00212       // for (double x = -4; x < 0; x = x + 0.01) {
00213       //   if (rand() >= hole_rate) {
00214       //     pcl::PointNormal p;
00215       //     p.x = x;
00216       //     p.y = y;
00217       //     p.z = 0;
00218       //     output.points.push_back(p);
00219       //   }
00220       // }
00221       for (double x = -4; x < 5; x = x + 0.01) {
00222         if (rand() >= hole_rate) {
00223           pcl::PointNormal p;
00224           p.x = x;
00225           p.y = y;
00226           if (x > 0) {
00227             p.z = floor(x * 3) * 0.1;
00228           }
00229           else {
00230             p.z = ceil(x * 3) * 0.1;
00231           }
00232           output.points.push_back(p);
00233         }
00234       }
00235     }
00236   }
00237   
00238 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:29