bench_ransac_plane_estimation.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 <iostream>
00037 #include <pcl/point_cloud.h>
00038 #include <pcl/point_types.h>
00039 #include <pcl/sample_consensus/method_types.h>
00040 #include <pcl/sample_consensus/model_types.h>
00041 #include <pcl/segmentation/sac_segmentation.h>
00042 #include <ctime>
00043 #include <boost/random.hpp>
00044 #include <string>
00045 #include <typeinfo>
00046 #include <sstream>
00047 #include<stdio.h>
00048 #include <sys/time.h>
00049 
00050 void usage(char** argv)
00051 {
00052   std::cerr << "Usage: " << argv[0] << " Iteration Number-of-points Max-iterations Gaussian-noise-variance Outlier-threshold" << std::endl;
00053 }
00054 
00055 pcl::PointCloud<pcl::PointXYZ>::Ptr generatePoints(const int num_points, const double variance)
00056 {
00057   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00058   boost::mt19937 gen = boost::mt19937(static_cast<unsigned long>(time(0)));
00059   boost::normal_distribution<> dst(0.0, sqrt(variance));
00060   boost::variate_generator<
00061     boost::mt19937&,
00062     boost::normal_distribution<> > rand(gen, dst);
00063    
00064   const double dx = 0.01;
00065   cloud->points.resize(num_points * num_points);
00066   for (size_t i = 0; i < num_points; i++) {
00067     for (size_t j = 0; j < num_points; j++) {
00068       pcl::PointXYZ p;
00069       p.x = i * dx;
00070       p.y = j * dx;
00071       p.z = rand();
00072       cloud->points[i * num_points + j] = p;
00073     }
00074   }
00075   return cloud;
00076 }
00077 
00078 int toInt(char* argv)
00079 {
00080   std::stringstream ss(argv);
00081   int val = 0;
00082   ss >> val;
00083   return val;
00084 }
00085 
00086 double toDouble(char* argv)
00087 {
00088   std::stringstream ss(argv);
00089   double val = 0;
00090   ss >> val;
00091   return val;
00092 }
00093 
00094 int main(int argc, char** argv)
00095 {
00096   if (argc != 6) {
00097     usage(argv);
00098     return 1;
00099   }
00100   int iteration = toInt(argv[1]);
00101   int number_of_points = toInt(argv[2]);
00102   int max_iterations = toInt(argv[3]);
00103   double variance = toDouble(argv[4]);
00104   double outlier_threshold = toDouble(argv[5]);
00105   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = generatePoints(number_of_points, variance);
00106   pcl::SACSegmentation<pcl::PointXYZ> seg;
00107   seg.setOptimizeCoefficients (true);
00108   seg.setRadiusLimits(0.0, std::numeric_limits<double>::max ()); // 0.3?
00109   seg.setMethodType (pcl::SAC_RANSAC);
00110   seg.setDistanceThreshold (outlier_threshold);
00111   seg.setInputCloud(cloud);
00112   seg.setMaxIterations (max_iterations);
00113   seg.setModelType (pcl::SACMODEL_PLANE);
00114   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00115   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00116   struct timeval s, e;
00117   gettimeofday(&s, NULL);
00118   for (size_t i = 0; i < iteration; i++) {
00119     seg.segment (*inliers, *coefficients);
00120   }
00121   gettimeofday(&e, NULL);
00122   // Total, each, iteration, number_of_points, max_iterations, variance, outlier_threshold
00123   double time = (e.tv_sec - s.tv_sec) + (e.tv_usec - s.tv_usec)*1.0E-6;
00124   printf("%lf,%lf,%d,%d,%d,%f,%f\n", time, time / iteration,
00125          iteration, number_of_points * number_of_points, max_iterations, variance, outlier_threshold);
00126   std::cerr << "Took " << time << " sec (" << time / iteration << " sec for each)" << std::endl;
00127   return 0;
00128 }


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 04:36:47