35 #define BOOST_PARAMETER_MAX_ARITY 7 38 #include <pcl/sample_consensus/method_types.h> 39 #include <pcl/sample_consensus/model_types.h> 40 #include <pcl/segmentation/sac_segmentation.h> 41 #include <pcl/filters/project_inliers.h> 42 #include <pcl/filters/extract_indices.h> 43 #include <pcl/features/normal_3d_omp.h> 44 #include <visualization_msgs/Marker.h> 45 #include <geometry_msgs/PoseStamped.h> 51 DiagnosticNodelet::onInit();
52 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
53 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
55 srv_->setCallback (f);
60 *
pnh_,
"debug/line_filtered_indices", 1);
62 *
pnh_,
"debug/line_filtered_normal", 1);
64 *
pnh_,
"debug/cylinder_marker", 1);
66 *
pnh_,
"output/cylinder_pose", 1);
68 *
pnh_,
"output/inliers", 1);
70 *
pnh_,
"output/coefficients", 1);
80 sync_ = boost::make_shared<message_filters::Synchronizer<ASyncPolicy> >(100);
97 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
112 const geometry_msgs::PolygonStamped::ConstPtr& hint_msg)
119 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
141 const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg,
142 const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg,
143 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
150 pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud 151 (
new pcl::PointCloud<pcl::PointXYZ>);
154 Eigen::Vector3f
a,
b;
156 pcl::PointIndices::Ptr candidate_indices
157 (
new pcl::PointIndices);
160 pcl::PointCloud<pcl::Normal>::Ptr normals
161 (
new pcl::PointCloud<pcl::Normal>);
162 pcl::PointCloud<pcl::PointXYZ>::Ptr normals_cloud
163 (
new pcl::PointCloud<pcl::PointXYZ>);
165 normalEstimate(cloud, candidate_indices, *normals, *normals_cloud);
169 pcl::PointCloud<pcl::Normal>::Ptr all_normals
170 (
new pcl::PointCloud<pcl::Normal>);
172 pcl::ExtractIndices<pcl::PointXYZ> xyz_extract;
173 xyz_extract.setInputCloud(cloud);
174 xyz_extract.setIndices(candidate_indices);
175 xyz_extract.filter(*normals_cloud);
177 pcl::ExtractIndices<pcl::Normal> normal_extract;
178 normal_extract.setInputCloud(all_normals);
179 normal_extract.setIndices(candidate_indices);
180 normal_extract.filter(*normals);
184 NODELET_WARN(
"detection time: %f", (end_time - start_time).toSec());
188 const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
189 const pcl::PointIndices::Ptr indices,
190 pcl::PointCloud<pcl::Normal>& normals,
191 pcl::PointCloud<pcl::PointXYZ>& normals_cloud)
193 pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
194 ne.setInputCloud(cloud);
195 ne.setIndices(indices);
196 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree
197 (
new pcl::search::KdTree<pcl::PointXYZ> ());
198 ne.setSearchMethod(tree);
199 ne.setRadiusSearch (0.03);
200 ne.compute (normals);
201 pcl::ExtractIndices<pcl::PointXYZ>
ex;
202 ex.setInputCloud(cloud);
203 ex.setIndices(indices);
204 ex.filter(normals_cloud);
209 const Eigen::Vector3f& a,
210 const Eigen::Vector3f& b)
212 Eigen::Vector3f hint_dir((b - a));
214 hint_dir.normalize();
215 Eigen::Vector3f cylinder_dir(cylinder->getDirection());
217 cylinder_dir.normalize();
218 double ang =
acos(cylinder_dir.dot(hint_dir));
224 const pcl::PointCloud<pcl::PointXYZ>::Ptr& filtered_cloud,
225 const pcl::PointCloud<pcl::Normal>::Ptr& cloud_normals,
226 const Eigen::Vector3f& a,
227 const Eigen::Vector3f& b)
229 pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
230 pcl::PointIndices::Ptr inliers (
new pcl::PointIndices);
231 pcl::ModelCoefficients::Ptr
coefficients(
new pcl::ModelCoefficients);
232 Eigen::Vector3f normal = (a - b).
normalized();
233 seg.setOptimizeCoefficients (
true);
234 seg.setModelType (pcl::SACMODEL_CYLINDER);
235 seg.setMethodType (pcl::SAC_RANSAC);
238 seg.setNormalDistanceWeight (0.1);
243 seg.setInputCloud(filtered_cloud);
244 seg.setInputNormals(cloud_normals);
246 seg.segment(*inliers, *coefficients);
248 Eigen::Vector3f dir(coefficients->values[3],
249 coefficients->values[4],
250 coefficients->values[5]);
251 if (dir.dot(Eigen::Vector3f(0, -1, 0)) < 0) {
255 coefficients->values[0],
256 coefficients->values[1],
257 coefficients->values[2]),
259 coefficients->values[6]));
260 pcl::PointIndices::Ptr cylinder_indices
261 (
new pcl::PointIndices);
262 cylinder->filterPointCloud(*filtered_cloud,
266 Eigen::Vector3f center;
267 cylinder->estimateCenterAndHeight(
268 *filtered_cloud, *cylinder_indices,
271 Eigen::Vector3f uz = Eigen::Vector3f(
274 visualization_msgs::Marker
marker;
275 cylinder->toMarker(marker, center, uz, height);
278 geometry_msgs::PoseStamped
pose;
279 pose.header = marker.header;
280 pose.pose = marker.pose;
288 ros_coefficients.values.push_back(center[0]);
289 ros_coefficients.values.push_back(center[1]);
290 ros_coefficients.values.push_back(center[2]);
291 ros_coefficients.values.push_back(dir[0]);
292 ros_coefficients.values.push_back(dir[1]);
293 ros_coefficients.values.push_back(dir[2]);
294 ros_coefficients.values.push_back(coefficients->values[6]);
295 ros_coefficients.values.push_back(height);
300 NODELET_WARN(
"failed to detect cylinder [%lu/%d]", i, cylinder_fitting_trial_);
305 const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
307 pcl::PointIndices& output_indices)
309 output_indices.indices.clear();
310 for (
size_t i = 0; i < cloud->points.size(); i++) {
311 pcl::PointXYZ
p = cloud->points[i];
312 if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
313 if (polygon->isProjectableInside(p.getVector3fMap())) {
315 output_indices.indices.push_back(i);
320 output_indices.header = cloud->header;
328 const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg,
333 cv::Point2d point_a(polygon_msg->polygon.points[0].x,
334 polygon_msg->polygon.points[0].y);
335 cv::Point2d point_b(polygon_msg->polygon.points[1].x,
336 polygon_msg->polygon.points[1].y);
339 a = Eigen::Vector3f(ray_a.x, ray_a.y, ray_a.z);
340 b = Eigen::Vector3f(ray_b.x, ray_b.y, ray_b.z);
342 Eigen::Vector3f far_a = 20.0 * a;
343 Eigen::Vector3f far_b = 20.0 * b;
344 Eigen::Vector3f O(0, 0, 0);
346 vertices.push_back(O);
347 vertices.push_back(far_a);
348 vertices.push_back(far_b);
virtual void hintCallback(const geometry_msgs::PolygonStamped::ConstPtr &hint_msg)
Non synchronized message callback for ~input/hint/line.
virtual void normalEstimate(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::PointIndices::Ptr indices, pcl::PointCloud< pcl::Normal > &normals, pcl::PointCloud< pcl::PointXYZ > &normals_cloud)
bool use_normal_
True if use ~input has normal fields.
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
ros::Publisher pub_line_filtered_indices_
geometry_msgs::PolygonStamped::ConstPtr latest_hint_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
Non synchronized message callback for ~input pointcloud.
ros::Subscriber sub_no_sync_camera_info_
double outlier_threshold_
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
ros::Publisher pub_line_filtered_normal_
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
Non synchronized message callback for ~input/camera_info.
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
TFSIMD_FORCE_INLINE Vector3 normalized() const
boost::shared_ptr< message_filters::Synchronizer< ASyncPolicy > > sync_
virtual void unsubscribe()
ros::Publisher pub_coefficients_
virtual void fittingCylinder(const pcl::PointCloud< pcl::PointXYZ >::Ptr &filtered_cloud, const pcl::PointCloud< pcl::Normal >::Ptr &cloud_nromals, const Eigen::Vector3f &a, const Eigen::Vector3f &b)
#define NODELET_WARN_THROTTLE(rate,...)
HintedStickFinderConfig Config
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
virtual jsk_recognition_utils::ConvexPolygon::Ptr polygonFromLine(const geometry_msgs::PolygonStamped::ConstPtr &polygon_msg, const image_geometry::PinholeCameraModel &model, Eigen::Vector3f &a, Eigen::Vector3f &b)
sensor_msgs::CameraInfo::ConstPtr latest_camera_info_
bool not_synchronize_
Run in continuous mode. continuous mode means this nodelet does not synchronize hint and input messag...
message_filters::Subscriber< geometry_msgs::PolygonStamped > sub_polygon_
virtual void filterPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const jsk_recognition_utils::ConvexPolygon::Ptr polygon, pcl::PointIndices &output_indices)
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
ros::Subscriber sub_no_sync_cloud_
#define NODELET_INFO(...)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
int cylinder_fitting_trial_
virtual void configCallback(Config &config, uint32_t level)
ros::Publisher pub_inliers_
virtual void detect(const geometry_msgs::PolygonStamped::ConstPtr &polygon_msg, const sensor_msgs::CameraInfo::ConstPtr &camera_info_msg, const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
Synchronized message callback.
ros::Subscriber sub_no_sync_polygon_
ros::Publisher pub_cylinder_marker_
pcl::PointIndices PCLIndicesMsg
pcl::ModelCoefficients PCLModelCoefficientMsg
virtual bool rejected2DHint(const jsk_recognition_utils::Cylinder::Ptr &cylinder, const Eigen::Vector3f &a, const Eigen::Vector3f &b)
Check direction of cylinder in 2-D image coordinate system and if it is larger than eps_2d_angle_...
std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::HintedStickFinder, nodelet::Nodelet)
ros::Publisher pub_cylinder_pose_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_