Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * tabletop_objects_standalone.cpp - Thread to detect tabletop objects 00004 * 00005 * Created: Sat Oct 01 11:51:00 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 /// @cond EXAMPLE 00023 00024 #include <boost/thread/thread.hpp> 00025 #include <boost/date_time/posix_time/posix_time.hpp> 00026 #include <pcl/point_cloud.h> 00027 #include <pcl/point_types.h> 00028 // include <pcl/io/openni_grabber.h> 00029 #include <pcl/visualization/cloud_viewer.h> 00030 #include <pcl/io/openni_camera/openni_driver.h> 00031 #include <pcl/console/parse.h> 00032 #include <pcl/sample_consensus/method_types.h> 00033 #include <pcl/sample_consensus/model_types.h> 00034 #include <pcl/segmentation/sac_segmentation.h> 00035 #include <pcl/segmentation/extract_clusters.h> 00036 #include <pcl/filters/voxel_grid.h> 00037 #include <pcl/filters/extract_indices.h> 00038 #include <pcl/surface/concave_hull.h> 00039 #include <pcl/surface/convex_hull.h> 00040 #include <pcl/kdtree/kdtree.h> 00041 #include <pcl/kdtree/kdtree_flann.h> 00042 #include <pcl/filters/project_inliers.h> 00043 #include <pcl/filters/conditional_removal.h> 00044 #include <pcl/segmentation/extract_polygonal_prism_data.h> 00045 00046 #include <fvcams/shmem.h> 00047 #include <fvutils/ipc/shm_image.h> 00048 #include <fvutils/adapters/pcl.h> 00049 #include <utils/time/time.h> 00050 00051 #define TABLE_MAX_X 3.0 00052 #define TABLE_MAX_Y 3.0 00053 #define TABLE_MIN_X -3.0 00054 #define TABLE_MIN_Y -3.0 00055 00056 using namespace fawkes; 00057 using namespace firevision; 00058 00059 template <typename PointT> 00060 class PolygonComparison : public pcl::ComparisonBase<PointT> 00061 { 00062 using pcl::ComparisonBase<PointT>::capable_; 00063 public: 00064 typedef boost::shared_ptr<PolygonComparison<PointT> > Ptr; 00065 typedef boost::shared_ptr<const PolygonComparison<PointT> > ConstPtr; 00066 00067 PolygonComparison(const pcl::PointCloud<PointT> &polygon, bool inside = true) 00068 : inside_(inside), polygon_(polygon) 00069 { 00070 capable_ = (polygon.size() >= 3); 00071 } 00072 virtual ~PolygonComparison() {} 00073 00074 virtual bool evaluate(const PointT &point) const 00075 { 00076 if (inside_) 00077 return pcl::isPointIn2DPolygon(point, polygon_); 00078 else 00079 return ! pcl::isPointIn2DPolygon(point, polygon_); 00080 } 00081 00082 protected: 00083 bool inside_; 00084 const pcl::PointCloud<PointT> &polygon_; 00085 00086 private: 00087 PolygonComparison() {} // not allowed 00088 }; 00089 00090 00091 template <typename PointT> 00092 class PlaneDistanceComparison : public pcl::ComparisonBase<PointT> 00093 { 00094 using pcl::ComparisonBase<PointT>::capable_; 00095 public: 00096 typedef boost::shared_ptr<PlaneDistanceComparison<PointT> > Ptr; 00097 typedef boost::shared_ptr<const PlaneDistanceComparison<PointT> > ConstPtr; 00098 00099 PlaneDistanceComparison(pcl::ModelCoefficients::ConstPtr coeff, 00100 pcl::ComparisonOps::CompareOp op = pcl::ComparisonOps::GT, 00101 float compare_val = 0.) 00102 : coeff_(coeff), op_(op), compare_val_(compare_val) 00103 { 00104 capable_ = (coeff_->values.size() == 4); 00105 } 00106 virtual ~PlaneDistanceComparison() {} 00107 00108 virtual bool evaluate(const PointT &point) const 00109 { 00110 float val = (coeff_->values[0] * point.x + coeff_->values[1] * point.y + 00111 coeff_->values[2] * point.z + coeff_->values[3]) / 00112 sqrtf(coeff_->values[0] * coeff_->values[0] + 00113 coeff_->values[1] * coeff_->values[1] + 00114 coeff_->values[2] * coeff_->values[2]); 00115 00116 //printf("%f > %f?: %d\n", val, compare_val_, (val > compare_val_)); 00117 00118 if (op_ == pcl::ComparisonOps::GT) { 00119 return val > compare_val_; 00120 } else if (op_ == pcl::ComparisonOps::GE) { 00121 return val >= compare_val_; 00122 } else if (op_ == pcl::ComparisonOps::LT) { 00123 return val < compare_val_; 00124 } else if (op_ == pcl::ComparisonOps::LE) { 00125 return val <= compare_val_; 00126 } else { 00127 return val == compare_val_; 00128 } 00129 } 00130 00131 protected: 00132 pcl::ModelCoefficients::ConstPtr coeff_; 00133 pcl::ComparisonOps::CompareOp op_; 00134 float compare_val_; 00135 00136 private: 00137 PlaneDistanceComparison() {} // not allowed 00138 }; 00139 00140 template <typename PointType> 00141 class OpenNIPlanarSegmentation 00142 { 00143 public: 00144 typedef pcl::PointCloud<PointType> Cloud; 00145 typedef typename Cloud::Ptr CloudPtr; 00146 typedef typename Cloud::ConstPtr CloudConstPtr; 00147 00148 OpenNIPlanarSegmentation (const std::string& device_id = "", double threshold = 0.01) 00149 : viewer ("PCL OpenNI Planar Segmentation Viewer"), 00150 device_id_ (device_id), 00151 new_cloud_(false) 00152 { 00153 grid_.setFilterFieldName ("z"); 00154 grid_.setFilterLimits (0.0, 3.0); 00155 grid_.setLeafSize (0.01, 0.01, 0.01); 00156 00157 seg_.setOptimizeCoefficients (true); 00158 seg_.setModelType (pcl::SACMODEL_PLANE); 00159 seg_.setMethodType (pcl::SAC_RANSAC); 00160 seg_.setMaxIterations (1000); 00161 seg_.setDistanceThreshold (threshold); 00162 00163 } 00164 00165 void 00166 cloud_cb_ (const CloudConstPtr& cloud) 00167 { 00168 set (cloud); 00169 } 00170 00171 void 00172 set (const CloudConstPtr& cloud) 00173 { 00174 //lock while we set our cloud; 00175 boost::mutex::scoped_lock lock (mtx_); 00176 cloud_ = cloud; 00177 } 00178 00179 int get_nn(float x, float y, float z) const 00180 { 00181 PointType p(x, y, z); 00182 std::vector<int> indices; 00183 std::vector<float> distances; 00184 00185 float min_dist = HUGE; 00186 int count = kdtree_.radiusSearch(p, 1.0, indices, distances); 00187 if (! indices.empty()) { 00188 printf("Got %i indices!\n", count); 00189 int index = 0; 00190 for (int i = 0; i < count; ++i) { 00191 if (distances[i] < min_dist) { 00192 index = i; 00193 min_dist = distances[i]; 00194 } 00195 } 00196 printf("Found at dist %f (%f, %f, %f)\n", distances[index], 00197 cloud_proj_->points[indices[index]].x, 00198 cloud_proj_->points[indices[index]].y, 00199 cloud_proj_->points[indices[index]].z); 00200 return indices[index]; 00201 } else { 00202 printf("No index found looking for (%f, %f, %f)\n", x, y, z); 00203 } 00204 00205 return -1; 00206 } 00207 00208 CloudPtr 00209 get () 00210 { 00211 //lock while we swap our cloud and reset it. 00212 boost::mutex::scoped_lock lock (mtx_); 00213 CloudPtr temp_cloud (new Cloud); 00214 CloudPtr temp_cloud2 (new Cloud); 00215 00216 grid_.setInputCloud (cloud_); 00217 grid_.filter (*temp_cloud); 00218 00219 // set all colors to white for better distinguishing the pixels 00220 typename pcl::PointCloud<PointType>::iterator p; 00221 for (p = temp_cloud->begin(); p != temp_cloud->end(); ++p) { 00222 p->r = 255; 00223 p->g = 255; 00224 p->b = 255; 00225 } 00226 00227 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); 00228 pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); 00229 00230 seg_.setInputCloud (temp_cloud); 00231 seg_.segment (*inliers, *coefficients); 00232 00233 extract_.setNegative (false); 00234 extract_.setInputCloud (temp_cloud); 00235 extract_.setIndices (inliers); 00236 extract_.filter (*temp_cloud2); 00237 00238 // Project the model inliers 00239 pcl::ProjectInliers<PointType> proj; 00240 proj.setModelType(pcl::SACMODEL_PLANE); 00241 proj.setInputCloud(temp_cloud2); 00242 proj.setModelCoefficients(coefficients); 00243 cloud_proj_.reset(new Cloud()); 00244 proj.filter (*cloud_proj_); 00245 //printf("PointCloud after projection has: %zu data points.\n", 00246 // cloud_proj_->points.size()); 00247 00248 00249 // Estimate 3D convex hull -> TABLE BOUNDARIES 00250 pcl::ConvexHull<PointType> hr; 00251 //hr.setAlpha (0.1); // only for ConcaveHull 00252 hr.setInputCloud(cloud_proj_); 00253 cloud_hull_.reset(new Cloud()); 00254 hr.reconstruct (*cloud_hull_, vertices_); 00255 00256 //printf("Found %zu vertices, first has size %zu\n", 00257 // vertices_.size(), vertices_[0].vertices.size()); 00258 00259 for (size_t i = 0; i < cloud_proj_->points.size(); ++i) { 00260 cloud_proj_->points[i].r = 0; 00261 cloud_proj_->points[i].g = 255; 00262 cloud_proj_->points[i].b = 0; 00263 } 00264 00265 // Extrat all non-plane points 00266 cloud_filt_.reset(new Cloud()); 00267 extract_.setNegative(true); 00268 extract_.filter(*cloud_filt_); 00269 00270 // remove all pixels below table 00271 typename PlaneDistanceComparison<PointType>::ConstPtr 00272 above_comp(new PlaneDistanceComparison<PointType>(coefficients, pcl::ComparisonOps::LT)); 00273 typename pcl::ConditionAnd<PointType>::Ptr 00274 above_cond(new pcl::ConditionAnd<PointType>()); 00275 above_cond->addComparison(above_comp); 00276 pcl::ConditionalRemoval<PointType> above_condrem(above_cond); 00277 above_condrem.setInputCloud(cloud_filt_); 00278 //above_condrem.setKeepOrganized(true); 00279 cloud_above_.reset(new Cloud()); 00280 above_condrem.filter(*cloud_above_); 00281 00282 printf("Before: %zu After: %zu\n", cloud_filt_->points.size(), 00283 cloud_above_->points.size()); 00284 00285 // Extract only points on the table plane 00286 if (! vertices_.empty()) { 00287 pcl::PointIndices::Ptr polygon(new pcl::PointIndices()); 00288 polygon->indices = vertices_[0].vertices; 00289 00290 pcl::PointCloud<PointType> polygon_cloud; 00291 pcl::ExtractIndices<PointType> polygon_extract; 00292 00293 polygon_extract.setInputCloud(cloud_hull_); 00294 polygon_extract.setIndices(polygon); 00295 polygon_extract.filter(polygon_cloud); 00296 00297 typename pcl::ConditionAnd<PointType>::Ptr 00298 polygon_cond(new pcl::ConditionAnd<PointType>()); 00299 00300 typename PolygonComparison<PointType>::ConstPtr 00301 inpoly_comp(new PolygonComparison<PointType>(polygon_cloud)); 00302 polygon_cond->addComparison(inpoly_comp); 00303 00304 // build the filter 00305 pcl::ConditionalRemoval<PointType> condrem(polygon_cond); 00306 condrem.setInputCloud(cloud_above_); 00307 //condrem.setKeepOrganized(true); 00308 cloud_objs_.reset(new Cloud()); 00309 condrem.filter(*cloud_objs_); 00310 } else { 00311 cloud_objs_.reset(new Cloud(*cloud_above_)); 00312 } 00313 00314 // CLUSTERS 00315 // extract clusters of OBJECTS 00316 00317 // Creating the KdTree object for the search method of the extraction 00318 pcl::KdTree<pcl::PointXYZRGB>::Ptr 00319 kdtree_cl(new pcl::KdTreeFLANN<pcl::PointXYZRGB>()); 00320 kdtree_cl->setInputCloud(cloud_objs_); 00321 00322 std::vector<pcl::PointIndices> cluster_indices; 00323 pcl::EuclideanClusterExtraction<PointType> ec; 00324 ec.setClusterTolerance(0.04); // 2cm 00325 ec.setMinClusterSize(50); 00326 ec.setMaxClusterSize(25000); 00327 ec.setSearchMethod(kdtree_cl); 00328 ec.setInputCloud(cloud_objs_); 00329 ec.extract(cluster_indices); 00330 00331 printf("Found %zu clusters\n", cluster_indices.size()); 00332 00333 uint8_t colors[5][3] = { {255, 0, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255}, 00334 {0, 255, 255} }; 00335 00336 std::vector<pcl::PointIndices>::const_iterator it; 00337 unsigned int color = 0; 00338 unsigned int i = 0; 00339 for (it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { 00340 uint8_t r, g, b; 00341 if (color < 5) { 00342 r = colors[color][0]; 00343 g = colors[color][1]; 00344 b = colors[color][2]; 00345 ++color; 00346 } else { 00347 double dr=0, dg=0, db=0; 00348 pcl::visualization::getRandomColors(dr, dg, db); 00349 r = (uint8_t)roundf(dr * 255); 00350 g = (uint8_t)roundf(dg * 255); 00351 b = (uint8_t)roundf(db * 255); 00352 } 00353 printf("Cluster %u size: %zu color %u, %u, %u\n", 00354 ++i, it->indices.size(), r, g, b); 00355 std::vector<int>::const_iterator pit; 00356 for (pit = it->indices.begin (); pit != it->indices.end(); pit++) { 00357 cloud_objs_->points[*pit].r = r; 00358 cloud_objs_->points[*pit].g = g; 00359 cloud_objs_->points[*pit].b = b; 00360 } 00361 } 00362 00363 /* To project into Z: 00364 // Create a set of planar coefficients with X=Y=0,Z=1 00365 pcl::ModelCoefficients::Ptr proj_coeff (new pcl::ModelCoefficients ()); 00366 proj_coeff->values.resize (4); 00367 proj_coeff->values[0] = proj_coeff->values[1] = 0; 00368 proj_coeff->values[2] = 1.0; 00369 proj_coeff->values[3] = 0; 00370 00371 // Create the filtering object 00372 pcl::ProjectInliers<PointType> proj; 00373 proj.setModelType(pcl::SACMODEL_PLANE); 00374 proj.setInputCloud(cloud_hull_); 00375 proj.setModelCoefficients(proj_coeff); 00376 cloud_proj_.reset(new Cloud()); 00377 proj.filter(*cloud_proj_); 00378 00379 printf("Vertices: %zu, first: %zu\n", vertices_.size(), vertices_[0].vertices.size()); 00380 vertices_.resize(5); 00381 00382 //get the extents of the table 00383 if (! cloud_proj_->points.empty()) { 00384 printf("Cloud hull non-empty\n"); 00385 float x_min = HUGE, y_min = HUGE, x_max = 0, y_max = 0; 00386 00387 for (size_t i = 1; i < cloud_proj_->points.size(); ++i) { 00388 if (cloud_proj_->points[i].x < x_min && cloud_proj_->points[i].x > -3.0) 00389 x_min = cloud_proj_->points[i].x; 00390 if (cloud_proj_->points[i].x > x_max && cloud_proj_->points[i].x < 3.0) 00391 x_max = cloud_proj_->points[i].x; 00392 if (cloud_proj_->points[i].y < y_min && cloud_proj_->points[i].y > -3.0) 00393 y_min = cloud_proj_->points[i].y; 00394 if (cloud_proj_->points[i].y > y_max && cloud_proj_->points[i].y < 3.0) 00395 y_max = cloud_proj_->points[i].y; 00396 } 00397 00398 kdtree_.setInputCloud(cloud_proj_); 00399 float table_z = cloud_proj_->points[0].z; // only need a very rough estimate 00400 00401 printf("x_min=%f x_max=%f y_min=%f y_max=%f table_z = %f\n", 00402 x_min, x_max, y_min, y_max, table_z); 00403 00404 int blp = get_nn(x_min, y_min, table_z); 00405 int brp = get_nn(x_max, y_min, table_z); 00406 int tlp = get_nn(x_min, y_max, table_z); 00407 int trp = get_nn(x_max, y_max, table_z); 00408 00409 printf("blp=%i (%f,%f,%f) brp=%i (%f,%f,%f) " 00410 "tlp=%i (%f,%f,%f) trp=%i (%f,%f,%f)\n", 00411 blp, cloud_proj_->points[blp].x, 00412 cloud_proj_->points[blp].y, cloud_proj_->points[blp].z, 00413 brp, cloud_proj_->points[brp].x, 00414 cloud_proj_->points[brp].y, cloud_proj_->points[brp].z, 00415 tlp, cloud_proj_->points[tlp].x, 00416 cloud_proj_->points[tlp].y, cloud_proj_->points[tlp].z, 00417 trp, cloud_proj_->points[trp].x, 00418 cloud_proj_->points[trp].y, cloud_proj_->points[trp].z); 00419 00420 pcl::Vertices v; 00421 v.vertices.push_back(blp); 00422 v.vertices.push_back(tlp); 00423 v.vertices.push_back(trp); 00424 v.vertices.push_back(brp); 00425 v.vertices.push_back(blp); 00426 vertices_.clear(); 00427 vertices_.push_back(v); 00428 } 00429 */ 00430 00431 new_cloud_ = true; 00432 00433 // To show differences between cloud_filt and cloud_above 00434 // (draw both, increase point size of cloud_above 00435 //for (int i = 0; i < cloud_filt_->points.size(); ++i) { 00436 // cloud_filt_->points[i].r = 255; 00437 // cloud_filt_->points[i].g = 0; 00438 // cloud_filt_->points[i].b = 0; 00439 //} 00440 00441 return (cloud_above_); 00442 } 00443 00444 void 00445 viz_cb (pcl::visualization::PCLVisualizer& viz) 00446 { 00447 if (!new_cloud_) 00448 { 00449 boost::this_thread::sleep(boost::posix_time::milliseconds(1)); 00450 return; 00451 } 00452 00453 { 00454 boost::mutex::scoped_lock lock (mtx_); 00455 // Render the data 00456 if (!viz.updatePointCloud(cloud_proj_, "table")) { 00457 viz.addPointCloud (cloud_proj_, "table"); 00458 } 00459 if (!viz.updatePointCloud(cloud_objs_, "clusters")) { 00460 viz.addPointCloud (cloud_objs_, "clusters"); 00461 } 00462 00463 viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "table"); 00464 viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "clusters"); 00465 00466 viz.removeShape ("hull"); 00467 if (!vertices_.empty()) { 00468 viz.addPolygonMesh<PointType>(cloud_hull_, vertices_, "hull"); 00469 } 00470 new_cloud_ = false; 00471 } 00472 } 00473 00474 void 00475 run () 00476 { 00477 /* 00478 pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id_); 00479 00480 boost::function<void (const CloudConstPtr&)> f = 00481 boost::bind (&OpenNIPlanarSegmentation::cloud_cb_, this, _1); 00482 boost::signals2::connection c = interface->registerCallback(f); 00483 */ 00484 00485 viewer.runOnVisualizationThread(boost::bind(&OpenNIPlanarSegmentation::viz_cb, 00486 this, _1), "viz_cb"); 00487 00488 SharedMemoryCamera cam("openni-pointcloud"); 00489 SharedMemoryImageBuffer *buf = cam.shared_memory_image_buffer(); 00490 //interface->start (); 00491 00492 fawkes::Time last_update; 00493 00494 while (!viewer.wasStopped ()) 00495 { 00496 fawkes::Time ct = buf->capture_time(); 00497 if (last_update != ct) { 00498 last_update = ct; 00499 00500 cam.capture(); 00501 00502 CloudPtr cloud(new Cloud()); 00503 convert_buffer_to_pcl(buf, *cloud); 00504 set(cloud); 00505 00506 //the call to get() sets the cloud_ to null; 00507 viewer.showCloud(get()); 00508 } 00509 } 00510 00511 //interface->stop (); 00512 } 00513 00514 pcl::visualization::CloudViewer viewer; 00515 pcl::VoxelGrid<PointType> grid_; 00516 pcl::SACSegmentation<PointType> seg_; 00517 pcl::ExtractIndices<PointType> extract_; 00518 std::vector<pcl::Vertices> vertices_; 00519 CloudPtr cloud_hull_; 00520 CloudPtr cloud_proj_; 00521 CloudPtr cloud_filt_; 00522 CloudPtr cloud_above_; 00523 CloudPtr cloud_objs_; 00524 pcl::KdTreeFLANN<PointType> kdtree_; 00525 00526 std::string device_id_; 00527 boost::mutex mtx_; 00528 CloudConstPtr cloud_; 00529 bool new_cloud_; 00530 }; 00531 00532 void 00533 usage (char ** argv) 00534 { 00535 std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n" 00536 << "where options are:\n -thresh X :: set the planar segmentation threshold (default: 0.5)\n"; 00537 00538 openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance (); 00539 if (driver.getNumberDevices () > 0) 00540 { 00541 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx) 00542 { 00543 cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx) 00544 << ", connected: " << (int)driver.getBus (deviceIdx) << " @ " << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl; 00545 cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << endl 00546 << " bus@address for the device connected to a specific usb-bus / address combination (wotks only in Linux) or" << endl 00547 << " <serial-number> (only in Linux and for devices which provide serial numbers)" << endl; 00548 } 00549 } 00550 else 00551 cout << "No devices connected." << endl; 00552 } 00553 00554 int 00555 main (int argc, char ** argv) 00556 { 00557 if (argc < 2) 00558 { 00559 usage (argv); 00560 return 1; 00561 } 00562 00563 std::string arg (argv[1]); 00564 00565 if (arg == "--help" || arg == "-h") 00566 { 00567 usage (argv); 00568 return 1; 00569 } 00570 00571 double threshold = 0.05; 00572 pcl::console::parse_argument (argc, argv, "-thresh", threshold); 00573 00574 //pcl::OpenNIGrabber grabber (arg); 00575 //if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb> ()) 00576 //{ 00577 OpenNIPlanarSegmentation<pcl::PointXYZRGB> v (arg); 00578 v.run (); 00579 /* 00580 } 00581 else 00582 { 00583 printf("Only RGBD supported atm\n"); 00584 //OpenNIPlanarSegmentation<pcl::PointXYZ> v (arg, threshold); 00585 //v.run (); 00586 } 00587 */ 00588 00589 return (0); 00590 } 00591 00592 /// @endcond EXAMPLE