void BirdEye::calculatePositionVelocityError()
{
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
if (kdtree.nearestKSearch(currentPoint,1,pointIdxNKNSearch,pointNKNSquaredDistance)>0)
{
for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
{
pcl::PointXYZ pathPoint,pathPoint_vel,pathPoint_acc,pathTangent,pathNormal,pathBinormal;
pathPoint = cloud->points[ pointIdxNKNSearch[i] ];
pathPoint_vel = cloud_vel->points[ pointIdxNKNSearch[i] ];
pathPoint_acc = cloud_acc->points[ pointIdxNKNSearch[i] ];
pathTangent = normalize(pathPoint_vel);
//pathNormal = normalize(pathPoint_acc);
//pathBinormal = crossProduct(pathTangent,pathNormal);
e_p = vectorMinus(currentPoint,pathPoint);
e_p = vectorMinus(e_p,scalarProduct( dotProduct(e_p,pathTangent),pathTangent) );
ROS_DEBUG("current point: %.3f,%.3f,%.3f",currentPoint.x,currentPoint.y,currentPoint.z);
ROS_DEBUG("path point: %.3f,%.3f,%.3f",pathPoint.x,pathPoint.y,pathPoint.z);
e_v = vectorMinus(currentVel,pathPoint_vel);
acc_t = pathPoint_acc;
ROS_DEBUG("error in position: %.3f,%.3f,%.3f",e_p.x,e_p.y,e_p.z);
}
}
}
vector<float> VAO::getRadius()
{
vector<float> radius;
int K = 12;
std::vector<glm::vec3> positions;
pcl::KdTreeFLANN<pcl::PointXYZRGBNormal> kdtree;
std::vector<int> k_indices;
std::vector<float> k_distances;
kdtree.setInputCloud(cloud->makeShared());
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
for (unsigned int i = 0; i < cloud->size(); i++) {
if ( kdtree.nearestKSearch (cloud->points[i], K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
radius.push_back(sqrt(pointNKNSquaredDistance[K-1]));
}
return radius;
}
void removeOutlier(
PointCloudPtr cloud,
PointCloudPtr cloud_output,
int K,
float distanceThreshold
)
{
int size = cloud->width * cloud->height;
pcl::KdTreeFLANN<PointT> kdtree;
kdtree.setInputCloud(cloud);
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
std::vector<int> chosenIndices;
for (int i=0; i<size; ++i)
{
PointT point = cloud->points[i];
float avgKDis = averageKDistance(kdtree, pointIdxNKNSearch, pointNKNSquaredDistance, point, K);
if (avgKDis<distanceThreshold) chosenIndices.push_back(i);
}
cloud_output->width = chosenIndices.size();
cloud_output->height = 1;
cloud_output->resize(chosenIndices.size());
for (int i=0; i<chosenIndices.size(); ++i) cloud_output->points[i]=cloud->points[chosenIndices[i]];
}
PointCloudPtr kdTreeMinus(
PointCloudPtr cloud_a,
PointCloudPtr cloud_b,
float threshold,
float distanceThreshold
)
{
std::vector<int> chosenIndices;
int size = cloud_a->width * cloud_a->height;
pcl::KdTreeFLANN<PointT> kdtree_a, kdtree_b;
kdtree_a.setInputCloud(cloud_a);
kdtree_b.setInputCloud(cloud_b);
int K = 10;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
for (int i=0; i<size; ++i)
{
PointT point = cloud_a->points[i];
float avgKDis_a = averageKDistance(kdtree_a, pointIdxNKNSearch, pointNKNSquaredDistance, point, K),
avgKDis_b = averageKDistance(kdtree_b, pointIdxNKNSearch, pointNKNSquaredDistance, point, K);
if (avgKDis_a<distanceThreshold && abs(avgKDis_a-avgKDis_b)>threshold) chosenIndices.push_back(i);
}
PointCloudPtr cloud_output(new PointCloud);
cloud_output->width = chosenIndices.size();
cloud_output->height = 1;
cloud_output->resize(chosenIndices.size());
for (int i=0; i<chosenIndices.size(); ++i) cloud_output->points[i]=cloud_a->points[chosenIndices[i]];
return cloud_output;
}
//--------------------------------------------------------------------------------
//questa callback riceve in ingresso l'array di waypoints e per ogni coppia di
//waypoints adiacenti invoca il planner
//--------------------------------------------------------------------------------
void goalSelectionCallback(geometry_msgs::PoseArray waypoints){
//dimensione dell'array di waypoints
size_t n = waypoints.poses.size();
for( size_t i = 0; i < n; i++){
//istanzio un planner per ogni coppia di waypoints
PathPlanning new_planner(nh);
planner = &new_planner;
nav_msgs::Path path;
//al primo step il punto iniziale è la posa del robot
if( i == 0 ) {
planner->set_input(traversability_pcl,wall_pcl,wall_kdTree,traversability_kdtree,robot_idx);
}
//agli step successivi il punto iniziale è il goal dello step precedente
else {
pcl::PointXYZI in_point = convert(waypoints.poses.at(i-1));
//faccio il KNearestNeighbor search giusto per utilizzare la planner->set_input(..) scritta dagli altri
//poi dobbiamo scriverci la nostra set_input(..) e tutto questo non sarà più necessario
pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
kdtree.setInputCloud(traversability_pcl.makeShared());
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtree.nearestKSearch(in_point, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
size_t input_idx = pointIdxNKNSearch[0];
planner->set_input(traversability_pcl,wall_pcl,wall_kdTree,traversability_kdtree,input_idx);
}
//qui setto il goal
pcl::PointXYZI goal_point = convert(waypoints.poses.at(i));
planner->set_goal(goal_point);
goal_selection = true;
ROS_INFO("goal selection");
//qui lancio il planner
if(planner->planning(path)){//<--questa funzione va riscritta!!!
path_pub.publish(path);
}
else{
ROS_INFO("no path exist for desired goal, please choose another goal");
goal_selection = true;
}
ROS_INFO("path_computed");
}
}
pcl17::PointIndicesPtr getIndicesFromPointCloud(const PointCloudConstPtr& source_cloud,
const PointCloudConstPtr& search_cloud_ptr)
{
pcl17::PointIndicesPtr output_indices_ptr (new pcl17::PointIndices);
pcl17::KdTreeFLANN<PointType> kdtreeNN;
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtreeNN.setInputCloud(search_cloud_ptr);
for(uint j = 0; j < source_cloud->points.size(); j++)
{
kdtreeNN.nearestKSearch(source_cloud->points[j], 1, pointIdxNKNSearch, pointNKNSquaredDistance);
output_indices_ptr->indices.push_back(pointIdxNKNSearch[0]);
}
return output_indices_ptr;
}
void pointCloudCallback(const sensor_msgs::PointCloud2& traversability_msg) {
//pcl::PointCloud<pcl::PointXYZI> traversability_pcl;
pcl::fromROSMsg(traversability_msg, traversability_pcl);
ROS_INFO("path planner input set");
if (traversability_pcl.size() > 0 && wall_flag){
tf::StampedTransform robot_pose;
getRobotPose(robot_pose);
pcl::PointXYZI robot;
robot.x = robot_pose.getOrigin().x();
robot.y = robot_pose.getOrigin().y();
robot.z = robot_pose.getOrigin().z();
//pcl::KdTreeFLANN<pcl::PointXYZI> traversability_kdtree;
traversability_kdtree.setInputCloud(traversability_pcl.makeShared());
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
traversability_kdtree.nearestKSearch(robot, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
robot_idx = pointIdxNKNSearch[0];
//----------------------------------------------------------------------------------------------------------------
//ho commentato questa parte perchè vogliamo disaccoppiare il planning dall'acquisizione della Point Cloud
//----------------------------------------------------------------------------------------------------------------
// planner->set_input(traversability_pcl, wall_pcl, wall_kdTree, traversability_kdtree, pointIdxNKNSearch[0]);
// wall_flag = false;
// if (goal_selection){
// nav_msgs::Path path;
// ROS_INFO("compute path");
// if(goal_selection){
// if(planner->planning(path)){
// path_pub.publish(path);
// }
// else{
// ROS_INFO("no path exist for desired goal, please choose another goal");
// goal_selection = true;
// }
// ROS_INFO("path_computed");
// }
// }
}
}
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_error ("Syntax is: %s <pcd-file> \n"
"--NT Dsables the single cloud transform \n"
"-v <voxel resolution>\n-s <seed resolution>\n"
"-c <color weight> \n-z <spatial weight> \n"
"-n <normal_weight>\n"
"---LCCP params----\n"
"-sc disable sanity criterion\n"
"-ct concavity tolerance\n"
"-st smoothness threshold\n"
"-ec enable extended criterion\n"
"-smooth min segment size\n"
"-- Others parameters ---"
"-zmin minimum distance orthogonal to the table plane to consider a point as valid\n"
"-zmax maximum distance orthogonal to the table plane to consider a point as valid\n"
"-th_points minimum amount of points to consider a cluster as an object\n"
"\n", argv[0]);
return (1);
}
// parameters for the LCCP segmentation
tos_supervoxels_parameters opt;
// ------------------- parsing the inputs ----------------------------
//--------------------------------------------------------------------
opt.disable_transform = pcl::console::find_switch (argc, argv, "--NT");
pcl::console::parse (argc, argv, "-v", opt.voxel_resolution);
pcl::console::parse (argc, argv, "-s", opt.seed_resolution);
pcl::console::parse (argc, argv, "-c", opt.color_importance);
pcl::console::parse (argc, argv, "-z", opt.spatial_importance);
pcl::console::parse (argc, argv, "-n", opt.normal_importance);
pcl::console::parse (argc, argv, "-ct", opt.concavity_tolerance_threshold);
pcl::console::parse (argc, argv, "-st", opt.smoothness_threshold);
opt.use_extended_convexity = pcl::console::find_switch (argc, argv, "-ec");
opt.use_sanity_criterion = !pcl::console::find_switch (argc, argv, "-sc");
pcl::console::parse (argc, argv, "-smooth", opt.min_segment_size);
// table plane estimation - parameters
pcl::console::parse (argc, argv, "-zmin", opt.zmin);
// minimum amount of points to consider a cluster as an object
pcl::console::parse (argc, argv, "-th_points", opt.th_points);
//-------------------------------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::console::print_highlight ("Loading point cloud...\n");
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud))
{
pcl::console::print_error ("Error loading cloud file!\n");
return (1);
}
//----------------------------------------
tos_supervoxels seg;
std::vector<Object> seg_objs; //("seg_objs" stands for "segmented objects")
seg.init(*cloud,opt);
//seg.init(*cloud);
seg.print_parameters();
seg.segment();
seg_objs = seg.get_segmented_objects();
/* you could eventually use the following:
std::vector<pcl::PointCloud<pcl::PointXYZRGBA> > segmented_objs;
segmented_objs = seg.get_segmented_objects_simple();
*/
std::cout << "\nDetected " << seg_objs.size() << " objects.\n\n";
//----------------------------------------
/// Configure Visualizer
pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);
seg.show_table_plane(viewer); // we add the table plane
std::cout << "Press 'n' to show the segmented objects\n";
while (!viewer->wasStopped () && !pressed) // the pressed variable is just usfull only for this first while (bad programming)
viewer->spinOnce (100);
// show super voxels with normals and adiacency map
bool show_adjacency_map = true;
bool show_super_voxel_normals = false;
seg.show_super_voxels(viewer,show_adjacency_map,show_super_voxel_normals);
while (!viewer->wasStopped () && !pressed) // the pressed variable is just usfull only for this first while (bad programming)
viewer->spinOnce (100);
seg.clean_viewer(viewer);
// show the segmented objects, the result of the segmentation
std::cout << "\nClose the visualizer to go to the next step: retrieving the objects point in the RGB image\n";
seg.show_labelled_segmented_objects(viewer);
while (!viewer->wasStopped ()) // the pressed variable is just usfull only for this first while (bad programming)
viewer->spinOnce (100);
seg.clean_viewer(viewer);
seg.reset(); // free memory
if(cloud->isOrganized())//if the point cloud is organized we can work with the RGB image
{
std::cout << "You are now viewing the RGB image (recovered by the point cloud)."
<< " Press whatever key to go further.";
// recover the RGB IMAGE from the point cloud
cv::Mat img_orignal(480, 640, CV_8UC3); //create an image ( 3 channels, 8 bit image depth);
for (int row = 0; row < img_orignal.rows; ++row)
{
for (int c = 0; c < img_orignal.cols; ++c)
{
pcl::PointXYZRGBA point = (*cloud)(c,row); //note: there is a transformation in the reference frame of the pclò and the image!!!!!!!!!!!!!!!!
uint8_t r = (uint8_t)point.r;
uint8_t g = (uint8_t)point.g;
uint8_t b = (uint8_t)point.b;
cv::Vec3b color;//vector of colors
color.val[0] = b;
color.val[1] = g;
color.val[2] = r;
img_orignal.at<cv::Vec3b>(row,c) = color;
}
}
cv::namedWindow("Original", CV_WINDOW_AUTOSIZE); //create a window with the name "MyWindow"
cv::imshow("Original", img_orignal); //display the image which is stored in the 'img' in the "MyWindow" window
cv::imwrite( "./original.jpg", img_orignal );
cv::waitKey(0);
// displaying in the RGB image what are the pixels of the rgb image related to the segmented objects
std::cout << "\nYou are now visualizing the results of the segmentation in the 2D rgb image."
<< " Press right arrow to visualize the next object, the left one to visualize the previous one, and ESC to exit.\n";
// to retireve the points in the RGB image we have to use a KdTree to get the points of the object
// in the input cloud
pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
kdtree.setInputCloud (cloud);
int k = 0;
uint32_t idx_v = 0; //index of the object to work with
cv::namedWindow("Segmented Results", CV_WINDOW_AUTOSIZE); //create a window with the name "MyWindow"
while( (k != 27 && k != 1048603) && idx_v < seg_objs.size() && idx_v >= 0) //while it ESC is not pressed
{
if(seg_objs[idx_v].object_cloud.points.size() > 400)
{
cv::Mat img(480, 640, CV_8UC3,cv::Scalar(255,255,255)); //create an image ( 3 channels, 8 bit image depth);
//fill the matrix "img" with only the points of the current object
for (int i = 0; i < seg_objs[idx_v].object_cloud.points.size(); ++i)
{
pcl::PointXYZRGBA searchPoint;
searchPoint.x = seg_objs[idx_v].object_cloud.points[i].x;
searchPoint.y = seg_objs[idx_v].object_cloud.points[i].y;
searchPoint.z = seg_objs[idx_v].object_cloud.points[i].z;
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
if ( kdtree.nearestKSearch (searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
{
pcl::PointXYZRGBA point = cloud->points[pointIdxNKNSearch[0]];
uint8_t r = (uint8_t)point.r;
uint8_t g = (uint8_t)point.g;
uint8_t b = (uint8_t)point.b;
cv::Vec3b color; //vector of colors
color.val[0] = b;
color.val[1] = g;
color.val[2] = r;
float x,y;
y = (int)(pointIdxNKNSearch[0]/cloud->width);
x = pointIdxNKNSearch[0] - y*cloud->width;
img.at<cv::Vec3b>(y,x) = color; //transformation coordinates
}
}
cv::imshow("Segmented Results", img); //display the image which is stored in the 'img' in the "MyWindow" window
std::cout << "Object: " << idx_v + 1 << " of " << seg_objs.size() << " objects." << std::endl;
std::stringstream ss;
ss << "img" << idx_v << ".jpg";
cv::imwrite( ss.str(), img );
k = cv::waitKey(0);
}
if( k == 65363 || k == 0 || k == 1113939) // right arrow
idx_v++;
if( k == 65361 || k == 1113937) // left arrow
idx_v > 0 ? idx_v-- : idx_v = 0;
}
}
else
std::cout << "The point cloud is not organized";
return (0);
}
int
main (int argc, char** argv)
{
srand (time (NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Generate pointcloud data
cloud->width = 1000;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud (cloud);
pcl::PointXYZ searchPoint;
searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
// K nearest neighbor search
int K = 10;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x
<< " " << cloud->points[ pointIdxNKNSearch[i] ].y
<< " " << cloud->points[ pointIdxNKNSearch[i] ].z
<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
std::cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << std::endl;
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].y
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
return 0;
}
void HintedHandleEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const geometry_msgs::PointStampedConstPtr &point_msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PassThrough<pcl::PointXYZ> pass;
int K = 1;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
geometry_msgs::PointStamped transed_point;
ros::Time now = ros::Time::now();
try
{
listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
}
catch(tf::TransformException ex)
{
JSK_ROS_ERROR("%s", ex.what());
return;
}
pcl::PointXYZ searchPoint;
searchPoint.x = transed_point.point.x;
searchPoint.y = transed_point.point.y;
searchPoint.z = transed_point.point.z;
//remove too far cloud
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
pass.filter(*cloud);
if(cloud->points.size() < 10){
JSK_ROS_INFO("points are too small");
return;
}
if(1){ //estimate_normal
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(kd_tree);
ne.setRadiusSearch(0.02);
ne.setViewPoint(0, 0, 0);
ne.compute(*cloud_normals);
}
else{ //use normal of msg
}
if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
JSK_ROS_INFO("kdtree failed");
return;
}
float x = cloud->points[pointIdxNKNSearch[0]].x;
float y = cloud->points[pointIdxNKNSearch[0]].y;
float z = cloud->points[pointIdxNKNSearch[0]].z;
float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
double theta = acos(v_x);
// use normal for estimating handle direction
tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
tf::Quaternion final_quaternion = normal;
double min_theta_index = 0;
double min_width = 100;
tf::Quaternion min_qua(0, 0, 0, 1);
visualization_msgs::Marker debug_hand_marker;
debug_hand_marker.header = cloud_msg->header;
debug_hand_marker.ns = string("debug_grasp");
debug_hand_marker.id = 0;
debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
debug_hand_marker.pose.orientation.w = 1;
debug_hand_marker.scale.x=0.003;
tf::Matrix3x3 best_mat;
//search 180 degree and calc the shortest direction
for(double theta_=0; theta_<3.14/2;
theta_+=3.14/2/30){
tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
tf::Quaternion temp_qua = normal * rotate_;
tf::Matrix3x3 temp_mat(temp_qua);
geometry_msgs::Pose pose_respected_to_tf;
pose_respected_to_tf.position.x = x;
pose_respected_to_tf.position.y = y;
pose_respected_to_tf.position.z = z;
pose_respected_to_tf.orientation.x = temp_qua.getX();
pose_respected_to_tf.orientation.y = temp_qua.getY();
pose_respected_to_tf.orientation.z = temp_qua.getZ();
pose_respected_to_tf.orientation.w = temp_qua.getW();
Eigen::Affine3d box_pose_respected_to_cloud_eigend;
tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
= box_pose_respected_to_cloud_eigend.inverse();
Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
= box_pose_respected_to_cloud_eigend_inversed.matrix();
jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
box_pose_respected_to_cloud_eigen_inversed_matrixd,
box_pose_respected_to_cloud_eigen_inversed_matrixf);
Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output_cloud, offset);
pcl::PassThrough<pcl::PointXYZ> pass;
pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pass.setInputCloud(output_cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
pass.filter(*points_z);
pass.setInputCloud(points_z);
pass.setFilterFieldName("z");
pass.setFilterLimits(-handle.finger_d, handle.finger_d);
pass.filter(*points_yz);
pass.setInputCloud(points_yz);
pass.setFilterFieldName("x");
pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
pass.filter(*points_xyz);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
for(size_t index=0; index<points_xyz->size(); index++){
points_xyz->points[index].x = points_xyz->points[index].z = 0;
}
if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
kdtree.setInputCloud(points_xyz);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
pcl::PointXYZ search_point_tree;
search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
double before_w=10, temp_w;
for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
temp_w =sqrt(pointRadiusSquaredDistance[index]);
if(temp_w - before_w > handle.finger_w*2){
break; // there are small space for finger
}
before_w=temp_w;
}
if(before_w < min_width){
min_theta_index = theta_;
min_width = before_w;
min_qua = temp_qua;
best_mat = temp_mat;
}
//for debug view
geometry_msgs::Point temp_point;
std_msgs::ColorRGBA temp_color;
temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
}
}
geometry_msgs::PoseStamped handle_pose_stamped;
handle_pose_stamped.header = cloud_msg->header;
handle_pose_stamped.pose.position.x = x;
handle_pose_stamped.pose.position.y = y;
handle_pose_stamped.pose.position.z = z;
handle_pose_stamped.pose.orientation.x = min_qua.getX();
handle_pose_stamped.pose.orientation.y = min_qua.getY();
handle_pose_stamped.pose.orientation.z = min_qua.getZ();
handle_pose_stamped.pose.orientation.w = min_qua.getW();
std_msgs::Float64 min_width_msg;
min_width_msg.data = min_width;
pub_pose_.publish(handle_pose_stamped);
pub_debug_marker_.publish(debug_hand_marker);
pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
jsk_recognition_msgs::SimpleHandle simple_handle;
simple_handle.header = handle_pose_stamped.header;
simple_handle.pose = handle_pose_stamped.pose;
simple_handle.handle_width = min_width;
pub_handle_.publish(simple_handle);
}
/** \brief The machine learning classifier, results are stored in the ClusterData structs.
* \param[in] cloud_in A pointer to the input point cloud.
* \param[in/out] clusters_data An array of information holders for each cluster
*/
void
applyObjectClassification (const pcl::PointCloud<PointType>::Ptr cloud_in, boost::shared_ptr<std::vector<ClusterData> > &clusters_data)
{
// Set up the machine learnin class
pcl::SVMTrain ml_svm_training; // To train the classifier
pcl::SVMClassify ml_svm_classify; // To classify
std::vector<pcl::SVMData> featuresSet; // Create the input vector for the SVM class
std::vector<std::vector<double> > predictionOut; // Prediction output vector
// If the input model_filename exists, it starts the classification.
// Otherwise it starts a new machine learning training.
if (global_data.model.size() > 0)
{
if (!ml_svm_classify.loadClassifierModel (global_data.model.data()))
return;
pcl::console::print_highlight (stderr, "Loaded ");
pcl::console::print_value (stderr, "%s ", global_data.model.data());
// Copy the input vector for the SVM classification
for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
featuresSet.push_back ( (*clusters_data) [c_it].features);
ml_svm_classify.setInputTrainingSet (featuresSet); // Set input clusters set
ml_svm_classify.saveNormClassProblem ("data_input"); //Save clusters features
ml_svm_classify.setProbabilityEstimates (1); // Estimates the probabilities
ml_svm_classify.classification ();
}
else
{
// Currently: analyze on voxels of 0.08 x 0.08 x 0.08 meter with slight alteration based on cluster aggressiveness
float resolution = 0.08 * global_data.scale / pow (0.5 + global_data.cagg, 2);
// Create the viewer
pcl::visualization::PCLVisualizer viewer ("cluster viewer");
// Output classifier model name
global_data.model.assign (global_data.cloud_name.data());
global_data.model.append (".model");
std::vector<bool> lab_cluster;// save whether a cluster is labelled
std::vector<int> pt_clst_pos; // used to memorize in the total cloud, the point affiliation to the original cluster
// fill the vector (1 = labelled, 0 = unlabelled)
lab_cluster.resize ( (std::size_t) clusters_data->size ());
for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
{
if ( (*clusters_data) [c_it].is_isolated)
{
(*clusters_data) [c_it].features.label = 0;
lab_cluster[c_it] = 1;
}
else
lab_cluster[c_it] = 0;
}
// Build a cloud with unlabelled clusters
pcl::PointCloud<PointType>::Ptr fragm_cloud (new pcl::PointCloud<PointType>);
// Initialize the viewer
initVisualizer (viewer);
PointType picked_point; // changed whether a mouse click occours. It saves the selected cluster index
viewer.registerPointPickingCallback (&pp_callback, (void *) &picked_point);
// Create a cloud with unlabelled clusters
for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
if (!lab_cluster[c_it])
{
pcl::PointCloud<PointType>::Ptr cluster (new pcl::PointCloud<PointType>);
pcl::copyPointCloud (*cloud_in, * (*clusters_data) [c_it].indices, *cluster);
// Downsample cluster
pcl::VoxelGrid<PointType> sor;
sor.setInputCloud (cluster);
sor.setLeafSize (resolution, resolution, resolution);
sor.filter (*cluster);
// Copy cluster into a global cloud
fragm_cloud->operator+= (*cluster);
// Fill a vector to memorize the original affiliation of a point to the cluster
for (int clust_pt = 0; clust_pt < cluster->size(); clust_pt++)
pt_clst_pos.push_back (c_it);
// Add cluster to the viewer
std::stringstream cluster_name;
cluster_name << "cluster" << c_it;
pcl::visualization::PointCloudColorHandlerGenericField<PointType> rgb (cluster, "intensity");// Get color handler for the cluster cloud
viewer.addPointCloud<PointType> (cluster, rgb, cluster_name.str().data());
}
// Create a tree for point searching in the total cloud
pcl::KdTreeFLANN<pcl::PointXYZI> tree_;
tree_.setInputCloud (fragm_cloud);
// Visualize the whole cloud
int selected = -1; // save the picked cluster
bool stop = 0;
while (!viewer.wasStopped())
{
viewer.registerKeyboardCallback (keyboardEventOccurred, (void*) &stop);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
viewer.spinOnce (500);
if (picked_point.x != 0.0f || picked_point.y != 0.0f || picked_point.z != 0.0f) // if a point is clicked
{
std::vector<int> pointIdxNKNSearch (1);
std::vector<float> pointNKNSquaredDistance (1);
pcl::PointCloud<PointType>::Ptr cluster (new pcl::PointCloud<PointType>);
tree_.nearestKSearch (picked_point, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
selected = pt_clst_pos[pointIdxNKNSearch[0]];
viewer.removePointCloud("cluster");
pcl::copyPointCloud (*cloud_in, * (*clusters_data) [selected].indices, *cluster);
pcl::visualization::PointCloudColorHandlerGenericField<PointType> rgb (cluster, "intensity");
viewer.addPointCloud<PointType> (cluster, rgb, "cluster");
picked_point.x = 0.0f;
picked_point.y = 0.0f;
picked_point.z = 0.0f;
}
if(selected != -1 && stop)
{
std::stringstream cluster_name;
cluster_name << "cluster" << selected;
lab_cluster[ selected ] = 1; // cluster is marked as labelled
(*clusters_data) [ selected ].features.label = 1; // the cluster is set as a noise
viewer.removePointCloud(cluster_name.str().data());
viewer.removePointCloud("cluster");
stop = 0;
selected = -1;
}
}
// Close the viewer
viewer.close();
// The remaining unlabelled clusters are marked as "good"
for (int c_it = 0; c_it < lab_cluster.size(); c_it++)
{
if (!lab_cluster[c_it])
(*clusters_data) [c_it].features.label = 0; // Mark remaining clusters as good
}
// Copy the input vector for the SVM classification
for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
featuresSet.push_back ( (*clusters_data) [c_it].features);
// Setting the training classifier
pcl::SVMParam trainParam;
trainParam.probability = 1; // Estimates the probabilities
trainParam.C = 512; // Initial C value of the classifier
trainParam.gamma = 2; // Initial gamma value of the classifier
ml_svm_training.setInputTrainingSet (featuresSet); // Set input training set
ml_svm_training.setParameters (trainParam);
ml_svm_training.trainClassifier(); // Train the classifier
ml_svm_training.saveClassifierModel (global_data.model.data()); // Save classifier model
pcl::console::print_highlight (stderr, "Saved ");
pcl::console::print_value (stderr, "%s ", global_data.model.data());
ml_svm_training.saveTrainingSet ("data_input"); // Save clusters features normalized
// Test the current classification
ml_svm_classify.loadClassifierModel (global_data.model.data());
ml_svm_classify.setInputTrainingSet (featuresSet);
ml_svm_classify.setProbabilityEstimates (1);
ml_svm_classify.classificationTest ();
}
ml_svm_classify.saveClassificationResult ("prediction"); // save prediction in outputtext file
ml_svm_classify.getClassificationResult (predictionOut);
// Get labels order
std::vector<int> labels;
ml_svm_classify.getLabel (labels);
// Store the boolean output inside clusters_data
for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
switch ( (int) predictionOut[c_it][0])
{
case 0:
(*clusters_data) [c_it].is_good = true;
break;
case 1:
(*clusters_data) [c_it].is_ghost = true;
break;
case 2:
(*clusters_data) [c_it].is_tree = true;
break;
}
// Store the percentage output inside cluster_data
for (size_t lab = 0; lab < labels.size (); lab++)
switch (labels[lab])
{
case 0:
for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
{
(*clusters_data) [c_it].is_good_prob = predictionOut[c_it][lab + 1];
}
break;
case 1:
for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
{
(*clusters_data) [c_it].is_ghost_prob = predictionOut[c_it][lab + 1];
}
break;
case 2:
for (size_t c_it = 0; c_it < clusters_data->size (); ++c_it)
{
(*clusters_data) [c_it].is_tree_prob = predictionOut[c_it][lab + 1];
}
break;
}
};
// input: cloudInput
// input: pointCloudNormals
// output: cloudOutput
// output: pointCloudNormalsFiltered
void extractHandles(PointCloud::Ptr& cloudInput, PointCloud::Ptr& cloudOutput, PointCloudNormal::Ptr& pointCloudNormals, std::vector<int>& handles) {
// PCL objects
//pcl::PassThrough<Point> vgrid_; // Filtering + downsampling object
pcl::VoxelGrid<Point> vgrid_; // Filtering + downsampling object
pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_; // Planar segmentation object
pcl::SACSegmentation<Point> seg_line_; // Planar segmentation object
pcl::ProjectInliers<Point> proj_; // Inlier projection object
pcl::ExtractIndices<Point> extract_; // Extract (too) big tables
pcl::ConvexHull<Point> chull_;
pcl::ExtractPolygonalPrismData<Point> prism_;
pcl::PointCloud<Point> cloud_objects_;
pcl::EuclideanClusterExtraction<Point> cluster_, handle_cluster_;
pcl::PCDWriter writer;
double sac_distance_, normal_distance_weight_;
double eps_angle_, seg_prob_;
int max_iter_;
sac_distance_ = 0.05; //0.02
normal_distance_weight_ = 0.05;
max_iter_ = 500;
eps_angle_ = 30.0; //20.0
seg_prob_ = 0.99;
btVector3 axis(0.0, 0.0, 1.0);
seg_.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
seg_.setMethodType(pcl::SAC_RANSAC);
seg_.setDistanceThreshold(sac_distance_);
seg_.setNormalDistanceWeight(normal_distance_weight_);
seg_.setOptimizeCoefficients(true);
seg_.setAxis(Eigen::Vector3f(fabs(axis.getX()), fabs(axis.getY()), fabs(axis.getZ())));
seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
seg_.setMaxIterations(max_iter_);
seg_.setProbability(seg_prob_);
cluster_.setClusterTolerance(0.03);
cluster_.setMinClusterSize(200);
KdTreePtr clusters_tree_(new KdTree);
clusters_tree_->setEpsilon(1);
cluster_.setSearchMethod(clusters_tree_);
seg_line_.setModelType(pcl::SACMODEL_LINE);
seg_line_.setMethodType(pcl::SAC_RANSAC);
seg_line_.setDistanceThreshold(0.05);
seg_line_.setOptimizeCoefficients(true);
seg_line_.setMaxIterations(max_iter_);
seg_line_.setProbability(seg_prob_);
//filter cloud
double leafSize = 0.001;
pcl::VoxelGrid<Point> sor;
sor.setInputCloud (cloudInput);
sor.setLeafSize (leafSize, leafSize, leafSize);
sor.filter (*cloudOutput);
//sor.setInputCloud (pointCloudNormals);
//sor.filter (*pointCloudNormalsFiltered);
//std::vector<int> tempIndices;
//pcl::removeNaNFromPointCloud(*cloudInput, *cloudOutput, tempIndices);
//pcl::removeNaNFromPointCloud(*pointCloudNormals, *pointCloudNormalsFiltered, tempIndices);
// Segment planes
pcl::OrganizedMultiPlaneSegmentation<Point, PointNormal, pcl::Label> mps;
ROS_INFO("Segmenting planes");
mps.setMinInliers (20000);
mps.setMaximumCurvature(0.02);
mps.setInputNormals (pointCloudNormals);
mps.setInputCloud (cloudInput);
std::vector<pcl::PlanarRegion<Point> > regions;
std::vector<pcl::PointIndices> regionPoints;
std::vector< pcl::ModelCoefficients > planes_coeff;
mps.segment(planes_coeff, regionPoints);
ROS_INFO_STREAM("Number of regions:" << regionPoints.size());
if ((int) regionPoints.size() < 1) {
ROS_ERROR("no planes found");
return;
}
std::stringstream filename;
for (size_t plane = 0; plane < regionPoints.size (); plane++)
{
filename.str("");
filename << "plane" << plane << ".pcd";
writer.write(filename.str(), *cloudInput, regionPoints[plane].indices, true);
ROS_INFO("Plane model: [%f, %f, %f, %f] with %d inliers.",
planes_coeff[plane].values[0], planes_coeff[plane].values[1],
planes_coeff[plane].values[2], planes_coeff[plane].values[3], (int)regionPoints[plane].indices.size ());
//Project Points into the Perfect plane
PointCloud::Ptr cloud_projected(new PointCloud());
pcl::PointIndicesPtr cloudPlaneIndicesPtr(new pcl::PointIndices(regionPoints[plane]));
pcl::ModelCoefficientsPtr coeff(new pcl::ModelCoefficients(planes_coeff[plane]));
proj_.setInputCloud(cloudInput);
proj_.setIndices(cloudPlaneIndicesPtr);
proj_.setModelCoefficients(coeff);
proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
proj_.filter(*cloud_projected);
PointCloud::Ptr cloud_hull(new PointCloud());
// Create a Convex Hull representation of the projected inliers
chull_.setInputCloud(cloud_projected);
chull_.reconstruct(*cloud_hull);
ROS_INFO("Convex hull has: %d data points.", (int)cloud_hull->points.size ());
if ((int) cloud_hull->points.size() == 0)
{
ROS_WARN("Convex hull has: %d data points. Returning.", (int)cloud_hull->points.size ());
return;
}
// Extract the handle clusters using a polygonal prism
pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
prism_.setHeightLimits(0.05, 0.1);
prism_.setInputCloud(cloudInput);
prism_.setInputPlanarHull(cloud_hull);
prism_.segment(*handlesIndicesPtr);
ROS_INFO("Number of handle candidates: %d.", (int)handlesIndicesPtr->indices.size ());
if((int)handlesIndicesPtr->indices.size () < 1100)
continue;
/*######### handle clustering code
handle_clusters.clear();
handle_cluster_.setClusterTolerance(0.03);
handle_cluster_.setMinClusterSize(200);
handle_cluster_.setSearchMethod(clusters_tree_);
handle_cluster_.setInputCloud(handles);
handle_cluster_.setIndices(handlesIndicesPtr);
handle_cluster_.extract(handle_clusters);
for(size_t j = 0; j < handle_clusters.size(); j++)
{
filename.str("");
filename << "handle" << (int)i << "-" << (int)j << ".pcd";
writer.write(filename.str(), *handles, handle_clusters[j].indices, true);
}*/
pcl::StatisticalOutlierRemoval<Point> sor;
PointCloud::Ptr cloud_filtered (new pcl::PointCloud<Point>);
sor.setInputCloud (cloudInput);
sor.setIndices(handlesIndicesPtr);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);
PointCloudNormal::Ptr cloud_filtered_hack (new PointCloudNormal);
pcl::copyPointCloud(*cloud_filtered, *cloud_filtered_hack);
// Our handle is in cloud_filtered. We want indices of a cloud (also filtered for NaNs)
pcl::KdTreeFLANN<PointNormal> kdtreeNN;
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtreeNN.setInputCloud(pointCloudNormals);
for(size_t j = 0; j < cloud_filtered_hack->points.size(); j++)
{
kdtreeNN.nearestKSearch(cloud_filtered_hack->points[j], 1, pointIdxNKNSearch, pointNKNSquaredDistance);
handles.push_back(pointIdxNKNSearch[0]);
}
}
}
