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);
}
sentibyte_geometry::sentibyte_geometry(sb_ptr &sb, vec4 b)
{
base_point = b;
host = sb;
sb_color[0] = randomFloat(0.0f, 1.0f, 2);
sb_color[1] = randomFloat(0.0f, 1.0f, 2);
sb_color[2] = randomFloat(0.0f, 1.0f, 2);
signed short radial_divisions = host->getTraitCount();
mat4 rotation_matrix = glm::rotate(mat4(1.0f), 360.0f / float(radial_divisions), vec3(0.0f, 0.0f, 1.0f));
for (int i = 0; i < host->getTraitCount(); i++)
{
float outline_thickness = .02f;
vec4 max_point(0.0f, 1.0, 0.0f, 1.0f);
vec4 outline_point(0.0f, 1.0 + outline_thickness, 0.0f, 1.0f);
// rotate point around the center axis
for (int n = 0; n < i; n++)
{
max_point = rotation_matrix * max_point;
outline_point = rotation_matrix * outline_point;
}
mat4 base_translation = glm::translate(
glm::mat4(1.0f), vec3(base_point.x, base_point.y, base_point.z));
max_point = base_translation * max_point;
outline_point = base_translation * outline_point;
max_levels.push_back(max_point);
outline.push_back(outline_point);
}
map<string, value_state> trait_map = host->getTraits();
signed short trait_number = 0;
for (map<string, value_state>::const_iterator it = trait_map.begin();
it != trait_map.end(); it++)
{
string trait_name = it->first;
float base_coefficient = it->second[VS_BASE] / 100.0f;
float upper_coefficient = it->second[VS_UPPER] / 100.0f;
float lower_coefficient = it->second[VS_LOWER] / 100.0f;
vec4 base_level_point(0.0f, base_coefficient, 0.0f, 1.0f);
vec4 lower_point(0.0f, lower_coefficient, 0.0f, 1.0f);
vec4 upper_point(0.0f, upper_coefficient, 0.0f, 1.0f);
// rotate point around the center axis
for (int i = 0; i < trait_number; i++)
{
base_level_point = rotation_matrix * base_level_point;
lower_point = rotation_matrix * lower_point;
upper_point = rotation_matrix * upper_point;
}
mat4 base_translation = glm::translate(
glm::mat4(1.0f), vec3(base_point.x, base_point.y, base_point.z));
base_level_point = base_translation * base_level_point;
lower_point = base_translation * lower_point;
upper_point = base_translation * upper_point;
base_levels[trait_name] = base_level_point;
upper_bounds[trait_name] = upper_point;
lower_bounds[trait_name] = lower_point;
trait_number++;
}
}