void Detector_Seg::ComputeFreespace(const Camera& camera,
Matrix<int>& occ_map_binary,
Matrix<int>& mat_2D_pos_x, Matrix<int>& mat_2D_pos_y,
const PointCloud &point_cloud,
Matrix<double>& occ_map)
{
// Set Freespace Parameters
double scale_z_ = Globals::freespace_scaleZ;
double scale_x_ = Globals::freespace_scaleX;
double min_x_ = Globals::freespace_minX;
double min_z_ = Globals::freespace_minZ;
double max_x_ = Globals::freespace_maxX;
double max_z_ = Globals::freespace_maxZ;
int x_bins = (int)round((max_x_ - min_x_)*scale_x_)+1;
int z_bins = (int)round((max_z_ - min_z_)*scale_z_)+1;
occ_map.set_size(x_bins, z_bins, 0.0);
occ_map_binary.set_size(x_bins, z_bins);
double step_x = (max_x_ - min_x_)/double(x_bins-1);
double step_z = (max_z_ - min_z_)/double(z_bins-1);
Vector<double> plane_parameters = AncillaryMethods::PlaneToCam(camera);
double plane_par_0 = plane_parameters(0);
double plane_par_1 = plane_parameters(1);
double plane_par_2 = plane_parameters(2);
double plane_par_3 = plane_parameters(3)*Globals::WORLD_SCALE;
double log_2 = log(2);
for(int j = 0; j < point_cloud.X.getSize(); j++)
{
double zj = point_cloud.Z(j);
if(zj < Globals::freespace_max_depth_to_cons && zj >= 0.1)
{
double xj = point_cloud.X(j);
double yj = point_cloud.Y(j);
double distance_to_plane = plane_par_0*xj + plane_par_1*yj + plane_par_2*zj + plane_par_3;
// Only accept points in upper_body height range (0.1m to 2.5m)
if(distance_to_plane < Globals::freespace_max_height && distance_to_plane > 0.1)
{
double x = xj - min_x_;
double z = zj - min_z_;
int pos_x = (int)round(x / step_x);
int pos_z = (int)round(z / step_z);
if(Globals::log_weight)
occ_map(pos_x, pos_z) += z*(log(z) / log_2);
else
occ_map(pos_x, pos_z) += z;
mat_2D_pos_x.data()[j] = pos_x;
mat_2D_pos_y.data()[j] = pos_z;
}
}
}
int occ_map_length = x_bins*z_bins;
for(int i = 0; i < occ_map_length; i++)
{
if(occ_map.data()[i] < Globals::freespace_threshold)
{
occ_map_binary.data()[i] = 0;
occ_map.data()[i] = 0;
}
else
occ_map_binary.data()[i] = 1;
}
}
void Detector::ComputeFreespace(const Camera& camera,
Matrix<int>& occ_map_binary,
Matrix<int>& mat_2D_pos_x, Matrix<int>& mat_2D_pos_y,
const PointCloud &point_cloud)
{
// Set Freespace Parameters
double scale_z_ = Globals::freespace_scaleZ;
double scale_x_ = Globals::freespace_scaleX;
double min_x_ = Globals::freespace_minX;
double min_z_ = Globals::freespace_minZ;
double max_x_ = Globals::freespace_maxX;
double max_z_ = Globals::freespace_maxZ;
int x_bins = (int)round((max_x_ - min_x_)*scale_x_)+1;
int z_bins = (int)round((max_z_ - min_z_)*scale_z_)+1;
Matrix<double> occ_map(x_bins, z_bins, 0.0);
occ_map_binary.set_size(x_bins, z_bins);
double step_x = (max_x_ - min_x_)/double(x_bins-1);
double step_z = (max_z_ - min_z_)/double(z_bins-1);
Vector<double> plane_parameters = camera.get_GP();
double plane_par_0 = plane_parameters(0);
double plane_par_1 = plane_parameters(1);
double plane_par_2 = plane_parameters(2);
double plane_par_3 = plane_parameters(3)*Globals::WORLD_SCALE;
// double log_2 = log(2);
for(int j = 0; j < point_cloud.X.getSize(); j++)
{
double zj = point_cloud.Z(j);
if(zj < Globals::freespace_max_depth_to_cons && zj >= 0.1)
{
double xj = point_cloud.X(j);
double yj = point_cloud.Y(j);
double distance_to_plane = plane_par_0*xj + plane_par_1*yj + plane_par_2*zj + plane_par_3;
// Only accept points in upper_body height range (0.1m to 2.5m)
if(distance_to_plane < 2.5 && distance_to_plane > 0.1)
{
double x = xj - min_x_;
double z = zj - min_z_;
int pos_x = (int)round(x / step_x);
int pos_z = (int)round(z / step_z);
// occ_map(pos_x, pos_z) += z*(log(z) / log_2);
occ_map(pos_x, pos_z) += z;
mat_2D_pos_x.data()[j] = pos_x;
mat_2D_pos_y.data()[j] = pos_z;
}
}
}
// occ_map.WriteToTXT("before.txt");
Vector<double> kernel = AncillaryMethods::getGaussian1D(2,3);
occ_map = AncillaryMethods::conv1D(occ_map, kernel, false);
occ_map = AncillaryMethods::conv1D(occ_map, kernel, true);
// occ_map.WriteToTXT("after.txt");
int occ_map_length = x_bins*z_bins;
for(int i = 0; i < occ_map_length; i++)
{
occ_map_binary.data()[i] = (occ_map.data()[i] < Globals::freespace_threshold) ? 0 : 1;
}
}
