Eigen::Vector3d obj_axe_2_points_next(int i_head,int i_center){ pcl::PointXYZRGB center,head; center = cloud->at(i_center); //x1 y1 z1 head = cloud->at(i_head); //x2 y2 z2 Eigen::Vector3d vector_l; vector_l(0)=5*(head.x - center.x); vector_l(1)=5*(head.y - center.y); vector_l(2)=5*(head.z - center.z); return vector_l; }
void App::VisZ(TUMDataSetVisualizer * viewer) { float const maxZ = max_element(Input_->begin(), Input_->end(), [] (PointType const& lft, PointType const& rgh) { if (pcl_isfinite(lft.z) && pcl_isfinite(rgh.z)) { return lft.z < rgh.z; } else { return ! pcl_isfinite(lft.z); } })->z; viewer->EasyAdd(Input_, "input", [this, &maxZ] (int i) { // #6 float const relZ = Input_->at(i).z / maxZ; return Color({static_cast<int>(relZ * 255), 0, static_cast<int>((1 - relZ) * 255)}); }); }
int ObjectAspect::calculate(const cv::Mat &image, const PointCloud::Ptr &pointcloud) { cv::Mat gray,timg; image.copyTo(this->image); this->points = pointcloud; #if 1 cv::cvtColor(image,gray,CV_BGR2HLS); //gray = planes[2]-planes[1]; std::vector<cv::Mat> planes; cv::split(gray,planes); gray = planes[1]; cv::merge(&planes[0],(size_t)3,timg); cv::cvtColor(timg,timg,CV_HSV2BGR); #else timg = image; #endif #ifdef DEBUG_VIS cv::cvtColor(gray,timg,CV_GRAY2BGR); #endif const int HessianThreshold = 500; cv::SURF bug(HessianThreshold); bug.extended = false; bug(gray,255*cv::Mat::ones(gray.rows,gray.cols,CV_8U),keypoints,descriptors); for(size_t i = 0; i < keypoints.size(); i++) { cv::KeyPoint kp = keypoints[i]; PointType pt3d = pointcloud->at(kp.pt.x,kp.pt.y); pt3d.rgb = kp.response; if(!isnan(pt3d.x)/*&&(kp.response/1000>3)*/) { keypoints3D->points.push_back(pt3d); map2D3D.insert(std::make_pair<int,int>(i,keypoints3D->points.size()-1)); map2D3Dinv.insert(std::make_pair<int,int>(keypoints3D->points.size()-1,i)); } } #ifdef DEBUG_VIS plotKeypoints(timg); cv::namedWindow("surf"); cv::imshow("surf",timg); #endif return 0; }
void App::VisOriginal(TUMDataSetVisualizer * viewer) { viewer->EasyAdd(Input_, "input", [this] (int i) { return Color(Input_->at(i)); }); }
bool send2(brio_assembly_vision_new::TrasformStampedRequest &req, brio_assembly_vision_new::TrasformStampedResponse &res) { request_a_new_cluster =true; if(find_cloud_from_kinect_head==true && final ==false) { brio_assembly_vision_new::Container * cont = new brio_assembly_vision_new::Container(); while((cont=client_call())==NULL) { std::this_thread::sleep_for (std::chrono::seconds(1)); //wait for a good response } if(first_time==true) { initial_cluster_size=cont->date_container.size(); } first_time=false; if(cont->date_container.size()>0) //after moving objects size is decreased { int i=0; //start with the lowest index std::string object_with_shape_requested = model[model_step]; while(cont->date_container[i].piece_type!=object_with_shape_requested && i<cont->date_container.size()-1) // { i++; } //end while when cluster_vector[i] has the requested shape or when the search index is bigger then cluster_vector if(i<=cont->date_container.size()-1) // if i<=cluster_vector.size() then we did find requested object at indices i { Eigen::Vector3d z_axe,x_or_y_axe; Eigen::Vector4d translation; z_axe=estimate_plane_normals(cloud); int i_center = cont->date_container[i].center_index; int i_head = cont->date_container[i].head_conn_index; x_or_y_axe= obj_axe_2_points_next(i_head,i_center); pcl::PointXYZRGB center = cloud->at(cont->date_container[i].center_index); translation[0] = center.x; translation[1] = center.y; translation[2] = center.z; calculate_transformation(x_or_y_axe,z_axe ,translation); ROS_INFO("Send TransformedPose"); Eigen::Quaternionf quat; quat = createQuaternion(); res.msg.child_frame_id = "brio_piece_frame"; res.msg.header.frame_id = "head_mount_kinect_rgb_optical_frame"; res.msg.transform.translation.x = transformata_finala(0,3); res.msg.transform.translation.y = transformata_finala(1,3); res.msg.transform.translation.z = transformata_finala(2,3); res.msg.transform.rotation.w = (double)quat.w(); res.msg.transform.rotation.x = (double)quat.x(); res.msg.transform.rotation.y = (double)quat.y(); res.msg.transform.rotation.z = (double)quat.z(); if(model_step<initial_cluster_size-1) model_step++; else { final=true; //return false; } return true; }