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