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;
}
