void jsk_pcl_ros::PointcloudScreenpoint::point_array_cb (const sensor_msgs::PointCloud2ConstPtr& pt_arr_ptr) {
if (publish_points_) {
pcl::PointCloud<pcl::PointXY>::Ptr point_array_cloud(new pcl::PointCloud<pcl::PointXY>);
pcl::fromROSMsg(*pt_arr_ptr, *point_array_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr result_cloud(new pcl::PointCloud<pcl::PointXYZ>);
result_cloud->header = pcl_conversions::toPCL(header_); // if hydro
for (size_t i = 0; i < point_array_cloud->points.size(); i++) {
pcl::PointXY point = point_array_cloud->points[i];
geometry_msgs::PointStamped ps;
bool ret; float rx, ry, rz;
ret = extract_point (pts_, point.x, point.y, rx, ry, rz);
if (ret) {
pcl::PointXYZ point_on_screen;
point_on_screen.x = rx;
point_on_screen.y = ry;
point_on_screen.z = rz;
result_cloud->points.push_back(point_on_screen);
}
}
sensor_msgs::PointCloud2::Ptr ros_cloud(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*result_cloud, *ros_cloud);
ros_cloud->header = header_;
pub_points_.publish(ros_cloud);
}
}
void MovingLeastSquareSmoothing::smooth(const sensor_msgs::PointCloud2ConstPtr& input)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*input, *cloud);
std::vector<int> indices;
cloud->is_dense = false;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB> smoother;
smoother.setSearchRadius (search_radius_);
if (gauss_param_set_) smoother.setSqrGaussParam (gauss_param_set_);
smoother.setPolynomialFit (use_polynomial_fit_);
smoother.setPolynomialOrder (polynomial_order_);
smoother.setComputeNormals (calc_normal_);
typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new typename pcl::search::KdTree<pcl::PointXYZRGB> ());
smoother.setSearchMethod (tree);
smoother.setInputCloud (cloud);
smoother.process (*result_cloud);
sensor_msgs::PointCloud2 pointcloud2;
pcl::toROSMsg(*result_cloud, pointcloud2);
pointcloud2.header.frame_id = input->header.frame_id;
pointcloud2.header.stamp = input->header.stamp;
pub_.publish(pointcloud2);
}
//Publish model reference point cloud
void
ParticleFilterTracking::publishResult ()
{
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
//Publisher object transformation
tf::Transform tfTransformation;
tf::transformEigenToTF((Eigen::Affine3d) transformation, tfTransformation);
static tf::TransformBroadcaster tfBroadcaster;
tfBroadcaster.sendTransform(tf::StampedTransform(tfTransformation, ros::Time::now(), frame_id_, "tracker_result"));
//move close to camera a little for better visualization
transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr result_cloud (new pcl::PointCloud<pcl::PointXYZRGBA> ());
pcl::transformPointCloud<pcl::PointXYZRGBA> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
//Publish model reference point cloud
sensor_msgs::PointCloud2 result_pointcloud2;
pcl::toROSMsg(*result_cloud, result_pointcloud2);
result_pointcloud2.header.frame_id = frame_id_;
track_result_publisher_.publish(result_pointcloud2);
}
void FeatureRegistration::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_cloud_ || !reference_feature_) {
JSK_NODELET_ERROR("Not yet reference data is available");
return;
}
pcl::PointCloud<pcl::PointNormal>::Ptr
cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointNormal>::Ptr
object_aligned (new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr
feature (new pcl::PointCloud<pcl::FPFHSignature33>);
pcl::fromROSMsg(*cloud_msg, *cloud);
pcl::fromROSMsg(*feature_msg, *feature);
pcl::SampleConsensusPrerejective<pcl::PointNormal,
pcl::PointNormal,
pcl::FPFHSignature33> align;
align.setInputSource(reference_cloud_);
align.setSourceFeatures(reference_feature_);
align.setInputTarget(cloud);
align.setTargetFeatures(feature);
align.setMaximumIterations(max_iterations_); // Number of RANSAC iterations
align.setNumberOfSamples(3); // Number of points to sample for generating/prerejecting a pose
align.setCorrespondenceRandomness(correspondence_randomness_); // Number of nearest features to use
align.setSimilarityThreshold(similarity_threshold_); // Polygonal edge length similarity threshold
align.setMaxCorrespondenceDistance(max_correspondence_distance_); // Inlier threshold
align.setInlierFraction(inlier_fraction_); // Required inlier fraction for accepting a pose hypothesis
align.align (*object_aligned);
if (align.hasConverged ())
{
// Print results
printf ("\n");
Eigen::Affine3f transformation(align.getFinalTransformation());
geometry_msgs::PoseStamped ros_pose;
tf::poseEigenToMsg(transformation, ros_pose.pose);
ros_pose.header = cloud_msg->header;
pub_pose_.publish(ros_pose);
pcl::PointCloud<pcl::PointNormal>::Ptr
result_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloud(
*reference_cloud_, *result_cloud, transformation);
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*object_aligned, ros_cloud);
ros_cloud.header = cloud_msg->header;
pub_cloud_.publish(ros_cloud);
//pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
}
else {
JSK_NODELET_WARN("failed to align pointcloud");
}
}
