void visualization::DetectionVisualizer::visualizeSampledGrasps()
{
if (obj().draw_sampled_grasps_)
{
Point middle;
middle.getVector3fMap() = obj().obj_bounding_box_->position_base_kinect_frame_;
removeShape(SUPPORT_PLANE);
addPlane(*(obj().table_plane_), middle.x, middle.y, middle.z, SUPPORT_PLANE);
CloudPtr side_grasps = obj().getSampledSideGrasps();
const pcl::PCLHeader &header = obj().world_obj_->header;
transformPointCloud(FOOTPRINT_FRAME, header.frame_id, side_grasps, side_grasps,header.stamp,tf_listener_);
visualizeCloud(SIDE_GRASPS, side_grasps, 255, 0, 0);
CloudPtr top_grasps = obj().getSampledTopGrasps();
transformPointCloud(FOOTPRINT_FRAME, header.frame_id, top_grasps, top_grasps,header.stamp,tf_listener_);
visualizeCloud(TOP_GRASPS, top_grasps, 255, 0, 0);
//visualizePoint(middle, 0, 0, 255, CENTROID);
obj().draw_sampled_grasps_ = false;
}
}
void visualization::DetectionVisualizer::visualizeBoundingBox()
{
if (obj().draw_bounding_box_)
{
detection::BoundingBox &box = *(obj().obj_bounding_box_);
// Draw world coordinate system
removeCoordinateSystem();
addCoordinateSystem(0.5);
addCoordinateSystem(0.25, box.getObjToKinectTransform());
CloudPtr &obj_kinect_frame = obj().world_obj_;
visualizeCloud(OBJ_KINECT_FRAME, obj_kinect_frame, 0, 255, 0);
visualizeCloud(OBJ_2D, obj().planar_obj_, 120, 120, 120);
CloudPtr obj_frame(new Cloud);
transformPointCloud(obj_kinect_frame->header.frame_id, box.OBJ_FRAME, obj_kinect_frame, obj_frame,
obj_kinect_frame->header.stamp, tf_listener_);
visualizeCloud(OBJ_3D, obj_frame, 0, 0, 255);
visualizeCloud(WORLD_PLANAR_OBJ, box.obj_2D_kinect_frame, 0, 0, 255);
// Draw the box in world coords
visualizeBox(box, WORLD_BOUNDING_BOX, box.position_3D_kinect_frame_, box.getRotationQuaternion());
// Draw the box in the origin (obj coords)
visualizeBox(box, BOUNDING_BOX);
CloudPtr a(new Cloud);
a->push_back(newPoint(box.position_base_kinect_frame_));
a->push_back(newPoint(box.position_3D_kinect_frame_));
visualizeCloud("a", a, 0, 0, 255);
CloudPtr b(new Cloud);
b->push_back(newPoint(box.centroid_2D_kinect_frame_.head<3>()));
b->push_back(newPoint(box.planar_shift_));
//b->push_back(newPoint(box.));
visualizeCloud("b", b, 255, 0, 0);
//showEigenVectors(box);
obj().draw_bounding_box_ = false;
}
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr model;
std::vector< pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > set_cloud;
std::vector< boost::shared_ptr< Eigen::MatrixXd > > set_covariance;
cv2PointCloudSet(imageList_rgb, imageList_depth, K, *Qn_global, *tr_global, set_cloud, set_covariance);
set2unique( set_cloud, model );
// ========================================== END Registration ==========================================
// ============================================ Visualization ============================================
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = visualizeCloud(model);
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
// To save as ply apply his rotation to the points
if (save_ply)
{
pcl::PLYWriter wr_ply;
wr_ply.write(outputFilename, *model);
}
// pcl::io::savePCDFileASCII ("gin_std.pcd", model);
// pcl::io::savePLYFileASCI("gin_std.ply", model);
bool service_callback(pcl_perception::PeopleDetectionSrv::Request &req,
pcl_perception::PeopleDetectionSrv::Response &res)
{
//the node cannot be asked to detect people while someone has asked it to detect people
if (node_state != IDLE) {
ROS_WARN("[segbot_pcl_person_detector] Person detection service may not be called until previous call is processed");
return true;
}
node_state = DETECTING;
ROS_INFO("[general_perception.cpp] Received command %s",req.command.c_str());
//command for detecting people standing up
if (req.command == "reocrd_cloud") {
PointCloudT::Ptr aggregated_cloud (new PointCloudT);
bool collecting_clouds = true;
int num_saved = 0;
while (collecting_clouds) {
//collect to grab cloud
if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available
{
ROS_INFO("Adding next cloud...");
*aggregated_cloud += *cloud;
num_saved++;
if (num_saved > num_frames_for_detection) {
collecting_clouds = false;
}
//unlock mutex and reset flag
new_cloud_available_flag = false;
}
//ROS_INFO("unlocking mutex...");
cloud_mutex.unlock ();
ros::spinOnce();
}
if (useVoxelGridFilter) {
pcl::PCLPointCloud2::Ptr pcl_pc (new pcl::PCLPointCloud2) ;
pcl::toPCLPointCloud2( *aggregated_cloud,*pcl_pc);
//filter each step
pcl::VoxelGrid< pcl::PCLPointCloud2 > sor;
sor.setInputCloud (pcl_pc);
sor.setLeafSize (0.005, 0.005, 0.005);
sor.filter (*pcl_pc);
//covert back
pcl::fromPCLPointCloud2(*pcl_pc, *aggregated_cloud);
}
if (useStatOutlierFilter) {
// Create the filtering object
pcl::StatisticalOutlierRemoval<PointT> sor;
sor.setInputCloud (aggregated_cloud);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*aggregated_cloud);
}
visualizeCloud(aggregated_cloud);
/*current_frame_counter = 0;
bool collecting_cloud = true;
node_state = DETECTING;
while (collecting_cloud){
if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available
{
ROS_INFO("Detecting on frame %i",current_frame_counter);
new_cloud_available_flag = false;
// Draw cloud and people bounding boxes in the viewer:
//increment counter and see if we need to stop detecting
current_frame_counter++;
if (current_frame_counter > num_frames_for_detection){
collecting_cloud = false;
}
}
cloud_mutex.unlock ();
//spin to collect next point cloud
ros::spinOnce();
if (visualize){
visualizeCloud();
}
}*/
}
node_state = IDLE;
return true;
}
