void Detector_Seg::GetROIs(const Camera &camera, const Matrix<double> &depth_map, Matrix<int>& occ_map_binary, const PointCloud &point_cloud) //Added { int width = depth_map.x_size(); int height = depth_map.y_size(); Matrix<int> mat_2D_pos_x(width, height, -1); Matrix<int> mat_2D_pos_y(width, height, -1); // Compute 2D_positions, and occ_Map matrices ComputeFreespace(camera, occ_map_binary, mat_2D_pos_x, mat_2D_pos_y, point_cloud, occ_map); Vector<ROI_SEG> all_ROIs; PreprocessROIs(occ_map_binary, all_ROIs); Vector<SegmentedObj> all_objs; //all objects in image SegmentationROI::RunSegmentation(all_objs, occ_map, occ_map_binary, all_ROIs, mat_2D_pos_x, mat_2D_pos_y, point_cloud.X, point_cloud.Y, point_cloud.Z); std::cout << "size all_objs vector : " << all_objs.getSize() << std::endl; std::cout << "size all_ROIs vector : " << all_ROIs.getSize() << std::endl; }
void Detector_Seg::ProcessFrame(const Camera &camera, const Matrix<double> &depth_map, const PointCloud &point_cloud, const Matrix<double> &upper_body_template, Vector<Vector<double> > &detected_bounding_boxes) { int width = depth_map.x_size(); int height = depth_map.y_size(); Matrix<int> mat_2D_pos_x(width, height, -1); Matrix<int> mat_2D_pos_y(width, height, -1); // Compute 2D_positions, and occ_Map matrices ComputeFreespace(camera, labeledROIs, mat_2D_pos_x, mat_2D_pos_y, point_cloud, occ_map); Vector<ROI_SEG> all_ROIs; PreprocessROIs(labeledROIs, all_ROIs); Vector<SegmentedObj> all_objs; //SegmentationROI::RunSegmentation(all_objs, occ_map, labeledROIs, all_ROIs, mat_2D_pos_x, mat_2D_pos_y, point_cloud.X, point_cloud.Y, point_cloud.Z); SegmentationROI::RunSegmentation(all_objs,roiInHist_view,roi_reshaped, occ_map, labeledROIs, all_ROIs, mat_2D_pos_x, mat_2D_pos_y, point_cloud.X, point_cloud.Y, point_cloud.Z); Camera camera_origin = AncillaryMethods::GetCameraOrigin(camera); Vector<double> plane_in_camera = camera_origin.get_GP(); Vector<Vector<double> > close_range_BBoxes; Vector<Vector<double > > distances; Vector<double> bb(4); Vector<double> distance(2); for(int i=0; i<all_objs.getSize(); ++i) { double height = AncillaryMethods::DistanceToPlane(all_objs(i).pos3DMinY, plane_in_camera); double mi = all_objs(i).pos3DMinZ(2), mx = all_objs(i).pos3DMaxZ(2); double cz = (mx+mi)/2, sz = mx-mi; if((cz-sz/2.0) < Globals::distance_range_accepted_detections && height < Globals::max_height && height > Globals::min_height) { if(all_objs(i).get2DBoundingBoxInImagePlane(bb,camera_origin)) { close_range_BBoxes.pushBack(bb); distance(0) = cz; distance(1) = sz; distances.pushBack(distance); } } } detected_bounding_boxes = EvaluateTemplate(upper_body_template, depth_map, close_range_BBoxes, distances); // // Trial to change roi_image to show ground plane with detected upper bodies // //Upper_body positions shall be in Vector<Vector<double> > max_pos; // Vector<double> pos2D; // Vector<double> pos3D(3); // for (nb_objects = 0;nb_objects < detected_bounding_boxes.getSize(); ++nb_objects) // { // pos2D.pushBack(detected_bounding_boxes(nb_objects)(0)+detected_bounding_boxes(nb_objects)(2)/2.0); // pos2D.pushBack(detected_bounding_boxes(nb_objects)(1)+detected_bounding_boxes(nb_objects)(3)/2.0); // pos2D.pushBack(1); // // // } // // // std::cout << "mat_2D_pos_x.x_size() : " << mat_2D_pos_x.y_size() << std::endl; // std::cout << "mat_2D_pos_x.y_size() : " << mat_2D_pos_x.y_size() << std::endl; // std::cout << "roi_image.x_size() : " << roi_image.x_size() << std::endl; // std::cout << "roi_image.y_size() : " << roi_image.y_size() << std::endl; // // for (int i = 0; i < close_range_BBoxes.getSize(); ++i) // { // int cropped_height = (int)(close_range_BBoxes(i)(3)/2.0); // cropped_height += (close_range_BBoxes(i)(3) * Globals::evaluation_inc_height_ratio)/2.0; // int start_column = (int)close_range_BBoxes(i)(0); // int end_column = (int)(close_range_BBoxes(i)(0) + close_range_BBoxes(i)(2)); // int start_row = (int)max(0.0, close_range_BBoxes(i)(1)); // int end_row = (int)close_range_BBoxes(i)(1) + cropped_height; // //////////////// just for test (must be removed) // if(visualize_roi) // for(int tmpx=start_column, tmpxx=0; tmpxx<mat_2D_pos_x.x_size(); ++tmpx,++tmpxx) // { // for(int tmpy=start_row, tmpyy=0; tmpyy<mat_2D_pos_x.y_size(); ++tmpy,++tmpyy) // { // if(tmpyy==0 || tmpyy==mat_2D_pos_x.y_size()-1 || tmpxx==0 || tmpxx==mat_2D_pos_x.x_size()-1) //if on the border of the image // roi_image(tmpx,tmpy)=i+1; // // if(mat_2D_pos_x(tmpxx,tmpyy)!=0) // roi_image(tmpx,tmpy)=i+1; // } // } // } }
void Detector::ProcessFrame(const Camera &camera_origin, const Matrix<double> &depth_map, const PointCloud &point_cloud, const Matrix<double> &upper_body_template, Vector<Vector<double> > &detected_bounding_boxes) { int width = depth_map.x_size(); int height = depth_map.y_size(); // Matrix<int> labeledROIs; Matrix<int> mat_2D_pos_x(width, height, -1); Matrix<int> mat_2D_pos_y(width, height, -1); // Compute 2D_positions, and occ_Map matrices ComputeFreespace(camera_origin, labeledROIs, mat_2D_pos_x, mat_2D_pos_y, point_cloud); Vector<ROI> all_ROIs; PreprocessROIs(labeledROIs, all_ROIs); // Vector<Vector<Vector<double> > > points3D_in_ROIs(all_ROIs.getSize()); ExtractPointsInROIs(/*points3D_in_ROIs*/all_ROIs, mat_2D_pos_x, mat_2D_pos_y, point_cloud, labeledROIs); double scaleZ = Globals::freespace_scaleZ; Vector<double> plane_in_camera = camera_origin.get_GP(); Vector<Vector<double> > close_range_BBoxes; Vector<Vector<double > > distances; Vector<double> lower_point(3); for(int l = 0; l < all_ROIs.getSize(); l++) { // if(all_ROIs(l).getSize() > 0) if(all_ROIs(l).has_any_point) { // int min_y_index = 0; // AncillaryMethods::MinVecVecDim(points3D_in_ROIs(l), 1, min_y_index); // Vector<double> lower_point = points3D_in_ROIs(l)(min_y_index); lower_point(0) = point_cloud.X(all_ROIs(l).ind_min_y); lower_point(1) = point_cloud.Y(all_ROIs(l).ind_min_y); lower_point(2) = point_cloud.Z(all_ROIs(l).ind_min_y); double height = plane_in_camera(0)*lower_point(0) + plane_in_camera(1)*lower_point(1) + plane_in_camera(2)*lower_point(2) + plane_in_camera(3)*Globals::WORLD_SCALE; ROS_DEBUG_STREAM("Checking height: " << Globals::min_height << " < " << height << " < " << Globals::max_height); ROS_DEBUG_STREAM("Checking distance: " << (all_ROIs(l).center_y()-all_ROIs(l).width()/2.0)/scaleZ << " < " << Globals::distance_range_accepted_detections); if((all_ROIs(l).center_y()-all_ROIs(l).width()/2.0)/scaleZ < Globals::distance_range_accepted_detections && height > Globals::min_height && height < Globals::max_height) { Vector<double> bbox(4, 0.0); // if there is a valid bounding box // if( ROI::GetRegion2D(bbox, camera_origin, points3D_in_ROIs(l)) ) if(all_ROIs(l).GetRegion2D(bbox, camera_origin, point_cloud) ) { close_range_BBoxes.pushBack(bbox); Vector<double> distance(2); distance(0) = all_ROIs(l).center_y()/scaleZ; distance(1) = all_ROIs(l).height()/scaleZ; distances.pushBack(distance); } } } } detected_bounding_boxes = EvaluateTemplate(upper_body_template, depth_map, close_range_BBoxes, distances); }