void CloudAssembler::addToBuffer(sensor_msgs::PointCloud2 cloud) { //ROS_INFO("Adding cloud %f to buffer. Current buffer length is %d",::frame_count-1, cloud_buffer_.size()); if (cloud_buffer_.size() >= (unsigned int)buffer_length_) { sensor_msgs::PointCloud2 cloud_msg; toROSMsg(assembled_PointCloudXYZ, cloud_msg); output_pub_.publish(cloud_msg); ROS_INFO("publish assembled cloud at frame %f ",::frame_count-1); cloud_buffer_.erase(cloud_buffer_.begin(),cloud_buffer_.end()); } PointCloudXYZ temp_cloud; fromROSMsg(cloud, temp_cloud); for (int j=0 ; j<temp_cloud.points.size();j++){ temp_cloud.points[j].z=::frame_count/18; } toROSMsg(temp_cloud, cloud); cloud_buffer_.push_back(cloud); }
void CloudAssembler::cloudCallback(const sensor_msgs::PointCloud2& cloud) { addToBuffer(cloud); assembleCloud(); sensor_msgs::PointCloud2 cloud_msg; toROSMsg(assembled_PointCloudXYZ, cloud_msg); cloud_msg.header.frame_id = cloud.header.frame_id; cloud_msg.header.stamp = ros::Time::now(); }
void ColorFilter<PackedComparison, Config>::filter(const sensor_msgs::PointCloud2ConstPtr &input, const PCLIndicesMsg::ConstPtr& indices) { boost::mutex::scoped_lock lock (mutex_); pcl::PointCloud<pcl::PointXYZRGB> tmp_in, tmp_out; sensor_msgs::PointCloud2 out; fromROSMsg(*input, tmp_in); filter_instance_.setInputCloud (tmp_in.makeShared()); if (indices) { pcl::IndicesPtr vindices; vindices.reset(new std::vector<int> (indices->indices)); filter_instance_.setIndices(vindices); } filter_instance_.filter(tmp_out); if (tmp_out.points.size() > 0) { toROSMsg(tmp_out, out); pub_.publish(out); } }
int main(int argc, char **argv) { cob_3d_mapping::Polygon p1; Eigen::Vector3f v; std::vector<Eigen::Vector3f> vv; p1.id = 1; p1.normal << 0.0,0.0,1.0; p1.d = -1; v << 1,-2,1; vv.push_back(v); v << 1,-3,1; vv.push_back(v); v << 2,-3,1; vv.push_back(v); v << 2,-2,1; vv.push_back(v); p1.contours.push_back(vv); p1.holes.push_back(false); cob_3d_mapping_msgs::Shape p_msg; toROSMsg(p1, p_msg); ros::init (argc, argv, "client"); ros::NodeHandle nh; ros::ServiceClient client = nh.serviceClient<cob_3d_mapping_msgs::GetApproachPoseForPolygon> ("compute_approach_pose"); cob_3d_mapping_msgs::GetApproachPoseForPolygonRequest req; req.polygon = p_msg; cob_3d_mapping_msgs::GetApproachPoseForPolygonResponse res; client.call (req, res); for (unsigned int i=0; i<res.approach_poses.poses.size(); i++) { tf::Quaternion q; tf::quaternionMsgToTF(res.approach_poses.poses[i].orientation, q); std::cout << "i=" << i << " x=" << res.approach_poses.poses[i].position.x << " y=" << res.approach_poses.poses[i].position.y << " ang=" << tf::getYaw(q) << std::endl; } return 0; }
void VoxelGridDownsampleManager::pointCB(const sensor_msgs::PointCloud2ConstPtr &input) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>); if (grid_.size() == 0) { ROS_INFO("the number of registered grids is 0, skipping"); return; } fromROSMsg(*input, *cloud); for (size_t i = 0; i < grid_.size(); i++) { visualization_msgs::Marker::ConstPtr target_grid = grid_[i]; // not yet tf is supported int id = target_grid->id; // solve tf with ros::Time 0.0 // transform pointcloud to the frame_id of target_grid pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl_ros::transformPointCloud(target_grid->header.frame_id, *cloud, *transformed_cloud, tf_listener); double center_x = target_grid->pose.position.x; double center_y = target_grid->pose.position.y; double center_z = target_grid->pose.position.z; double range_x = target_grid->scale.x * 1.0; // for debug double range_y = target_grid->scale.y * 1.0; double range_z = target_grid->scale.z * 1.0; double min_x = center_x - range_x / 2.0; double max_x = center_x + range_x / 2.0; double min_y = center_y - range_y / 2.0; double max_y = center_y + range_y / 2.0; double min_z = center_z - range_z / 2.0; double max_z = center_z + range_z / 2.0; double resolution = target_grid->color.r; // filter order: x -> y -> z -> downsample pcl::PassThrough<pcl::PointXYZ> pass_x; pass_x.setFilterFieldName("x"); pass_x.setFilterLimits(min_x, max_x); pcl::PassThrough<pcl::PointXYZ> pass_y; pass_y.setFilterFieldName("y"); pass_y.setFilterLimits(min_y, max_y); pcl::PassThrough<pcl::PointXYZ> pass_z; pass_z.setFilterFieldName("z"); pass_z.setFilterLimits(min_z, max_z); ROS_INFO_STREAM(id << " filter x: " << min_x << " - " << max_x); ROS_INFO_STREAM(id << " filter y: " << min_y << " - " << max_y); ROS_INFO_STREAM(id << " filter z: " << min_z << " - " << max_z); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_x (new pcl::PointCloud<pcl::PointXYZ>); pass_x.setInputCloud (transformed_cloud); pass_x.filter(*cloud_after_x); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_y (new pcl::PointCloud<pcl::PointXYZ>); pass_y.setInputCloud (cloud_after_x); pass_y.filter(*cloud_after_y); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_z (new pcl::PointCloud<pcl::PointXYZ>); pass_z.setInputCloud (cloud_after_y); pass_z.filter(*cloud_after_z); // downsample pcl::VoxelGrid<pcl::PointXYZ> sor; sor.setInputCloud (cloud_after_z); sor.setLeafSize (resolution, resolution, resolution); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); sor.filter (*cloud_filtered); // reverse transform pcl::PointCloud<pcl::PointXYZ>::Ptr reverse_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl_ros::transformPointCloud(input->header.frame_id, *cloud_filtered, *reverse_transformed_cloud, tf_listener); // adding the output into *output_cloud // tmp <- cloud_filtered + output_cloud // output_cloud <- tmp //pcl::PointCloud<pcl::PointXYZ>::Ptr tmp (new pcl::PointCloud<pcl::PointXYZ>); //pcl::concatenatePointCloud (*cloud_filtered, *output_cloud, tmp); //output_cloud = tmp; ROS_INFO_STREAM(id << " includes " << reverse_transformed_cloud->points.size() << " points"); for (size_t i = 0; i < reverse_transformed_cloud->points.size(); i++) { output_cloud->points.push_back(reverse_transformed_cloud->points[i]); } } sensor_msgs::PointCloud2 out; toROSMsg(*output_cloud, out); out.header = input->header; pub_.publish(out); // for debugging // for concatenater size_t cluster_num = output_cloud->points.size() / max_points_ + 1; ROS_INFO_STREAM("encoding into " << cluster_num << " clusters"); for (size_t i = 0; i < cluster_num; i++) { size_t start_index = max_points_ * i; size_t end_index = max_points_ * (i + 1) > output_cloud->points.size() ? output_cloud->points.size(): max_points_ * (i + 1); sensor_msgs::PointCloud2 cluster_out_ros; pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_out_pcl(new pcl::PointCloud<pcl::PointXYZ>); cluster_out_pcl->points.resize(end_index - start_index); // build cluster_out_pcl ROS_INFO_STREAM("make cluster from " << start_index << " to " << end_index); for (size_t j = start_index; j < end_index; j++) { cluster_out_pcl->points[j - start_index] = output_cloud->points[j]; } // conevrt cluster_out_pcl into ros msg toROSMsg(*cluster_out_pcl, cluster_out_ros); cluster_out_ros.header = input->header; cluster_out_ros.header.frame_id = (boost::format("%s %d %d") % (cluster_out_ros.header.frame_id) % (i) % (sequence_id_)).str(); pub_encoded_.publish(cluster_out_ros); ros::Duration(1.0 / rate_).sleep(); } }
void pointcloud_incoming (const sensor_msgs::PointCloud2ConstPtr& input) { ros::NodeHandle nh("~"); // // retrieve all parameter variables from server or set to a default value // nh.param<double>("cloud_ss" , cloud_ss_ , 0.01 ); nh.param<double>("descr_rad", descr_rad_, 0.02 ); nh.param<string>("output_frame", output_frame, "pcd_frame"); // // create all neccessary objects // output_keypoints = KeypointMsg::Ptr (new KeypointMsg); output_descriptors_shot352 = Shot352Msg ::Ptr (new Shot352Msg ); output_descriptors_shot1344= Shot1344Msg::Ptr (new Shot1344Msg); cloud = PointCloud::Ptr (new PointCloud ()); cloud_keypoints = PointCloud::Ptr (new PointCloud ()); cloud_normals = NormalCloud::Ptr (new NormalCloud ()); cloud_descriptors_shot352 = DescriptorCloudShot352::Ptr (new DescriptorCloudShot352()); cloud_descriptors_shot1344= DescriptorCloudShot1344::Ptr(new DescriptorCloudShot1344()); // // convert ROS message to PCL message // fromROSMsg(*input, *cloud); // // compute normals // cout << "... computing normals from cloud ..." << endl; norm_est_cloud.setKSearch (10); norm_est_cloud.setInputCloud (cloud); norm_est_cloud.compute (*cloud_normals); // // Downsample cloud to extract keypoints // cout << "... downsampling cloud ..." << endl; uniform_sampling_cloud.setInputCloud (cloud); uniform_sampling_cloud.setRadiusSearch (cloud_ss_); uniform_sampling_cloud.compute (sampled_indices_cloud); pcl::copyPointCloud (*cloud, sampled_indices_cloud.points, *cloud_keypoints); std::cout << "Cloud total points: " << cloud->size () << "; Selected Keypoints: " << cloud_keypoints->size () << std::endl; // // Extract descriptors // cout << "... extracting descriptors from cloud ..." << endl; descr_est_shot352.setInputCloud (cloud_keypoints); descr_est_shot352.setRadiusSearch (descr_rad_); descr_est_shot352.setInputNormals (cloud_normals); descr_est_shot352.setSearchSurface (cloud); descr_est_shot352.compute (*cloud_descriptors_shot352); descr_est_shot1344.setInputCloud (cloud_keypoints); descr_est_shot1344.setRadiusSearch (descr_rad_); descr_est_shot1344.setInputNormals (cloud_normals); descr_est_shot1344.setSearchSurface (cloud); descr_est_shot1344.compute (*cloud_descriptors_shot1344); // // Convert to ROS message // pcl::toROSMsg(*cloud_keypoints, *output_keypoints); toROSMsg(*cloud_descriptors_shot352, *output_descriptors_shot352); toROSMsg(*cloud_descriptors_shot1344, *output_descriptors_shot1344); output_keypoints->header.frame_id = output_frame; // check if anyone subscribed to keypoints if (pub_keypoints.getNumSubscribers() == 0) { ROS_WARN("No keypoint subscriber found"); while (pub_keypoints.getNumSubscribers() == 0 && ros::ok()) { ros::Duration(1).sleep(); } ROS_WARN("Subscriber now available, wait another 1 second"); // give some time to set up connection ros::Duration(1).sleep(); } // // Publish Keypoints and Descriptors // pub_keypoints.publish(*output_keypoints); pub_descriptors_Shot352.publish(*output_descriptors_shot352); pub_descriptors_Shot1344.publish(*output_descriptors_shot1344); }
void OctreeVoxelGrid::generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr& input_msg) { typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ()); typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (new pcl::PointCloud<PointT> ()); pcl::fromROSMsg(*input_msg, *cloud); // generate octree typename pcl::octree::OctreePointCloud<PointT> octree(resolution_); // add point cloud to octree octree.setInputCloud(cloud); octree.addPointsFromInputCloud(); // get points where grid is occupied typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec; octree.getOccupiedVoxelCenters(point_vec); // put points into point cloud cloud_voxeled->width = point_vec.size(); cloud_voxeled->height = 1; for (int i = 0; i < point_vec.size(); i++) { cloud_voxeled->push_back(point_vec[i]); } // publish point cloud sensor_msgs::PointCloud2 output_msg; toROSMsg(*cloud_voxeled, output_msg); output_msg.header = input_msg->header; pub_cloud_.publish(output_msg); // publish marker if (publish_marker_flag_) { visualization_msgs::Marker marker_msg; marker_msg.type = visualization_msgs::Marker::CUBE_LIST; marker_msg.scale.x = resolution_; marker_msg.scale.y = resolution_; marker_msg.scale.z = resolution_; marker_msg.header = input_msg->header; marker_msg.pose.orientation.w = 1.0; if (marker_color_ == "flat") { marker_msg.color = jsk_topic_tools::colorCategory20(0); marker_msg.color.a = marker_color_alpha_; } // compute min and max Eigen::Vector4f minpt, maxpt; pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt); PointT p; for (size_t i = 0; i < cloud_voxeled->size(); i++) { p = cloud_voxeled->at(i); geometry_msgs::Point point_ros; point_ros.x = p.x; point_ros.y = p.y; point_ros.z = p.z; marker_msg.points.push_back(point_ros); if (marker_color_ == "flat") { marker_msg.colors.push_back(jsk_topic_tools::colorCategory20(0)); } else if (marker_color_ == "z") { marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.z - minpt[2]) / (maxpt[2] - minpt[2]))); } else if (marker_color_ == "x") { marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.x - minpt[0]) / (maxpt[0] - minpt[0]))); } else if (marker_color_ == "y") { marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.y - minpt[1]) / (maxpt[1] - minpt[1]))); } marker_msg.colors[marker_msg.colors.size() - 1].a = marker_color_alpha_; } pub_marker_.publish(marker_msg); } }
void spin() { std::string robot_description_str; std::string robot_semantic_description_str; nh_.getParam("/robot_description", robot_description_str); nh_.getParam("/robot_semantic_description", robot_semantic_description_str); // // collision model // boost::shared_ptr<self_collision::CollisionModel> col_model = self_collision::CollisionModel::parseURDF(robot_description_str); col_model->parseSRDF(robot_semantic_description_str); col_model->generateCollisionPairs(); // set colors for each link // for (int l_idx = 0; l_idx < col_model->getLinksCount(); l_idx++) { // for (self_collision::Link::VecPtrCollision::iterator it = col_model->getLink(l_idx)->collision_array.begin(); it != col_model->getLink(l_idx)->collision_array.end(); it++) { // (*it)->geometry->setColor(0,1,0,0.5); // } // } boost::shared_ptr< self_collision::Collision > shpere = self_collision::createCollisionSphere(tolerance_, KDL::Frame()); ros::Rate loop_rate(5); int errors = 0; while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); if (errors > 5) { point_cloud_processed_ = true; } if (!point_cloud_processed_) { tf::StampedTransform tf_W_C; try { tf_listener_.lookupTransform("world", pc_frame_id_, pc_stamp_, tf_W_C); } catch(tf::TransformException& ex){ ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback"); errors++; continue; } geometry_msgs::TransformStamped tfm_W_C; KDL::Frame T_W_C; tf::transformStampedTFToMsg(tf_W_C, tfm_W_C); tf::transformMsgToKDL(tfm_W_C.transform, T_W_C); std::vector<KDL::Frame > links_tf(col_model->getLinksCount()); std::set<std::string > colliding_links; std::vector<bool > pt_col(pc_.points.size(), false); bool tf_error = false; for (int l_idx = 0; l_idx < col_model->getLinksCount(); l_idx++) { const boost::shared_ptr< self_collision::Link > plink = col_model->getLink(l_idx); if (plink->collision_array.size() == 0) { continue; } tf::StampedTransform tf_W_L; try { tf_listener_.lookupTransform("world", plink->name, pc_stamp_, tf_W_L); } catch(tf::TransformException& ex){ ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback"); tf_error = true; break; } geometry_msgs::TransformStamped tfm_W_L; KDL::Frame T_W_L; tf::transformStampedTFToMsg(tf_W_L, tfm_W_L); tf::transformMsgToKDL(tfm_W_L.transform, T_W_L); KDL::Frame T_C_L = T_W_C.Inverse() * T_W_L; links_tf[l_idx] = T_W_L; for (int pidx = 0; pidx < pc_.points.size(); pidx++) { if (pt_col[pidx]) { continue; } KDL::Frame T_C_P(KDL::Vector(pc_.points[pidx].x, pc_.points[pidx].y, pc_.points[pidx].z)); if (self_collision::checkCollision(shpere, T_C_P, plink, T_C_L)) { pt_col[pidx] = true; colliding_links.insert( plink->name ); } } } if (tf_error) { errors++; continue; } PclPointCloud pc_out; for (int pidx = 0; pidx < pc_.points.size(); pidx++) { if (pt_col[pidx]) { // pcl::PointXYZRGB pt = pc_.points[pidx]; // pt.r = 0; // pt.g = 0; // pt.b = 1; // pc_out.push_back( pt ); continue; } pc_out.push_back( pc_.points[pidx] ); } sensor_msgs::PointCloud2 ros_pc_out; toROSMsg(pc_out, ros_pc_out); ros_pc_out.header.stamp = pc_stamp_; ros_pc_out.header.frame_id = pc_frame_id_; // std::cout << pc_frame_id_ << " " << errors << std::endl; // for (std::set<std::string >::const_iterator it = colliding_links.begin(); it != colliding_links.end(); it++) { // std::cout << (*it) << " "; // } // std::cout << std::endl; pub_pc_.publish(ros_pc_out); point_cloud_processed_ = true; errors = 0; // addRobotModelVis(markers_pub_, 0, col_model, links_tf); // markers_pub_.publish(); } } }
template<class T> void filter (const sensor_msgs::PointCloud2::ConstPtr &input, const PCLIndicesMsg::ConstPtr &indices) { pcl::PointCloud<T> pcl_input_cloud, output; fromROSMsg(*input, pcl_input_cloud); boost::mutex::scoped_lock lock (mutex_); std::vector<int> ex_indices; ex_indices.resize(0); int width = input->width; int height = input->height; int ox, oy, sx, sy; sx = step_x_; ox = sx/2; if(height == 1) { sy = 1; oy = 0; } else { sy = step_y_; oy = sy/2; } if (indices) { std::vector<int> flags; flags.resize(width*height); //std::vector<int>::iterator it; //for(it = indices->begin(); it != indices->end(); it++) //flags[*it] = 1; for(unsigned int i = 0; i < indices->indices.size(); i++) { flags[indices->indices.at(i)] = 1; } for(int y = oy; y < height; y += sy) { for(int x = ox; x < width; x += sx) { if (flags[y*width + x] == 1) { ex_indices.push_back(y*width + x); // just use points in indices } } } } else { for(int y = oy; y < height; y += sy) { for(int x = ox; x < width; x += sx) { ex_indices.push_back(y*width + x); } } } pcl::ExtractIndices<T> extract; extract.setInputCloud (pcl_input_cloud.makeShared()); extract.setIndices (boost::make_shared <std::vector<int> > (ex_indices)); extract.setNegative (false); extract.filter (output); if (output.points.size() > 0) { sensor_msgs::PointCloud2 ros_out; toROSMsg(output, ros_out); ros_out.header = input->header; ros_out.width = (width - ox)/sx; if((width - ox)%sx) ros_out.width += 1; ros_out.height = (height - oy)/sy; if((height - oy)%sy) ros_out.height += 1; ros_out.row_step = ros_out.point_step * ros_out.width; ros_out.is_dense = input->is_dense; #if DEBUG JSK_NODELET_INFO("%dx%d (%d %d)(%d %d) -> %dx%d %d", width,height, ox, oy, sx, sy, ros_out.width, ros_out.height, ex_indices.size()); #endif pub_.publish(ros_out); JSK_NODELET_INFO("%s:: input header stamp is [%f]", getName().c_str(), input->header.stamp.toSec()); JSK_NODELET_INFO("%s:: output header stamp is [%f]", getName().c_str(), ros_out.header.stamp.toSec()); } }
void cluster(const pcl::CorrespondencesPtr &object_world_corrs) { ros::NodeHandle nh_param("~"); nh_param.param<double>("cg_size" , cg_size_ , 0.01 ); nh_param.param<double>("cg_thresh", cg_thresh_, 5.0); // // Debug output // PointCloud correspondence_object; PointCloud correspondence_world; for (int j = 0; j < object_world_corrs->size (); ++j) { PointType& object_point = object_keypoints->at(object_world_corrs->at(j).index_query); PointType& world_point = world_keypoints->at(object_world_corrs->at(j).index_match); correspondence_object.push_back(object_point); correspondence_world.push_back(world_point); // cout << object_point.x << " " << object_point.y << " " << object_point.z << endl; // cout << world_point.x << " " << world_point.y << " " << world_point.z << endl; } PointCloudROS pub_me_object2; PointCloudROS pub_me_world2; toROSMsg(correspondence_object, pub_me_object2); toROSMsg(correspondence_world, pub_me_world2); pub_me_object2.header.frame_id = "/object"; pub_me_world2.header.frame_id = "/world"; pub_object2.publish(pub_me_object2); pub_world2.publish(pub_me_world2); // // Actual Clustering // cout << "... clustering ..." << endl; std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations; std::vector<pcl::Correspondences> clustered_corrs; pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer; gc_clusterer.setGCSize (cg_size_); gc_clusterer.setGCThreshold (cg_thresh_); gc_clusterer.setInputCloud (object_keypoints); gc_clusterer.setSceneCloud (world_keypoints); gc_clusterer.setModelSceneCorrespondences (object_world_corrs); gc_clusterer.recognize (rototranslations, clustered_corrs); // // Output results // std::cout << "Object instances found: " << rototranslations.size () << std::endl; int maximum = 0; int best; for (int i = 0; i < rototranslations.size (); ++i) { cout << "Instance "<< i << " has " << clustered_corrs[i].size () << " correspondences" << endl; if (maximum < clustered_corrs[i].size ()) { maximum = clustered_corrs[i].size (); best = i; } } if (rototranslations.size () > 0) { cout << "selecting instance " << best << " and calculating TF" << endl; // Print the rotation matrix and translation vector Eigen::Matrix3f rotation = rototranslations[best].block<3,3>(0, 0); Eigen::Vector3f translation = rototranslations[best].block<3,1>(0, 3); printf ("\n"); printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2)); printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2)); printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2)); printf ("\n"); printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2)); // convert Eigen matricies into ROS TF message tf::Vector3 object_offset; tf::Quaternion object_rotation; object_offset[0] = translation (0); object_offset[1] = translation (1); object_offset[2] = translation (2); // convert rotation matrix to quaternion Eigen::Quaternionf quaternion (rotation); object_rotation[0] = quaternion.x(); object_rotation[1] = quaternion.y(); object_rotation[2] = quaternion.z(); object_rotation[3] = quaternion.w(); static tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin (object_offset); transform.setRotation (object_rotation); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "object")); // // Debug output // PointCloud correspondence_object_cluster; PointCloud correspondence_world_cluster; for (int j = 0; j < clustered_corrs[best].size (); ++j) { PointType& model_point = object_keypoints->at(clustered_corrs[best][j].index_query); PointType& scene_point = world_keypoints->at(clustered_corrs[best][j].index_match); correspondence_object_cluster.push_back(model_point); correspondence_world_cluster.push_back(scene_point); //cout << model_point.x << " " << model_point.y << " " << model_point.z << endl; //cout << scene_point.x << " " << scene_point.y << " " << scene_point.z << endl; } PointCloudROS pub_me_object; PointCloudROS pub_me_world; toROSMsg(correspondence_object_cluster, pub_me_object); toROSMsg(correspondence_world_cluster, pub_me_world); pub_me_object.header.frame_id = "/object"; pub_me_world.header.frame_id = "/world"; pub_object.publish(pub_me_object); pub_world.publish(pub_me_world); } }
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) { // std::cout << "\n\n----------------Received point cloud!-----------------\n"; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_planar (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_red (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_green (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_blue (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_yellow (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_clusters (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_hulls (new pcl::PointCloud<pcl::PointXYZRGB>); sensor_msgs::PointCloud2 downsampled2, planar2, objects2, filtered2, red2, green2, blue2, yellow2, concat_clusters2, concat_hulls2; std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> color_clouds, cluster_clouds, hull_clouds; std::vector<pcl::PointXYZRGB> labels; // fromROSMsg(*input, *cloud); // pub_input.publish(*input); // Downsample the input PointCloud pcl::VoxelGrid<sensor_msgs::PointCloud2> sor; sor.setInputCloud (input); // sor.setLeafSize (0.01, 0.01, 0.01); //play around with leafsize (more samples, better resolution?) sor.setLeafSize (0.001, 0.001, 0.001); sor.filter (downsampled2); fromROSMsg(downsampled2,*downsampled); pub_downsampled.publish (downsampled2); // Segment the largest planar component from the downsampled cloud pcl::SACSegmentation<pcl::PointXYZRGB> seg; pcl::ModelCoefficients::Ptr coeffs (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.0085); seg.setInputCloud (downsampled); seg.segment (*inliers, *coeffs); // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud (downsampled); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_planar); // toROSMsg(*cloud_planar,planar2); // pub_planar.publish (planar2); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_objects); // toROSMsg(*cloud_objects,objects2); // pub_objects.publish (objects2); // PassThrough Filter pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud (cloud_objects); pass.setFilterFieldName ("z"); //all duplos in pcd1 pass.setFilterLimits (0.8, 1.0); pass.filter (*cloud_filtered); toROSMsg(*cloud_filtered,filtered2); pub_filtered.publish (filtered2); //don't passthrough filter, does color filter work too? (cloud_red has many points in top right off the table) // Segment filtered PointCloud by color (red, green, blue, yellow) for (size_t i = 0 ; i < cloud_filtered->points.size () ; i++) { if ( (int(cloud_filtered->points[i].r) > 2*int(cloud_filtered->points[i].g)) && (cloud_filtered->points[i].r > cloud_filtered->points[i].b) ) cloud_red->points.push_back(cloud_filtered->points[i]); if ( (cloud_filtered->points[i].g > cloud_filtered->points[i].r) && (cloud_filtered->points[i].g > cloud_filtered->points[i].b) ) cloud_green->points.push_back(cloud_filtered->points[i]); if ( (cloud_filtered->points[i].b > cloud_filtered->points[i].r) && (cloud_filtered->points[i].b > cloud_filtered->points[i].g) ) cloud_blue->points.push_back(cloud_filtered->points[i]); if ( (cloud_filtered->points[i].r > cloud_filtered->points[i].g) && (int(cloud_filtered->points[i].g) - int(cloud_filtered->points[i].b) > 30) ) cloud_yellow->points.push_back(cloud_filtered->points[i]); } cloud_red->header.frame_id = "base_link"; cloud_red->width = cloud_red->points.size (); cloud_red->height = 1; color_clouds.push_back(cloud_red); toROSMsg(*cloud_red,red2); pub_red.publish (red2); cloud_green->header.frame_id = "base_link"; cloud_green->width = cloud_green->points.size (); cloud_green->height = 1; color_clouds.push_back(cloud_green); toROSMsg(*cloud_green,green2); pub_green.publish (green2); cloud_blue->header.frame_id = "base_link"; cloud_blue->width = cloud_blue->points.size (); cloud_blue->height = 1; color_clouds.push_back(cloud_blue); toROSMsg(*cloud_blue,blue2); pub_blue.publish (blue2); cloud_yellow->header.frame_id = "base_link"; cloud_yellow->width = cloud_yellow->points.size (); cloud_yellow->height = 1; color_clouds.push_back(cloud_yellow); toROSMsg(*cloud_yellow,yellow2); pub_yellow.publish (yellow2); // Extract Euclidian clusters from color-segmented PointClouds int j(0), num_red (0), num_green(0), num_blue(0), num_yellow(0); for (size_t cit = 0 ; cit < color_clouds.size() ; cit++) { pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>); tree->setInputCloud (color_clouds[cit]); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec; ec.setClusterTolerance (0.0075); // 0.01 // ec.setMinClusterSize (12); // ec.setMaxClusterSize (75); ec.setMinClusterSize (100); ec.setMaxClusterSize (4000); ec.setSearchMethod (tree); ec.setInputCloud (color_clouds[cit]); ec.extract (cluster_indices); for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>); for (std::vector<int>::const_iterator pit = it->indices.begin () ; pit != it->indices.end () ; pit++) cloud_cluster->points.push_back (color_clouds[cit]->points[*pit]); cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; cloud_cluster->header.frame_id = "base_link"; cluster_clouds.push_back(cloud_cluster); labels.push_back(cloud_cluster->points[0]); if (cit == 0) num_red++; if (cit == 1) num_green++; if (cit == 2) num_blue++; if (cit == 3) num_yellow++; // Create ConvexHull for cluster (keep points on perimeter of cluster) pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::ConvexHull<pcl::PointXYZRGB> chull; chull.setInputCloud (cloud_cluster); chull.reconstruct (*cloud_hull); cloud_hull->width = cloud_hull->points.size (); cloud_hull->height = 1; cloud_hull->header.frame_id = "base_link"; hull_clouds.push_back(cloud_hull); j++; } } std::cout << "Number of RED clusters: " << num_red << std::endl; std::cout << "Number of GREEN clusters: " << num_green << std::endl; std::cout << "Number of BLUE clusters: " << num_blue << std::endl; std::cout << "Number of YELLOW clusters: " << num_yellow << std::endl; std::cout << "TOTAL number of clusters: " << j << std::endl; // Concatenate PointCloud clusters and convex hulls for (size_t k = 0 ; k < cluster_clouds.size() ; k++) { for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++) { cloud_concat_clusters->points.push_back(cluster_clouds[k]->points[l]); cloud_concat_hulls->points.push_back(hull_clouds[k]->points[l]); } } cloud_concat_clusters->header.frame_id = "base_link"; cloud_concat_clusters->width = cloud_concat_clusters->points.size (); cloud_concat_clusters->height = 1; toROSMsg(*cloud_concat_clusters,concat_clusters2); pub_concat_clusters.publish (concat_clusters2); cloud_concat_hulls->header.frame_id = "base_link"; cloud_concat_hulls->width = cloud_concat_hulls->points.size (); cloud_concat_hulls->height = 1; toROSMsg(*cloud_concat_hulls,concat_hulls2); pub_concat_hulls.publish (concat_hulls2); // Estimate the volume of each cluster double height, width; std::vector <double> heights, widths; std::vector <int> height_ids, width_ids; for (size_t k = 0 ; k < cluster_clouds.size() ; k++) { // Calculate cluster height double tallest(0), shortest(1000), widest(0) ; for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++) { double point_to_plane_dist; point_to_plane_dist = coeffs->values[0] * cluster_clouds[k]->points[l].x + coeffs->values[1] * cluster_clouds[k]->points[l].y + coeffs->values[2] * cluster_clouds[k]->points[l].z + coeffs->values[3] / sqrt( pow(coeffs->values[0], 2) + pow(coeffs->values[1], 2)+ pow(coeffs->values[2], 2) ); if (point_to_plane_dist < 0) point_to_plane_dist = 0; if (point_to_plane_dist > tallest) tallest = point_to_plane_dist; if (point_to_plane_dist < shortest) shortest = point_to_plane_dist; } // Calculate cluster width for (size_t m = 0 ; m < hull_clouds[k]->size() ; m++) { double parallel_vector_dist; for (size_t n = m ; n < hull_clouds[k]->size()-1 ; n++) { parallel_vector_dist = sqrt( pow(hull_clouds[k]->points[m].x - hull_clouds[k]->points[n+1].x,2) + pow(hull_clouds[k]->points[m].y - hull_clouds[k]->points[n+1].y,2) + pow(hull_clouds[k]->points[m].z - hull_clouds[k]->points[n+1].z,2) ); if (parallel_vector_dist > widest) widest = parallel_vector_dist; } } // Classify block heights (error +/- .005m) height = (shortest < .01) ? tallest : tallest - shortest; //check for stacked blocks heights.push_back(height); if (height > .020 && height < .032) height_ids.push_back(0); //0: standing flat else if (height > .036 && height < .043) height_ids.push_back(1); //1: standing side else if (height > .064) height_ids.push_back(2); //2: standing long else height_ids.push_back(-1); //height not classified // Classify block widths (error +/- .005m) width = widest; widths.push_back(widest); if (width > .032-.005 && width < .0515+.005) width_ids.push_back(1); //1: short else if (width > .065-.005 && width < .0763+.005) width_ids.push_back(2); //2: medium else if (width > .1275-.005 && width < .1554+.005) width_ids.push_back(4); //4: long else width_ids.push_back(-1); //width not classified } // Classify block size using width information std::vector<int> block_ids, idx_1x1, idx_1x2, idx_1x4, idx_unclassified; int num_1x1(0), num_1x2(0), num_1x4(0), num_unclassified(0); for (size_t p = 0 ; p < width_ids.size() ; p++) { if (width_ids[p] == 1) { block_ids.push_back(1); //block is 1x1 idx_1x1.push_back(p); num_1x1++; } else if (width_ids[p] == 2) { block_ids.push_back(2); //block is 1x2 idx_1x2.push_back(p); num_1x2++; } else if (width_ids[p] == 4) { block_ids.push_back(4); //block is 1x4 idx_1x4.push_back(p); num_1x4++; } else { block_ids.push_back(-1); //block not classified idx_unclassified.push_back(p); num_unclassified++; } } // Determine Duplos of the same size std::cout << "\nThere are " << num_1x1 << " blocks of size 1x1 "; if (num_1x1>0) std::cout << "(cluster index: "; for (size_t q = 0 ; q < idx_1x1.size() ; q++) std::cout << idx_1x1[q] << ", "; if (num_1x1>0) std::cout << ")"; std::cout << "\nThere are " << num_1x2 << " blocks of size 1x2 "; if (num_1x2>0) std::cout << "(cluster index: "; for (size_t q = 0 ; q < idx_1x2.size() ; q++) std::cout << idx_1x2[q] << ", "; if (num_1x2>0) std::cout << ")"; std::cout << "\nThere are " << num_1x4 << " blocks of size 1x4 "; if (num_1x4>0) std::cout << "(cluster index: "; for (size_t q = 0 ; q < idx_1x4.size() ; q++) std::cout << idx_1x4[q] << ", "; if (num_1x4>0) std::cout << ")"; std::cout << "\nThere are " << num_unclassified << " unclassified blocks "; if (num_unclassified>0) std::cout << "(cluster index: "; for (size_t q = 0 ; q < idx_unclassified.size() ; q++) std::cout << idx_unclassified[q] << ", "; if (num_unclassified>0) std::cout << ")"; std::cout << "\n\n\n"; return; }