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 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/torus_finder.h"
00037 #include "jsk_recognition_utils/pcl_conversion_util.h"
00038 #include <jsk_topic_tools/rosparam_utils.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 <pcl/filters/voxel_grid.h>
00043
00044 namespace jsk_pcl_ros
00045 {
00046 void TorusFinder::onInit()
00047 {
00048 DiagnosticNodelet::onInit();
00049 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00050 pnh_->param("use_hint", use_hint_, false);
00051 if (use_hint_) {
00052 if (pnh_->hasParam("initial_axis_hint")) {
00053 std::vector<double> axis;
00054 jsk_topic_tools::readVectorParameter(*pnh_, "initial_axis_hint", axis);
00055 if (axis.size() == 3) {
00056 hint_axis_[0] = axis[0];
00057 hint_axis_[1] = axis[1];
00058 hint_axis_[2] = axis[2];
00059 }
00060 else {
00061 hint_axis_[0] = 0;
00062 hint_axis_[1] = 0;
00063 hint_axis_[2] = 1;
00064 }
00065 }
00066 }
00067 pnh_->param("use_normal", use_normal_, false);
00068 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00069 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00070 boost::bind (&TorusFinder::configCallback, this, _1, _2);
00071 srv_->setCallback (f);
00072
00073 pub_torus_ = advertise<jsk_recognition_msgs::Torus>(*pnh_, "output", 1);
00074 pub_torus_array_ = advertise<jsk_recognition_msgs::TorusArray>(*pnh_, "output/array", 1);
00075 pub_torus_with_failure_ = advertise<jsk_recognition_msgs::Torus>(*pnh_, "output/with_failure", 1);
00076 pub_torus_array_with_failure_ = advertise<jsk_recognition_msgs::TorusArray>(*pnh_, "output/with_failure/array", 1);
00077 pub_inliers_ = advertise<PCLIndicesMsg>(*pnh_, "output/inliers", 1);
00078 pub_pose_stamped_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output/pose", 1);
00079 pub_coefficients_ = advertise<PCLModelCoefficientMsg>(
00080 *pnh_, "output/coefficients", 1);
00081 pub_latest_time_ = advertise<std_msgs::Float32>(
00082 *pnh_, "output/latest_time", 1);
00083 pub_average_time_ = advertise<std_msgs::Float32>(
00084 *pnh_, "output/average_time", 1);
00085
00086 done_initialization_ = true;
00087 onInitPostProcess();
00088 }
00089
00090 void TorusFinder::configCallback(Config &config, uint32_t level)
00091 {
00092 boost::mutex::scoped_lock lock(mutex_);
00093 min_radius_ = config.min_radius;
00094 max_radius_ = config.max_radius;
00095 outlier_threshold_ = config.outlier_threshold;
00096 max_iterations_ = config.max_iterations;
00097 min_size_ = config.min_size;
00098 eps_hint_angle_ = config.eps_hint_angle;
00099 algorithm_ = config.algorithm;
00100 voxel_grid_sampling_ = config.voxel_grid_sampling;
00101 voxel_size_ = config.voxel_size;
00102 }
00103
00104 void TorusFinder::subscribe()
00105 {
00106 sub_ = pnh_->subscribe("input", 1, &TorusFinder::segment, this);
00107 sub_points_ = pnh_->subscribe("input/polygon", 1, &TorusFinder::segmentFromPoints, this);
00108 }
00109
00110 void TorusFinder::unsubscribe()
00111 {
00112 sub_.shutdown();
00113 sub_points_.shutdown();
00114 }
00115
00116 void TorusFinder::segmentFromPoints(
00117 const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg)
00118 {
00119 if (!done_initialization_) {
00120 return;
00121 }
00122
00123
00124 pcl::PointCloud<pcl::PointNormal>::Ptr cloud
00125 (new pcl::PointCloud<pcl::PointNormal>);
00126 for (size_t i = 0; i < polygon_msg->polygon.points.size(); i++) {
00127 geometry_msgs::Point32 p = polygon_msg->polygon.points[i];
00128 pcl::PointNormal pcl_point;
00129 pcl_point.x = p.x;
00130 pcl_point.y = p.y;
00131 pcl_point.z = p.z;
00132 cloud->points.push_back(pcl_point);
00133 }
00134 sensor_msgs::PointCloud2 ros_cloud;
00135 pcl::toROSMsg(*cloud, ros_cloud);
00136 ros_cloud.header = polygon_msg->header;
00137 segment(boost::make_shared<sensor_msgs::PointCloud2>(ros_cloud));
00138 }
00139
00140 void TorusFinder::segment(
00141 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00142 {
00143 if (!done_initialization_) {
00144 return;
00145 }
00146 boost::mutex::scoped_lock lock(mutex_);
00147 vital_checker_->poke();
00148 pcl::PointCloud<pcl::PointNormal>::Ptr cloud
00149 (new pcl::PointCloud<pcl::PointNormal>);
00150 pcl::fromROSMsg(*cloud_msg, *cloud);
00151 jsk_recognition_utils::ScopedWallDurationReporter r
00152 = timer_.reporter(pub_latest_time_, pub_average_time_);
00153 if (voxel_grid_sampling_) {
00154 pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
00155 (new pcl::PointCloud<pcl::PointNormal>);
00156 pcl::VoxelGrid<pcl::PointNormal> sor;
00157 sor.setInputCloud (cloud);
00158 sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
00159 sor.filter (*downsampled_cloud);
00160 cloud = downsampled_cloud;
00161 }
00162
00163 pcl::SACSegmentation<pcl::PointNormal> seg;
00164 if (use_normal_) {
00165 pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
00166 seg_normal.setInputNormals(cloud);
00167 seg = seg_normal;
00168 }
00169
00170
00171 seg.setOptimizeCoefficients (true);
00172 seg.setInputCloud(cloud);
00173 seg.setRadiusLimits(min_radius_, max_radius_);
00174 if (algorithm_ == "RANSAC") {
00175 seg.setMethodType(pcl::SAC_RANSAC);
00176 }
00177 else if (algorithm_ == "LMEDS") {
00178 seg.setMethodType(pcl::SAC_LMEDS);
00179 }
00180 else if (algorithm_ == "MSAC") {
00181 seg.setMethodType(pcl::SAC_MSAC);
00182 }
00183 else if (algorithm_ == "RRANSAC") {
00184 seg.setMethodType(pcl::SAC_RRANSAC);
00185 }
00186 else if (algorithm_ == "RMSAC") {
00187 seg.setMethodType(pcl::SAC_RMSAC);
00188 }
00189 else if (algorithm_ == "MLESAC") {
00190 seg.setMethodType(pcl::SAC_MLESAC);
00191 }
00192 else if (algorithm_ == "PROSAC") {
00193 seg.setMethodType(pcl::SAC_PROSAC);
00194 }
00195
00196 seg.setDistanceThreshold (outlier_threshold_);
00197 seg.setMaxIterations (max_iterations_);
00198 seg.setModelType(pcl::SACMODEL_CIRCLE3D);
00199 if (use_hint_) {
00200 seg.setAxis(hint_axis_);
00201 seg.setEpsAngle(eps_hint_angle_);
00202 }
00203 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00204 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00205 seg.segment(*inliers, *coefficients);
00206 NODELET_INFO("input points: %lu", cloud->points.size());
00207 if (inliers->indices.size() > min_size_) {
00208
00209
00210 Eigen::Vector3f dir(coefficients->values[4],
00211 coefficients->values[5],
00212 coefficients->values[6]);
00213 if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
00214 dir = - dir;
00215 }
00216
00217 Eigen::Affine3f pose = Eigen::Affine3f::Identity();
00218 Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
00219 coefficients->values[1],
00220 coefficients->values[2]);
00221 Eigen::Quaternionf rot;
00222 rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
00223 pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
00224 PCLIndicesMsg ros_inliers;
00225 ros_inliers.indices = inliers->indices;
00226 ros_inliers.header = cloud_msg->header;
00227 pub_inliers_.publish(ros_inliers);
00228 PCLModelCoefficientMsg ros_coefficients;
00229 ros_coefficients.values = coefficients->values;
00230 ros_coefficients.header = cloud_msg->header;
00231 pub_coefficients_.publish(ros_coefficients);
00232 jsk_recognition_msgs::Torus torus_msg;
00233 torus_msg.header = cloud_msg->header;
00234 tf::poseEigenToMsg(pose, torus_msg.pose);
00235 torus_msg.small_radius = 0.01;
00236 torus_msg.large_radius = coefficients->values[3];
00237 pub_torus_.publish(torus_msg);
00238 jsk_recognition_msgs::TorusArray torus_array_msg;
00239 torus_array_msg.header = cloud_msg->header;
00240 torus_array_msg.toruses.push_back(torus_msg);
00241 pub_torus_array_.publish(torus_array_msg);
00242
00243 geometry_msgs::PoseStamped pose_stamped;
00244 pose_stamped.header = torus_msg.header;
00245 pose_stamped.pose = torus_msg.pose;
00246 pub_pose_stamped_.publish(pose_stamped);
00247 pub_torus_array_with_failure_.publish(torus_array_msg);
00248 pub_torus_with_failure_.publish(torus_msg);
00249 }
00250 else {
00251 jsk_recognition_msgs::Torus torus_msg;
00252 torus_msg.header = cloud_msg->header;
00253 torus_msg.failure = true;
00254 pub_torus_with_failure_.publish(torus_msg);
00255 jsk_recognition_msgs::TorusArray torus_array_msg;
00256 torus_array_msg.header = cloud_msg->header;
00257 torus_array_msg.toruses.push_back(torus_msg);
00258 pub_torus_array_with_failure_.publish(torus_array_msg);
00259 NODELET_INFO("failed to find torus");
00260 }
00261 }
00262 }
00263
00264 #include <pluginlib/class_list_macros.h>
00265 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::TorusFinder, nodelet::Nodelet);