35 #define BOOST_PARAMETER_MAX_ARITY 7 39 #include <pcl/sample_consensus/method_types.h> 40 #include <pcl/sample_consensus/model_types.h> 41 #include <pcl/segmentation/sac_segmentation.h> 42 #include <pcl/filters/voxel_grid.h> 48 DiagnosticNodelet::onInit();
49 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
52 if (
pnh_->hasParam(
"initial_axis_hint")) {
53 std::vector<double> axis;
55 if (axis.size() == 3) {
68 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
69 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
71 srv_->setCallback (f);
73 pub_torus_ = advertise<jsk_recognition_msgs::Torus>(*
pnh_,
"output", 1);
80 *
pnh_,
"output/coefficients", 1);
82 *
pnh_,
"output/latest_time", 1);
84 *
pnh_,
"output/average_time", 1);
117 const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg)
124 pcl::PointCloud<pcl::PointNormal>::Ptr
cloud 125 (
new pcl::PointCloud<pcl::PointNormal>);
126 for (
size_t i = 0; i < polygon_msg->polygon.points.size(); i++) {
127 geometry_msgs::Point32
p = polygon_msg->polygon.points[i];
128 pcl::PointNormal pcl_point;
132 cloud->points.push_back(pcl_point);
134 sensor_msgs::PointCloud2 ros_cloud;
136 ros_cloud.header = polygon_msg->header;
137 segment(boost::make_shared<sensor_msgs::PointCloud2>(ros_cloud));
141 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
148 pcl::PointCloud<pcl::PointNormal>::Ptr
cloud 149 (
new pcl::PointCloud<pcl::PointNormal>);
154 pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
155 (
new pcl::PointCloud<pcl::PointNormal>);
156 pcl::VoxelGrid<pcl::PointNormal> sor;
157 sor.setInputCloud (cloud);
159 sor.filter (*downsampled_cloud);
160 cloud = downsampled_cloud;
163 pcl::SACSegmentation<pcl::PointNormal> seg;
165 pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
166 seg_normal.setInputNormals(cloud);
171 seg.setOptimizeCoefficients (
true);
172 seg.setInputCloud(cloud);
175 seg.setMethodType(pcl::SAC_RANSAC);
178 seg.setMethodType(pcl::SAC_LMEDS);
181 seg.setMethodType(pcl::SAC_MSAC);
184 seg.setMethodType(pcl::SAC_RRANSAC);
187 seg.setMethodType(pcl::SAC_RMSAC);
190 seg.setMethodType(pcl::SAC_MLESAC);
193 seg.setMethodType(pcl::SAC_PROSAC);
198 seg.setModelType(pcl::SACMODEL_CIRCLE3D);
203 pcl::PointIndices::Ptr inliers (
new pcl::PointIndices);
204 pcl::ModelCoefficients::Ptr
coefficients (
new pcl::ModelCoefficients);
205 seg.segment(*inliers, *coefficients);
206 NODELET_INFO(
"input points: %lu", cloud->points.size());
207 if (inliers->indices.size() >
min_size_) {
210 Eigen::Vector3f dir(coefficients->values[4],
211 coefficients->values[5],
212 coefficients->values[6]);
213 if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
217 Eigen::Affine3f
pose = Eigen::Affine3f::Identity();
218 Eigen::Vector3f
pos = Eigen::Vector3f(coefficients->values[0],
219 coefficients->values[1],
220 coefficients->values[2]);
221 Eigen::Quaternionf
rot;
222 rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
223 pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
225 ros_inliers.indices = inliers->indices;
226 ros_inliers.header = cloud_msg->header;
229 ros_coefficients.values = coefficients->values;
230 ros_coefficients.header = cloud_msg->header;
232 jsk_recognition_msgs::Torus torus_msg;
233 torus_msg.header = cloud_msg->header;
235 torus_msg.small_radius = 0.01;
236 torus_msg.large_radius = coefficients->values[3];
238 jsk_recognition_msgs::TorusArray torus_array_msg;
239 torus_array_msg.header = cloud_msg->header;
240 torus_array_msg.toruses.push_back(torus_msg);
243 geometry_msgs::PoseStamped pose_stamped;
244 pose_stamped.header = torus_msg.header;
245 pose_stamped.pose = torus_msg.pose;
251 jsk_recognition_msgs::Torus torus_msg;
252 torus_msg.header = cloud_msg->header;
253 torus_msg.failure =
true;
255 jsk_recognition_msgs::TorusArray torus_array_msg;
256 torus_array_msg.header = cloud_msg->header;
257 torus_array_msg.toruses.push_back(torus_msg);
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::TorusFinder, nodelet::Nodelet)
double outlier_threshold_
Eigen::Vector3f hint_axis_
void publish(const boost::shared_ptr< M > &message) const
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
ros::Publisher pub_latest_time_
virtual ScopedWallDurationReporter reporter()
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void configCallback(Config &config, uint32_t level)
virtual void unsubscribe()
ros::Publisher pub_torus_
virtual void segmentFromPoints(const geometry_msgs::PolygonStamped::ConstPtr &polygon_msg)
ros::Publisher pub_average_time_
ros::Publisher pub_torus_array_
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
ros::Publisher pub_torus_with_failure_
bool done_initialization_
ros::Publisher pub_torus_array_with_failure_
ros::Publisher pub_coefficients_
#define NODELET_INFO(...)
virtual void segment(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
ros::Publisher pub_inliers_
ros::Publisher pub_pose_stamped_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
pcl::PointIndices PCLIndicesMsg
bool voxel_grid_sampling_
pcl::ModelCoefficients PCLModelCoefficientMsg
jsk_recognition_utils::WallDurationTimer timer_
ros::Subscriber sub_points_