-
Iván del Pino authoredIván del Pino authored
kf_based_terrain_analysis.cpp 65.20 KiB
#include "kf_based_terrain_analysis.h"
CKf_Based_Terrain_Analysis::CKf_Based_Terrain_Analysis()
{
performance_supervisor_ptr_ = boost::make_shared<CTimeSupervisor>();
}
CKf_Based_Terrain_Analysis::~CKf_Based_Terrain_Analysis()
{
}
void CKf_Based_Terrain_Analysis::showPerformanceStatistics(void)
{
performance_supervisor_ptr_->printStatistics();
}
float CKf_Based_Terrain_Analysis::computeTriangleArea(const Eigen::Vector3d vertex_1, const Eigen::Vector3d vertex_2,
const Eigen::Vector3d vertex_3)
{
float area = 0.0;
Eigen::Matrix3d triangle = Eigen::Matrix3d::Ones();
triangle.row(0) = vertex_1;
triangle.row(1) = vertex_2;
triangle.row(2) = vertex_3;
area = 0.5 * triangle.determinant();
return (area);
}
bool CKf_Based_Terrain_Analysis::checkInside(
const kf_based_terrain_analysis_lib::Coordinates2D coord,
const std::vector<kf_based_terrain_analysis_lib::Coordinates2D> &vertex_list)
{
bool inside = false;
//Point to evaluate
Eigen::Vector3d p = Eigen::Vector3d::Ones();
p(0) = coord.x;
p(1) = coord.y;
//Vertex
Eigen::Vector3d p1 = Eigen::Vector3d::Ones();
p1(0) = vertex_list[0].x;
p1(1) = vertex_list[0].y;
Eigen::Vector3d p2 = Eigen::Vector3d::Ones();
p2(0) = vertex_list[1].x;
p2(1) = vertex_list[1].y;
Eigen::Vector3d p3 = Eigen::Vector3d::Ones();
p3(0) = vertex_list[2].x;
p3(1) = vertex_list[2].y;
Eigen::Vector3d p4 = Eigen::Vector3d::Ones();
p4(0) = vertex_list[3].x;
p4(1) = vertex_list[3].y;
//We will evaluate the areas of the for triangles that can be form using the
//four pairs of rectangle vertex and the evaluated point, if all the areas computed
//using a determinant are positive or negative, the evaluated point is inside
//the rectangle (this formula works for any arbitrary convex region on a plane)
float a1 = computeTriangleArea(p1, p2, p);
float a2 = computeTriangleArea(p2, p3, p);
float a3 = computeTriangleArea(p3, p4, p);
float a4 = computeTriangleArea(p4, p1, p);
if ((a1 < 0.0 && a2 < 0.0 && a3 < 0.0 && a4 < 0.0) || (a1 > 0.0 && a2 > 0.0 && a3 > 0.0 && a4 > 0.0))
{
inside = true;
}
return (inside);
}
void CKf_Based_Terrain_Analysis::calculateVertexCoordinates(
const kf_based_terrain_analysis_lib::Robot2DFootprint footprint,
std::vector<kf_based_terrain_analysis_lib::Coordinates2D> &vertex_list, int &closest_vertex_index)
{
// Vertex coordinates in object frame
Eigen::Vector3d p1 = Eigen::Vector3d::Ones();
p1(0) = footprint.dimensions.x / 2.0;
p1(1) = footprint.dimensions.y / 2.0;
Eigen::Vector3d p2 = Eigen::Vector3d::Ones();
p2(0) = footprint.dimensions.x / 2.0;
p2(1) = -1 * footprint.dimensions.y / 2.0;
Eigen::Vector3d p3 = Eigen::Vector3d::Ones();
p3(0) = -1 * footprint.dimensions.x / 2.0;
p3(1) = -1 * footprint.dimensions.y / 2.0;
Eigen::Vector3d p4 = Eigen::Vector3d::Ones();
p4(0) = -1 * footprint.dimensions.x / 2.0;
p4(1) = footprint.dimensions.y / 2.0;
// Rotation matrix
Eigen::Matrix3d transform = Eigen::Matrix3d::Identity();
transform(0, 0) = cos(footprint.yaw);
transform(0, 1) = -1 * sin(footprint.yaw);
transform(0, 2) = footprint.centroid.x;
transform(1, 0) = sin(footprint.yaw);
transform(1, 1) = cos(footprint.yaw);
transform(1, 2) = footprint.centroid.y;
p1 = transform * p1;
p2 = transform * p2;
p3 = transform * p3;
p4 = transform * p4;
kf_based_terrain_analysis_lib::Coordinates2D p;
p.x = p1(0);
p.y = p1(1);
vertex_list.push_back(p);
p.x = p2(0);
p.y = p2(1);
vertex_list.push_back(p);
p.x = p3(0);
p.y = p3(1);
vertex_list.push_back(p);
p.x = p4(0);
p.y = p4(1);
vertex_list.push_back(p);
// Find closest vertex
float min_distance = 1000000.0;
float x = p1(0);
float y = p1(1);
float dp = sqrt(x * x + y * y);
if (dp < min_distance)
{
min_distance = dp;
closest_vertex_index = 0; }
x = p2(0);
y = p2(1);
dp = sqrt(x * x + y * y);
if (dp < min_distance)
{
min_distance = dp;
closest_vertex_index = 1;
}
x = p3(0);
y = p3(1);
dp = sqrt(x * x + y * y);
if (dp < min_distance)
{
min_distance = dp;
closest_vertex_index = 2;
}
x = p4(0);
y = p4(1);
dp = sqrt(x * x + y * y);
if (dp < min_distance)
{
min_distance = dp;
closest_vertex_index = 3;
}
}
void CKf_Based_Terrain_Analysis::initizalizeShadowAreaGroundReferencePoint(
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration, const float x, const float y,
const int index, pcl::PointXYZRGBNormal &ground_point)
{
ground_point.x = x;
ground_point.y = y;
ground_point.z = -1.0 * lidar_configuration.sensor_height; // we set the prior
ground_point.r = 0; // we set these values to cero because it corresponds to cero cost, however
ground_point.g = 0; // they will be later properly computed (with the exception of point at origin of sensor frame
ground_point.b = 0; // because this point is already reached, so it has to be a zero cost point
ground_point.data_n[GRND_REF_DATA_N_2_GOAL_COST] = COST_UNKNOWN;
ground_point.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] = (float)index;
// we set the prior
ground_point.data_c[GRND_REF_DATA_C_1_ROLL] = 0.0;
ground_point.data_c[GRND_REF_DATA_C_2_PITCH] = 0.0;
ground_point.data_c[GRND_REF_DATA_C_3_Z_VARIANCE] = filtering_configuration.z_initial_std_dev
* filtering_configuration.z_initial_std_dev;
ground_point.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE] = filtering_configuration.roll_initial_std_dev
* filtering_configuration.roll_initial_std_dev;
ground_point.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE] = filtering_configuration.pitch_initial_std_dev
* filtering_configuration.pitch_initial_std_dev;
//std::cout << "initizalizeShadowAreaGroundReferencePoint: r = " << (int)ground_point.r << " g = " << (int)ground_point.g
// << " b = " << (int)ground_point.b << std::endl;
}
void CKf_Based_Terrain_Analysis::extractPointsInROI(
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
const pcl::PointXYZRGBNormal reference, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr points_in_ROI,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr references_in_ROI, float &search_range)
{
if (filtering_configuration.measure_performance) CFunctionMonitor performance_monitor("extractPointsInROI", performance_supervisor_ptr_);
//std::cout << "Extracting points in ROI: reference.x = " << reference.x << " reference.y = " << reference.y
// << " reference.z = " << reference.z << std::endl;
if ((int)round(reference.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]) == INDEX_UNKNOWN
|| (int)round(reference.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]) == 0)
{
search_range = filtering_configuration.search_limit_in_shadow_area;
//std::cout << "Using shadow area reference point! " << std::endl;
//std::cout << "search_range = " << search_range << std::endl;
}
else
{
float r = sqrt(reference.x * reference.x + reference.y * reference.y);
float alpha = atan2(lidar_configuration.sensor_height, r);
float beta = alpha - (filtering_configuration.angle_reduction_factor * M_PI / 180.0);
if (beta < 1.0 * M_PI / 180.0)
beta = 1.0 * M_PI / 180.0;
//std::cout << "r = " << r << " alpha = " << alpha * 180.0 / M_PI << " beta = " << beta * 180.0 / M_PI
// << std::endl;
float r2 = lidar_configuration.sensor_height / tan(beta);
search_range = r2 - r;
if (search_range < filtering_configuration.search_limit_in_shadow_area)
search_range = filtering_configuration.search_limit_in_shadow_area;
//std::cout << "r2 = " << r2 << " search_range = " << r2 - r << std::endl;
//std::cout << "saturated_search_range = " << search_range << std::endl;
assert(search_range > 0.0 && "negative search range!");
}
float x_min = reference.x - search_range;
float y_min = reference.y - search_range;
float x_max = reference.x + search_range;
float y_max = reference.y + search_range;
// Extracting lidar points
pcl::PassThrough < pcl::PointXYZRGBNormal > pass;
pass.setInputCloud(elevation_cloud_ptr);
pass.setFilterFieldName("x");
pass.setFilterLimits(x_min, x_max);
pass.filter(*points_in_ROI);
pass.setInputCloud(points_in_ROI);
pass.setFilterFieldName("y");
pass.setFilterLimits(y_min, y_max);
pass.filter(*points_in_ROI);
//std::cout << "X limits = " << x_min << " ; " << x_max << std::endl;
//std::cout << "Y limits = " << y_min << " ; " << y_max << std::endl;
//std::cout << "Z ground ref = " << reference.z << std::endl;
//getchar();
pass.setInputCloud(ground_reference_cloud_ptr);
pass.setFilterFieldName("x");
pass.setFilterLimits(x_min, x_max);
pass.filter(*references_in_ROI);
pass.setInputCloud(references_in_ROI);
pass.setFilterFieldName("y");
pass.setFilterLimits(y_min, y_max);
pass.filter(*references_in_ROI);
}
float CKf_Based_Terrain_Analysis::euclideanDistance(const pcl::PointXYZRGBNormal reference_in_sensor_frame, const pcl::PointXYZRGBNormal point_in_sensor_frame)
{
CFunctionMonitor performance_monitor("euclideanDistance", performance_supervisor_ptr_);
// Expectation
float roll = reference_in_sensor_frame.data_c[GRND_REF_DATA_C_1_ROLL];
float pitch = reference_in_sensor_frame.data_c[GRND_REF_DATA_C_2_PITCH];
float delta_x = point_in_sensor_frame.x - reference_in_sensor_frame.x;
float delta_y = point_in_sensor_frame.y - reference_in_sensor_frame.y;
float prediction = reference_in_sensor_frame.z + delta_x * pitch + delta_y * roll;
// Innovation
float observation = point_in_sensor_frame.z;
float innovation = observation - prediction;
return (innovation);
}
float CKf_Based_Terrain_Analysis::computeLikelihood(const pcl::PointXYZRGBNormal reference_in_sensor_frame,
const pcl::PointXYZRGBNormal point_in_sensor_frame)
{
float z_pred = 0.0;
float var_z_pred = 0.0;
float delta_x = point_in_sensor_frame.x - reference_in_sensor_frame.x;
float delta_y = point_in_sensor_frame.y - reference_in_sensor_frame.y;
predictZWithVariance(reference_in_sensor_frame, delta_x, delta_y, z_pred, var_z_pred);
float sigma = sqrt(var_z_pred);
float regularization_term = 1.0 / (sigma * 2.506628275);
float likelihood = regularization_term
* (exp(-1.0 * ((point_in_sensor_frame.z - z_pred) * (point_in_sensor_frame.z - z_pred)) / (2 * var_z_pred)));
//DEBUG!float likelihood = ((point_in_sensor_frame.z - z_pred) * (point_in_sensor_frame.z - z_pred)) / (2 * var_z_pred);
if (false) //point_in_sensor_frame.z < -1.75)
{
std::cout << "delta_x = " << delta_x << std::endl;
std::cout << "delta_y = " << delta_y << std::endl;
std::cout << "z_pred = " << z_pred << std::endl;
std::cout << "var_z_pred = " << var_z_pred << std::endl;
std::cout << "sigma = " << sigma << std::endl;
std::cout << "regularization_term = " << regularization_term << std::endl;
std::cout << "z observed = " << point_in_sensor_frame.z << std::endl;
std::cout << "likelihood = " << likelihood << std::endl;
std::cout << "reference_in_sensor_frame.x = " << reference_in_sensor_frame.x << " reference_in_sensor_frame.y = "
<< reference_in_sensor_frame.y << " reference_in_sensor_frame.z = " << reference_in_sensor_frame.z
<< std::endl;
getchar();
}
return (likelihood);
}
float CKf_Based_Terrain_Analysis::mahalanobisDistance(const pcl::PointXYZRGBNormal reference_in_sensor_frame,
const pcl::PointXYZRGBNormal point_in_sensor_frame,
float &predicted_std_dev)
{
//CFunctionMonitor performance_monitor("mahalanobisDistance", performance_supervisor_ptr_);
//std::cout << "<------- Exectuting CKf_Based_Terrain_Analysis::mahalanobisDistance... --------->" << std::endl;
float prediction_fast = reference_in_sensor_frame.z + (point_in_sensor_frame.x - reference_in_sensor_frame.x)
* reference_in_sensor_frame.data_c[GRND_REF_DATA_C_2_PITCH]
+ (point_in_sensor_frame.y - reference_in_sensor_frame.y)
* reference_in_sensor_frame.data_c[GRND_REF_DATA_C_1_ROLL];
float prediction_std_fast = sqrt(
reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]
+ (point_in_sensor_frame.x - reference_in_sensor_frame.x)
* (point_in_sensor_frame.x - reference_in_sensor_frame.x)
* reference_in_sensor_frame.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE]
+ (point_in_sensor_frame.y - reference_in_sensor_frame.y)
* (point_in_sensor_frame.y - reference_in_sensor_frame.y)
* reference_in_sensor_frame.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE]);
float mahalanobis_distance_fast = fabs(point_in_sensor_frame.z - prediction_fast) / prediction_std_fast;
predicted_std_dev = prediction_std_fast;
//assert(!std::isnan(mahalanobis_distance_fast));
if (std::isnan(mahalanobis_distance_fast))
{
std::cout << "ref (x,y,z) = " << reference_in_sensor_frame.x << ", " << reference_in_sensor_frame.y << ", "
<< reference_in_sensor_frame.z << std::endl;
std::cout << "point (x,y,z) = " << point_in_sensor_frame.x << ", " << point_in_sensor_frame.y << ", "
<< point_in_sensor_frame.z << std::endl;
std::cout << "ref (z, sz, a, sa, b, sb) = " << reference_in_sensor_frame.z << ", "
<< reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE] << ", "
<< reference_in_sensor_frame.data_c[GRND_REF_DATA_C_2_PITCH] << ", "
<< reference_in_sensor_frame.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE] << ", "
<< reference_in_sensor_frame.data_c[GRND_REF_DATA_C_1_ROLL] << ", "
<< reference_in_sensor_frame.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE] << std::endl;
}
return (mahalanobis_distance_fast);
}
float CKf_Based_Terrain_Analysis::predictZ(const pcl::PointXYZRGBNormal reference, const float delta_x,
const float delta_y)
{
//CFunctionMonitor performance_monitor("predictZ", performance_supervisor_ptr_);
float roll_slope = reference.data_c[GRND_REF_DATA_C_1_ROLL];
float pitch_slope = reference.data_c[GRND_REF_DATA_C_2_PITCH];
float predicted_z = reference.z + delta_x * pitch_slope + delta_y * roll_slope;
return (predicted_z);
}
void CKf_Based_Terrain_Analysis::predictZWithVariance(const pcl::PointXYZRGBNormal reference, const float delta_x,
const float delta_y, float &z_predicted, float &var_z_predicted)
{
CFunctionMonitor performance_monitor("predictZWithVariance", performance_supervisor_ptr_);
float z_local_frame = reference.z;
float roll_slope = reference.data_c[GRND_REF_DATA_C_1_ROLL];
float pitch_slope = reference.data_c[GRND_REF_DATA_C_2_PITCH];
z_predicted = z_local_frame + delta_x * pitch_slope + delta_y * roll_slope;
Eigen::Vector3f partial_derivatives;
partial_derivatives(0) = 1;
partial_derivatives(1) = delta_x;
partial_derivatives(2) = delta_y;
Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
cov(0, 0) = reference.data_c[GRND_REF_DATA_C_3_Z_VARIANCE];
cov(1, 1) = reference.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE];
cov(2, 2) = reference.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE];
var_z_predicted = partial_derivatives.transpose() * cov * partial_derivatives;
}
void CKf_Based_Terrain_Analysis::estimateTangentPlane(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr observations_for_tangent_plane_estimation,
const double measurement_std_dev, pcl::PointXYZRGBNormal &reference_in_sensor_frame)
{
//CFunctionMonitor performance_monitor("estimateTangentPlane", performance_supervisor_ptr_);
Eigen::Vector3d x;
x << (double)reference_in_sensor_frame.z, (double)reference_in_sensor_frame.data_c[GRND_REF_DATA_C_2_PITCH], (double)reference_in_sensor_frame.data_c[GRND_REF_DATA_C_1_ROLL];
Eigen::Matrix3d P;
P << (double)reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE], 0.0, 0.0, 0.0, (double)reference_in_sensor_frame.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE], 0.0, 0.0, 0.0, (double)reference_in_sensor_frame.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE];
//std::cout << "x prior = " << std::endl << x << std::endl;
//std::cout << "P prior = " << std::endl << P << std::endl;
Eigen::RowVector3d H;
Eigen::Vector3d K;
double r = measurement_std_dev * measurement_std_dev;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = observations_for_tangent_plane_estimation->begin();
i != observations_for_tangent_plane_estimation->end(); ++i)
{
H << (double)1.0, (i->x - (double)reference_in_sensor_frame.x), (i->y - (double)reference_in_sensor_frame.y);
//std::cout << "H = " << std::endl << H << std::endl;
K = P * H.transpose() / (H * P * H.transpose() + r);
//std::cout << "K = " << std::endl << K << std::endl;
x = x + K * (i->z - (H * x));
P = P - K * H * P;
//std::cout << "x posterior = " << std::endl << x << std::endl;
//std::cout << "P posterior = " << std::endl << P << std::endl;
}
reference_in_sensor_frame.z = (float)x(0);
reference_in_sensor_frame.data_c[GRND_REF_DATA_C_2_PITCH] = (float)x(1);
reference_in_sensor_frame.data_c[GRND_REF_DATA_C_1_ROLL] = (float)x(2);
reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE] = (float)P(0, 0);
reference_in_sensor_frame.data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE] = (float)P(1, 1);
reference_in_sensor_frame.data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE] = (float)P(2, 2);
//std::cout << "x posterior = " << std::endl << x << std::endl;
//std::cout << "P posterior = " << std::endl << P << std::endl;
//getchar();
}
bool CKf_Based_Terrain_Analysis::checkCollisionInPath(
const pcl::PointXYZRGBNormal vertex_a, const pcl::PointXYZRGBNormal vertex_b,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr points_in_ROI, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr,
const std::vector<std::vector<int> > &correspondence_indexes,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration)
{
bool collision = false;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = points_in_ROI->begin();
i != points_in_ROI->end() && !collision; ++i)
{
pcl::PointXYZRGBNormal point_in_sensor_frame = *i;
// 1)We point to the vector of indices related to the elevation_cloud point under evaluation
std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
+ point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX];
// and iterate over this vector
for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
point_iterator != index_iterator->end() && !collision; ++point_iterator) {
pcl::PointXYZRGBNormal point_under_evaluation = cloud_ptr->points[*point_iterator];
if ((int)std::floor(point_under_evaluation.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND)
{
if (point_under_evaluation.z - point_under_evaluation.data_n[DATA_N_1_Z_GROUND]
< filtering_configuration.robot_height + filtering_configuration.safety_margin)
{
kf_based_terrain_analysis_lib::Coordinates2D coord;
coord.x = point_under_evaluation.x;
coord.y = point_under_evaluation.y;
std::vector<kf_based_terrain_analysis_lib::Coordinates2D> vertex_list;
int closest_vertex_index;
kf_based_terrain_analysis_lib::Robot2DFootprint rectangular_path_between_vertices;
float delta_x = vertex_b.x - vertex_a.x;
float delta_y = vertex_b.y - vertex_a.y;
rectangular_path_between_vertices.yaw = atan2(delta_y, delta_x);
rectangular_path_between_vertices.dimensions.x = sqrt(delta_x * delta_x + delta_y * delta_y);
rectangular_path_between_vertices.dimensions.y = filtering_configuration.robot_width
+ filtering_configuration.safety_margin * 2.0;
rectangular_path_between_vertices.centroid.x = vertex_a.x
+ (rectangular_path_between_vertices.dimensions.x / 2.0) * cos(rectangular_path_between_vertices.yaw);
rectangular_path_between_vertices.centroid.y = vertex_a.y
+ (rectangular_path_between_vertices.dimensions.x / 2.0) * sin(rectangular_path_between_vertices.yaw);
calculateVertexCoordinates(rectangular_path_between_vertices, vertex_list, closest_vertex_index);
if (!collision)
collision = checkInside(coord, vertex_list);
if (false)
{
std::cout << "Collision detected! Edge discarded!" << std::endl;
std::cout << "point_under_evaluation class = "
<< (int)std::floor(point_under_evaluation.data_c[DATA_C_1_ID_CLASS]) << std::endl << "point x = "
<< coord.x << " point y = " << coord.y << std::endl << "vertex_a.x = " << vertex_a.x
<< " vertex_a.y = " << vertex_a.y << std::endl << "vertex_b.x = " << vertex_b.x
<< " vertex_b.y = " << vertex_b.y << std::endl << "rectangular_path_between_vertices.yaw = "
<< rectangular_path_between_vertices.yaw * 180.0 / M_PI << std::endl
<< "rectangular_path_between_vertices.dimensions.x = " << rectangular_path_between_vertices.dimensions.x
<< std::endl << "rectangular_path_between_vertices.dimensions.y = "
<< rectangular_path_between_vertices.dimensions.y << std::endl
<< "rectangular_path_between_vertices.centroid.x = " << rectangular_path_between_vertices.centroid.x
<< std::endl << "rectangular_path_between_vertices.centroid.y = "
<< rectangular_path_between_vertices.centroid.y << std::endl;
std::cout << "Vertices = " << std::endl << "p1 = " << vertex_list[0].x << " " << vertex_list[0].y
<< std::endl << "p2 = " << vertex_list[1].x << " " << vertex_list[1].y << std::endl << "p3 = "
<< vertex_list[2].x << " " << vertex_list[2].y << std::endl << "p4 = " << vertex_list[3].x << " "
<< vertex_list[3].y << std::endl;
//getchar();
}
}
}
}
}
return (collision);
}
void CKf_Based_Terrain_Analysis::makeVirtualObstacle(
const pcl::PointXYZRGBNormal point_a, const pcl::PointXYZRGBNormal point_b,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr virtual_obstacle_cloud)
{
float delta_x = point_b.x - point_a.x;
float delta_y = point_b.y - point_a.y;
std::vector<kf_based_terrain_analysis_lib::Coordinates2D> vertex_list;
int closest_vertex_index;
kf_based_terrain_analysis_lib::Robot2DFootprint rectangular_path_between_vertices;
rectangular_path_between_vertices.yaw = atan2(delta_y, delta_x); rectangular_path_between_vertices.dimensions.x = sqrt(delta_x * delta_x + delta_y * delta_y);
rectangular_path_between_vertices.dimensions.y = filtering_configuration.robot_width
+ filtering_configuration.safety_margin * 2.0;
rectangular_path_between_vertices.centroid.x = point_a.x
+ (rectangular_path_between_vertices.dimensions.x / 2.0) * cos(rectangular_path_between_vertices.yaw);
rectangular_path_between_vertices.centroid.y = point_b.y
+ (rectangular_path_between_vertices.dimensions.x / 2.0) * sin(rectangular_path_between_vertices.yaw);
calculateVertexCoordinates(rectangular_path_between_vertices, vertex_list, closest_vertex_index);
float x_min = 10000.0;
float x_max = -10000.0;
float y_min = 10000.0;
float y_max = -10000.0;
for (int i = 0; i < 4; ++i)
{
if (vertex_list[i].x < x_min)
x_min = vertex_list[i].x;
if (vertex_list[i].x > x_max)
x_max = vertex_list[i].x;
if (vertex_list[i].y < y_min)
y_min = vertex_list[i].y;
if (vertex_list[i].y > y_max)
y_max = vertex_list[i].y;
}
//std::cout << "x_min = " << x_min << " x_max = " << x_max << std::endl;
//std::cout << "y_min = " << y_min << " y_max = " << y_max << std::endl;
float delta_object = 0.1; //TODO: extract as parameter
for (float i = x_min; i <= x_max; i += delta_object)
{
for (float j = y_min; j <= y_max; j += delta_object)
{
//std::cout << "Generating virtual obstacle point at x = " << i << " y = " << j << std::endl;
kf_based_terrain_analysis_lib::Coordinates2D coord;
coord.x = i;
coord.y = j;
bool inside = checkInside(coord, vertex_list);
if (inside)
{
pcl::PointXYZRGBNormal virtual_obstacle_point;
virtual_obstacle_point.x = coord.x;
virtual_obstacle_point.y = coord.y;
virtual_obstacle_point.z = point_a.z;
virtual_obstacle_point.data_c[DATA_C_1_ID_CLASS] = CLASS_DISCONTINUITY;
virtual_obstacle_point.r = 255;
virtual_obstacle_point.g = 0;
virtual_obstacle_point.b = 0;
virtual_obstacle_cloud->push_back(virtual_obstacle_point);
}
}
}
}
void CKf_Based_Terrain_Analysis::makeVirtualObstacle(
const pcl::PointXYZRGBNormal point, const float yaw,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr virtual_obstacle_cloud)
{
}
void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration, const std::vector<std::vector<int> > &correspondence_indexes, const int reference_index, const float search_range,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr points_in_ROI,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr references_in_ROI,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr, std::vector<std::vector<int>> &edges,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr virtual_obstacles_cloud_ptr)
{
//std::cout << "Starting labelLocalGroundPoints!!" << std::endl;
if (filtering_configuration.measure_performance)
CFunctionMonitor performance_monitor("labelLocalGroundPoints", performance_supervisor_ptr_);
CObservationDownsampler new_references_creator(filtering_configuration.ground_reference_search_resolution,
filtering_configuration.propagation_additive_noise,
filtering_configuration.propagation_z_additive_noise);
// std::cout << "Reference_index = " << reference_index << std::endl;
// std::cout << "Using reference with ID = "
// << ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] << std::endl;
pcl::PointXYZRGBNormal reference_in_sensor_frame = ground_reference_cloud_ptr->points[reference_index];
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr observations_for_tangent_plane_estimation(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
// points in ROI come from elevation pointcloud, which is a reduction of the original
// pointcloud (we have stored the indexes to be able to label the original pointcloud)
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = points_in_ROI->begin(); i != points_in_ROI->end(); ++i)
{
pcl::PointXYZRGBNormal point_in_sensor_frame = *i;
float prediction_std_dev = -1.0;
float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame, point_in_sensor_frame,
prediction_std_dev);
if (std::isnan(mahalanobis_distance))
{
std::cout << "prior pred sigma = " << prediction_std_dev << " uncertainty = "
<< prediction_std_dev * filtering_configuration.mahalanobis_threshold << " mahalanobis distance = "
<< mahalanobis_distance << std::endl;
std::getchar();
}
if (mahalanobis_distance < filtering_configuration.mahalanobis_threshold)
{
// std::cout << "Ground point found! x = " << point_in_sensor_frame.x << " y = " << point_in_sensor_frame.y
// << " z = " << point_in_sensor_frame.z << std::endl
// << "mahalanobis_distance = " << mahalanobis_distance << " mahalanobis_threshold = "
// << filtering_configuration.mahalanobis_threshold << std::endl;
// std::cout << "Inserting observation for plane estimation!" << std::endl;
observations_for_tangent_plane_estimation->push_back(point_in_sensor_frame); // independently of exploration, any ground point is
// used for plane estimation, to no discard useful
// information!
if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) == CLASS_UNKNOWN) // we use unknown points to drive the exploration
{
// std::cout << "Inserting observation to drive exploration!" << std::endl;
new_references_creator.addGroundObservationInSensorFrame(
reference_in_sensor_frame,
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]]);
}
// std::cout << "Updating elevation cloud point to represent ground data" << std::endl;
if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND) // if the point was seen as obstacle from another POV
// or was unexplored, with set it to ground class
{
// std::cout << "Storing z point as z ground" << std::endl;
// as now the point is classified as ground, we store its own z value as z ground
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_n[DATA_N_1_Z_GROUND] = elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].z;
// set the class as ground
float score = 0.99 - (mahalanobis_distance / filtering_configuration.mahalanobis_threshold);
if (score < 0.0)
score = 0.0;
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_1_ID_CLASS] =
(float)CLASS_GROUND + score;
// colour the point properly
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].r = R_CLASS_GROUND;
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].g = G_CLASS_GROUND;
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].b = B_CLASS_GROUND;
}
}
else
{
// std::cout << "Obstacle point found! x = " << point_in_sensor_frame.x << " y = " << point_in_sensor_frame.y
// << " z = " << point_in_sensor_frame.z << std::endl
// << "mahalanobis_distance = " << mahalanobis_distance << " mahalanobis_threshold = "
// << filtering_configuration.mahalanobis_threshold << std::endl;
if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) == CLASS_UNKNOWN) // we only store de data if it has not been done
// previously
{
// if it is an obstacle point we use the ground reference z prediction as z ground
float z_ground = predictZ(ground_reference_cloud_ptr->points[reference_index], point_in_sensor_frame.x,
point_in_sensor_frame.y);
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_n[DATA_N_1_Z_GROUND] =
z_ground;
// set the class to OBSTACLE
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_1_ID_CLASS] =
(float)CLASS_OBSTACLE + 0.99;
// and colour the point
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].r = R_CLASS_OBSTACLE;
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].g = G_CLASS_OBSTACLE;
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].b = B_CLASS_OBSTACLE;
}
}
}
// std::cout << "Reference Prior = " << std::endl;
// std::cout << "x = " << ground_reference_cloud_ptr->points[reference_index].x << " y = "
// << ground_reference_cloud_ptr->points[reference_index].y << std::endl << " z = ( "
// << ground_reference_cloud_ptr->points[reference_index].z << " , "
// << sqrt(ground_reference_cloud_ptr->points[reference_index].data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) << " )"
// << " roll = ( " << atan(ground_reference_cloud_ptr->points[reference_index].data_c[GRND_REF_DATA_C_1_ROLL]) * 180.0 / M_PI
// << " , " << sqrt(ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE]) * 180.0 / M_PI << " )"
// << " pitch = ( " << atan(ground_reference_cloud_ptr->points[reference_index].data_c[GRND_REF_DATA_C_2_PITCH]) * 180.0 / M_PI
// << " , " << sqrt(ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE]) * 180.0 / M_PI << " )"
// << std::endl;
estimateTangentPlane(observations_for_tangent_plane_estimation, (double)filtering_configuration.z_observation_std_dev,
reference_in_sensor_frame);
ground_reference_cloud_ptr->points[reference_index] = reference_in_sensor_frame;
// std::cout << "Reference Posterior = " << std::endl;
// std::cout << "x = " << ground_reference_cloud_ptr->points[reference_index].x << " y = "
// << ground_reference_cloud_ptr->points[reference_index].y << std::endl << " z = ( "
// << ground_reference_cloud_ptr->points[reference_index].z << " , "
// << sqrt(ground_reference_cloud_ptr->points[reference_index].data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) << " )"
// << " roll = ( " << atan(ground_reference_cloud_ptr->points[reference_index].data_c[GRND_REF_DATA_C_1_ROLL]) * 180.0 / M_PI
// << " , " << sqrt(ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_0_ROLL_VARIANCE]) * 180.0 / M_PI << " )"
// << " pitch = ( " << atan(ground_reference_cloud_ptr->points[reference_index].data_c[GRND_REF_DATA_C_2_PITCH]) * 180.0 / M_PI
// << " , " << sqrt(ground_reference_cloud_ptr->points[reference_index].data_n[GRND_REF_DATA_N_1_PITCH_VARIANCE]) * 180.0 / M_PI << " )"
// << std::endl;
// Now we evaluate the points in the original pointcloud using the ROI indexes vectors! using the posterior!
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = points_in_ROI->begin(); i != points_in_ROI->end(); ++i)
{
pcl::PointXYZRGBNormal point_in_sensor_frame = *i;
// 1)We point to the vector of indices related to the elevation_cloud point under evaluation
std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
+ point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX];
// and iterate over this vector
for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
point_iterator != index_iterator->end(); ++point_iterator)
{
if ((int)std::floor(cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND)
{
float prediction_std_dev = -1.0;
float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame, cloud_ptr->points[*point_iterator],
prediction_std_dev);
if (std::isnan(mahalanobis_distance))
{
std::cout << "posterior pred sigma = " << prediction_std_dev << " uncertainty = "
<< prediction_std_dev * filtering_configuration.mahalanobis_threshold << " mahalanobis distance = "
<< mahalanobis_distance << std::endl;
std::getchar();
}
if (mahalanobis_distance < filtering_configuration.mahalanobis_threshold)
{
// Intensity is already in the pointcloud so we do not copy data_n[0] (in fact this value is not present in elevation cloud)
cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = cloud_ptr->points[*point_iterator].z;
cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] = point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
point_in_sensor_frame.data_n[DATA_N_3_Z_VARIANCE];
// data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
cloud_ptr->points[*point_iterator].r = R_CLASS_GROUND; // We use instead the r g b channels directly
cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
cloud_ptr->points[*point_iterator].b = B_CLASS_GROUND;
float score = 0.99 - (mahalanobis_distance / filtering_configuration.mahalanobis_threshold);
if (score < 0.0)
score = 0.0;
cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
// data_c[2] is reserved for Car probability, as we do not estimate it in this algorithm we leave this value untouched
// so if there are others segmentators working out there, we do not erase their job!
// data_c[3] is reserved to store the original index, so we do not want to change it!
}
else
{
// Intensity is already in the pointcloud so we do not copy data_n[0] (in fact this value is not present in elevation cloud)
cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = point_in_sensor_frame.z;
cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] = point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
point_in_sensor_frame.data_n[DATA_N_3_Z_VARIANCE];
// data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE; // We use instead the r g b channels directly
cloud_ptr->points[*point_iterator].g = G_CLASS_OBSTACLE;
cloud_ptr->points[*point_iterator].b = B_CLASS_OBSTACLE;
cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.99;
// data_c[2] is reserved for Car probability, as we do not estimate it in this algorithm we leave this value untouched
// so if there are others segmentators working out there, we do not erase their job!
// data_c[3] is reserved to store the original index, so we do not want to change it!
}
}
} }
assert(
reference_index == reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
&& "something unexpected happened!!");
// Now we make edges to existing references in the ROI
//std::cout << "Number of references in ROI = " << references_in_ROI->size() << std::endl;
int discarded_edges = 0;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = references_in_ROI->begin(); i != references_in_ROI->end();
++i)
{
pcl::PointXYZRGBNormal reference_in_ROI = *i;
if (reference_index != reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
&& (std::find(edges[reference_index].begin(), edges[reference_index].end(),
reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]) == edges[reference_index].end())
&& (std::find(edges[reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].begin(),
edges[reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].end(), reference_index)
== edges[reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].end()))
{
//std::cout << "reference_index = " << reference_index << std::endl;
//std::cout << "reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] = "
// << reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] << std::endl;
//std::cout << "edges[reference_index].size() = " << edges[reference_index].size() << std::endl;
//int n = 0;
//for (std::vector<int>::iterator i = edges[reference_index].begin(); i != edges[reference_index].end(); i++)
//{
// std::cout << "edges[" << n++ << "] = " << *i << std::endl;
//}
// std::cout << "Analizing vertex in ROI with id = " << reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
// << std::endl;
float prediction_std_dev = -1.0;
float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame, reference_in_ROI, prediction_std_dev);
// Just a check!
if (std::isnan(mahalanobis_distance))
{
std::cout << "posterior pred sigma = " << prediction_std_dev << " uncertainty = "
<< prediction_std_dev * filtering_configuration.mahalanobis_threshold << " mahalanobis distance = "
<< mahalanobis_distance << std::endl;
std::getchar();
}
//std::cout << "Checking connections..." << std::endl;
bool connect = true;
bool make_virtual_obstacle = false;
float delta_x = reference_in_ROI.x - reference_in_sensor_frame.x;
float delta_y = reference_in_ROI.y - reference_in_sensor_frame.y;
float delta_2D = sqrt(delta_x * delta_x + delta_y * delta_y);
float delta_z = reference_in_ROI.z - reference_in_sensor_frame.z;
float slope = delta_z / delta_2D;
if (mahalanobis_distance > filtering_configuration.mahalanobis_threshold
|| fabs(slope) > filtering_configuration.max_slope_abs)
{
std::cout << "Discarding connection! Euclidean distance = " << delta_2D << " Mahalanobis distance = " << mahalanobis_distance
<< " Mahalanobis threshold = " << filtering_configuration.mahalanobis_threshold << std::endl
<< " Slope = " << fabs(slope) << " slope threshold = " << filtering_configuration.max_slope_abs
<< std::endl;
connect = false;
if(delta_2D < 2.0)
make_virtual_obstacle = true;
}
else
{
if (!filtering_configuration.flag_connect_vertices_even_if_there_are_obstacles
&& checkCollisionInPath(reference_in_sensor_frame, reference_in_ROI, points_in_ROI, cloud_ptr, correspondence_indexes, filtering_configuration))
{
//std::cout << "Obstacle detected, discarding connection!" << std::endl;
connect = false;
if (filtering_configuration.flag_generate_discontinuity_obstacles_for_real_obstacles)
{
make_virtual_obstacle = true;
}
}
}
if (connect)
{
//std::cout << "Adding edge between vertex " << reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
// << " and vertex " << reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] << std::endl;
edges[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].push_back(
reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]);
}
else
{
discarded_edges++;
}
if (make_virtual_obstacle)
{
//std::cout << "Generating points for virtual obstacle!" << std::endl;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr virtual_obstacle(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
makeVirtualObstacle(reference_in_sensor_frame, reference_in_ROI, filtering_configuration, virtual_obstacle);
*virtual_obstacles_cloud_ptr += *virtual_obstacle;
}
}
else
{
// std::cout << "Skip adding edge between vertex " << reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
// << " and vertex " << reference_in_ROI.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] << std::endl;
}
}
//std::cout << "Total edges evaluated = " << references_in_ROI->points.size() << " discarded = " << discarded_edges
// << std::endl;
//getchar();
new_references_creator.generateNewGroundReferences(reference_index, reference_in_sensor_frame,
ground_reference_cloud_ptr, edges);
}
void CKf_Based_Terrain_Analysis::labelGroundPoints(
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
const std::vector<std::vector<int> > &correspondence_indexes, const int reference_index,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr, std::vector<std::vector<int>> &edges,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr virtual_obstacles_cloud_ptr)
{
float search_range = 0.0;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr points_in_ROI(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr references_in_ROI(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
extractPointsInROI(lidar_configuration, filtering_configuration, ground_reference_cloud_ptr->points[reference_index],
elevation_cloud_ptr, points_in_ROI, ground_reference_cloud_ptr, references_in_ROI, search_range);
// std::cout << "Reference index = " << reference_index << std::endl;
// std::cout << "Points in ROI extracted!! Number of points in ROI = " << points_in_ROI->points.size() << std::endl;
// std::cout << "References in ROI extracted!! Number of references in ROI = " << references_in_ROI->points.size()
// << std::endl;
//std::cout << "Number of edges in reference prior to labeling = " << edges[reference_index].size() << std::endl;
labelLocalGroundPoints(filtering_configuration, correspondence_indexes, reference_index, search_range, points_in_ROI,
references_in_ROI, ground_reference_cloud_ptr, elevation_cloud_ptr, cloud_ptr, edges,
virtual_obstacles_cloud_ptr);
//std::cout << "Number of edges after labeling = " << edges[reference_index].size() << std::endl;
//getchar();
}
void CKf_Based_Terrain_Analysis::generateElevationCloud(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud, std::vector<std::vector<int>> &correspondence_indexes)
{
if (filtering_configuration.measure_performance)
CFunctionMonitor performance_monitor("generateElevationCloud", performance_supervisor_ptr_);
pcl::PointCloud<pcl::PointXY>::Ptr pcl_pointcloud_2D_ptr(new pcl::PointCloud<pcl::PointXY>);
pcl::PointXY point;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::const_iterator it = pcl_cloud_ptr->begin(); it != pcl_cloud_ptr->end();
++it)
{
//if(it->data_c[DATA_C_1_ID_CLASS] != CLASS_UNKNOWN)
//{
// std::cout << "class in original point cloud prior to label process = " <<
// it->data_c[DATA_C_1_ID_CLASS] << std::endl;
//}
point.x = it->x;
point.y = it->y;
pcl_pointcloud_2D_ptr->points.push_back(point);
}
//std::cout << "Number of points in pcl_pointcloud_2D_ptr = " << pcl_pointcloud_2D_ptr->points.size() << std::endl;
pcl::KdTreeFLANN < pcl::PointXY > kdtree_2D;
kdtree_2D.setInputCloud(pcl_pointcloud_2D_ptr);
// We want to find the x and y values, but not interested in z
//std::cout << "Elevation grid resolution = " << filtering_configuration.elevation_grid_resolution << std::endl;
pcl::VoxelGrid < pcl::PointXYZRGBNormal > voxel_grid;
voxel_grid.setInputCloud(pcl_cloud_ptr);
voxel_grid.setLeafSize(filtering_configuration.elevation_grid_resolution,
filtering_configuration.elevation_grid_resolution, OUT_OF_RANGE);
voxel_grid.filter(*elevation_cloud);
//std::cout << "Number of points after voxelization = " << elevation_cloud->points.size() << std::endl;
const float DISTANCE_FROM_CELL_CENTER_TO_CELL_CORNER = 0.7071067812
* filtering_configuration.elevation_grid_resolution; //sqrt(2)/2.0
int i = 0;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator it = elevation_cloud->begin(); it != elevation_cloud->end();
++it)
{
point.x = it->x;
point.y = it->y;
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSearchSquaredDistance;
kdtree_2D.radiusSearch(point, DISTANCE_FROM_CELL_CENTER_TO_CELL_CORNER, pointIdxRadiusSearch,
pointRadiusSearchSquaredDistance);
std::vector<float> z_coordinates;
std::vector<float> x_coordinates;
std::vector<float> y_coordinates;
bool outlier_detected = false;
for (std::vector<int>::const_iterator iterator = pointIdxRadiusSearch.begin();
iterator != pointIdxRadiusSearch.end(); ++iterator)
{
z_coordinates.push_back(pcl_cloud_ptr->points[*iterator].z);
x_coordinates.push_back(pcl_cloud_ptr->points[*iterator].x);
y_coordinates.push_back(pcl_cloud_ptr->points[*iterator].y);
if (pcl_cloud_ptr->points[*iterator].data_c[DATA_C_1_ID_CLASS] == OUTLIER) outlier_detected = true;
}
if (outlier_detected)
{
it->data_c[DATA_C_1_ID_CLASS] = OUTLIER;
it->r = R_CLASS_OUTLIER;
it->g = G_CLASS_OUTLIER;
it->b = B_CLASS_OUTLIER;
}
else
{
it->data_c[DATA_C_1_ID_CLASS] = CLASS_UNKNOWN;
it->r = R_CLASS_UNKNOWN;
it->g = G_CLASS_UNKNOWN;
it->b = B_CLASS_UNKNOWN;
}
float sum = std::accumulate(z_coordinates.begin(), z_coordinates.end(), 0.0);
float z_mean = sum / (float)z_coordinates.size();
float var = 0;
for (int n = 0; n < z_coordinates.size(); ++n)
var += ((z_coordinates[n] - z_mean) * (z_coordinates[n] - z_mean));
var /= (float)z_coordinates.size();
std::vector<float>::iterator lowest_point_in_cell = std::min_element(z_coordinates.begin(), z_coordinates.end());
int position_in_vector = std::distance(z_coordinates.begin(), lowest_point_in_cell);
it->z = z_coordinates[position_in_vector];
it->x = x_coordinates[position_in_vector];
it->y = y_coordinates[position_in_vector];
// Filling remaining fields
it->data_n[DATA_N_0_INTENSITY] = INTENSITY_UNKNOWN;
it->data_n[DATA_N_1_Z_GROUND] = Z_GROUND_UNKNOWN;
it->data_n[DATA_N_2_Z_MEAN] = z_mean;
it->data_n[DATA_N_3_Z_VARIANCE] = var;
it->data_c[DATA_C_2_CAR_PROB] = PROB_UNKNOWN;
it->data_c[DATA_C_3_ORIGINAL_INDEX] = i++; //storing the index for later in labelling step
//std::cout << "var z = " << it->data_n[DATA_N_3_Z_VARIANCE] << std::endl;
//std::cout << "z_coordinates.size() = " << z_coordinates.size() << std::endl;
correspondence_indexes.push_back(pointIdxRadiusSearch);
}
}
void CKf_Based_Terrain_Analysis::generateGroundReferencesInShadowArea(
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr, std::vector<std::vector<int>> &edges)
{
//std::cout << "Generating vertices in shadow area!" << std::endl;
//CFunctionMonitor performance_monitor("generateGroundReferencesInShadowArea", performance_supervisor_ptr_);
pcl::PointXYZRGBNormal ground_at_shadow_area;
// The first reference point (root vertex) is the ground in (0,0) approximately known by sensor extrinsic calibration
int index = 0;
initizalizeShadowAreaGroundReferencePoint(lidar_configuration, filtering_configuration, 0.0, 0.0, index++,
ground_at_shadow_area);
//std::cout << "generateGroundReferencesInShadowArea: r = " << (int)ground_at_shadow_area.r << " g = " << (int)ground_at_shadow_area.g
// << " b = " << (int)ground_at_shadow_area.b << std::endl;
// we change the UNKNOWN cost to MIN_COST because we are already in the origin so it is absolutely reachable!!
ground_at_shadow_area.data_n[GRND_REF_DATA_N_2_GOAL_COST] = MIN_GOAL_COST;
// but we keep the UNKNOWN original index because the origin has not a previous origin!
ground_reference_cloud_ptr->points.push_back(ground_at_shadow_area);//std::cout << "First ground reference point generated! Npoints = " << ground_reference_cloud_ptr->points.size()
// << std::endl;
float range = filtering_configuration.distance_to_virtual_references_in_shadow_area;
float delta_theta = filtering_configuration.ground_reference_search_resolution;
std::vector<int> edges_from_root_to_virtual_points_in_shadow_area;
for (float theta = 0; theta < 1.99 * M_PI; theta += delta_theta) // the 1.99 value is for just giving a margin to not repeat the first reference
{
float x = range * cos(theta);
float y = range * sin(theta);
initizalizeShadowAreaGroundReferencePoint(lidar_configuration, filtering_configuration, x, y, index,
ground_at_shadow_area);
//std::cout << "Added edge between root and vertex " << index << std::endl;
edges[0].push_back(index++); // all shadow vertex are connected with the root
//std::cout << "generateGroundReferencesInShadowArea: r = " << (int)ground_at_shadow_area.r << " g = "
// << (int)ground_at_shadow_area.g << " b = " << (int)ground_at_shadow_area.b << std::endl;
ground_reference_cloud_ptr->points.push_back(ground_at_shadow_area);
//getchar();
}
}
void CKf_Based_Terrain_Analysis::labelGround(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr)
{
CFunctionMonitor performance_monitor("labelGround", performance_supervisor_ptr_);
pcl::KdTreeFLANN < pcl::PointXYZRGBNormal > kdtree;
kdtree.setInputCloud(ground_reference_cloud_ptr);
// std::vector<int> pointIdxNKNSearch(filtering_configuration.number_of_references_used_in_labelling); //to store the id of the closest real data point
// std::vector<float> pointNKNSquaredDistance(filtering_configuration.number_of_references_used_in_labelling);
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator it_points = cloud_ptr->begin(); it_points != cloud_ptr->end();
++it_points)
{
std::vector<int> pointIdxNKNSearch(filtering_configuration.number_of_references_used_in_labelling); //to store the id of the closest real data point
std::vector<float> pointNKNSquaredDistance(filtering_configuration.number_of_references_used_in_labelling);
assert(
kdtree.nearestKSearch(*it_points, filtering_configuration.number_of_references_used_in_labelling,
pointIdxNKNSearch, pointNKNSquaredDistance) > 0 && "Kdtree could not find a neighbourg!");
pcl::PointXYZRGBNormal point_to_evaluate = *it_points;
float min_mahalanobis_distance = 100000.0;
pcl::PointXYZRGBNormal best_reference;
for (int i = 0; i < filtering_configuration.number_of_references_used_in_labelling; ++i)
{
pcl::PointXYZRGBNormal reference = ground_reference_cloud_ptr->points[pointIdxNKNSearch[i]];
float prediction_std_dev = -1.0;
float mahalanobis_distance = mahalanobisDistance(reference, point_to_evaluate, prediction_std_dev);
//std::cout << "mahalab = " << mahalanobis_distance << " min mahalab = " << min_mahalanobis_distance << std::endl;
if (mahalanobis_distance < min_mahalanobis_distance)
{
best_reference = reference;
min_mahalanobis_distance = mahalanobis_distance;
}
}
// std::cout << "----------------------------------------------------------------------------" << std::endl;
// std::cout << "Number of references found = " << pointIdxNKNSearch.size() << std::endl;// std::cout << "z_observed = " << point_to_evaluate.z
// << " z_predicted = " << predictZ(best_reference, point_to_evaluate.x, point_to_evaluate.y)
// << std::endl;
// std::cout << "min_mahalanobis_distance = " << min_mahalanobis_distance << std::endl;
//
// std::getchar();
if (min_mahalanobis_distance < filtering_configuration.mahalanobis_threshold)
{
it_points->data_n[DATA_N_1_Z_GROUND] = it_points->z;
// set the class as ground
float threshold = filtering_configuration.mahalanobis_threshold;
if (min_mahalanobis_distance > threshold)
threshold = min_mahalanobis_distance * 1.2;
it_points->data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + 1.0 - (min_mahalanobis_distance / threshold);
// colour the point properly
it_points->r = R_CLASS_GROUND;
it_points->g = G_CLASS_GROUND;
it_points->b = B_CLASS_GROUND;
}
else
{
it_points->data_n[DATA_N_1_Z_GROUND] = predictZ(best_reference, point_to_evaluate.x, point_to_evaluate.y);
// set the class to OBSTACLE
it_points->data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 1.0
- (filtering_configuration.mahalanobis_threshold / min_mahalanobis_distance);
// and colour the point
it_points->r = R_CLASS_OBSTACLE;
it_points->g = G_CLASS_OBSTACLE;
it_points->b = B_CLASS_OBSTACLE;
}
}
}
void CKf_Based_Terrain_Analysis::labelGroundUsingElevationCloudAndCorrespondenceIndexes(
const std::vector<std::vector<int> > &correspondence_indexes,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr)
{
CFunctionMonitor performance_monitor("labelGroundUsingElevationCloudAndCorrespondenceIndexes",
performance_supervisor_ptr_);
std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin();
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::const_iterator it = elevation_cloud_ptr->begin();
it != elevation_cloud_ptr->end(); ++it)
{
for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
point_iterator != index_iterator->end(); ++point_iterator)
{
// Intensity is already in the pointcloud so we do not copy data_n[0] (in fact this value is not present in elevation cloud)
cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = it->data_n[DATA_N_1_Z_GROUND];
cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] = it->data_n[DATA_N_2_Z_MEAN];
cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] = it->data_n[DATA_N_3_Z_VARIANCE];
// data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
cloud_ptr->points[*point_iterator].r = it->r; // We use instead the r g b channels directly
cloud_ptr->points[*point_iterator].g = it->g;
cloud_ptr->points[*point_iterator].b = it->b;
cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = it->data_c[DATA_C_1_ID_CLASS] + 0.99;
// data_c[2] is reserved for Car probability, as we do not estimate it in this algorithm we leave this value untouched
// so if there are others segmentators working out there, we do not erase their job!
// data_c[3] is reserved to store the original index, so we do not want to change it!
}
++index_iterator;
}}
void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_dense_cloud_ptr)
{
std::cout << "setting input cloud" << std::endl;
pcl::KdTreeFLANN < pcl::PointXYZRGBNormal > kdtree;
kdtree.setInputCloud(ground_reference_cloud_ptr);
std::vector<int> pointIdxNKNSearch(1); //to store the id of the closest real data point
std::vector<float> pointNKNSquaredDistance(1);
std::cout << "Starting dense cloud creation" << std::endl;
for (float i = -50.0; i < 50.0; i = i + 0.5)
{
for (float j = -35.0; j < 35.0; j = j + 0.5)
{
pcl::PointXYZRGBNormal point;
point.x = i;
point.y = j;
point.z = -1.75;
point.r = 127;
point.g = 127;
point.b = 127;
assert(
kdtree.nearestKSearch(point, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0
&& "Kdtree could not find a neighbourg!");
//std::cout << "Extracting closest reference" << std::endl;
pcl::PointXYZRGBNormal reference = ground_reference_cloud_ptr->points[pointIdxNKNSearch[0]];
float delta_x = point.x - reference.x;
float delta_y = point.y - reference.y;
//std::cout << "Predicting point height" << std::endl;
point.z = predictZ(reference, delta_x, delta_y);
//std::cout << "Storing the point!" << std::endl;
pcl_dense_cloud_ptr->push_back(point);
}
}
std::cout << "Finished dense cloud creation!!" << std::endl;
}
void CKf_Based_Terrain_Analysis::groundSegmentation(
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal> &pcl_cloud,
pcl::PointCloud<pcl::PointXYZRGBNormal> &ground_references_pcl_cloud, std::vector<std::vector<int>> &edges)
{
//std::cout << "Starting ground segmentation!!" << std::endl;
//std::cout << "filtering_configuration_.show_dense_reconstruction = "
// << filtering_configuration.show_dense_reconstruction << std::endl;
// std::cout << "Initializing pointcloud: Number of points in pcl_cloud = " << pcl_cloud.points.size() << std::endl;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
*pcl_cloud_ptr = pcl_cloud;
// We create a pointcloud that will contain only trusted subsampled ground points, these points will be used
// as references to compute local planes to label near points
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector < std::vector<int> > correspondence_indexes;
generateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
// std::cout << "Elevation cloud generated! Npoints = " << elevation_cloud_ptr->points.size() << std::endl;
generateGroundReferencesInShadowArea(lidar_configuration, filtering_configuration, ground_reference_cloud_ptr, edges);
// std::cout << "Shadow area reference points generated! Npoints = " << ground_reference_cloud_ptr->points.size()
// << std::endl << "Number of edges = " << edges[0].size() << std::endl;
//getchar();
// We run the algorithm, starting from the reference at sensor origin it will try to find others trusty local
// ground references, which will increase the value of ground_reference_cloud->points.size(), evaluating
// local planes using these reference points, once no new references appear. The algorithm ends after evaluating
// all of them
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr virtual_obstacles_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
int reference_index = 0;
while (reference_index < ground_reference_cloud_ptr->points.size())
{
//std::cout << "Evaluating reference num " << reference_index << ":" << std::endl << "x = "
// << ground_reference_cloud_ptr->points[reference_index].x << " y = "
// << ground_reference_cloud_ptr->points[reference_index].y << " z = "
// << ground_reference_cloud_ptr->points[reference_index].z << std::endl;
//
// std::cout << "r = " << (int)ground_reference_cloud_ptr->points[reference_index].r << " g = "
// << (int)ground_reference_cloud_ptr->points[reference_index].g << " b = "
// << (int)ground_reference_cloud_ptr->points[reference_index].b << std::endl;
labelGroundPoints(lidar_configuration, filtering_configuration, correspondence_indexes, reference_index++,
ground_reference_cloud_ptr, elevation_cloud_ptr, pcl_cloud_ptr, edges,
virtual_obstacles_cloud_ptr);
// std::cout << "reference_index = " << reference_index << std::endl;
// std::cout << "number of references = " << ground_reference_cloud_ptr->points.size() << std::endl;
}
// std::cout << "reference_index = " << reference_index << std::endl;
// std::cout << "number of references = " << ground_reference_cloud_ptr->points.size() << std::endl;
//getchar();
ground_references_pcl_cloud.clear();
if (filtering_configuration.show_dense_reconstruction)
{
std::cout << "Ready to create dense reconstruction" << std::endl;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_dense_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
createDenseGroundCloud(ground_reference_cloud_ptr, pcl_dense_cloud_ptr);
ground_references_pcl_cloud = *pcl_dense_cloud_ptr;
}
else
{
//std::cout << "Not creating dense reconstruction!"
// << " filtering_configuration.show_dense_reconstruction = "
// << filtering_configuration.show_dense_reconstruction << std::endl;
ground_references_pcl_cloud = *ground_reference_cloud_ptr;
}
pcl_cloud.clear();
pcl_cloud = *pcl_cloud_ptr + *virtual_obstacles_cloud_ptr;
//pcl_cloud = *elevation_cloud_ptr;
//pcl_cloud.width = pcl_cloud.size();
//std::cout << "pcl_cloud.size() = " << pcl_cloud.size() << " pcl_cloud.width = " << pcl_cloud.width << std::endl;
//std::cout << "elevation_cloud_ptr->size() = " << elevation_cloud_ptr->size() << " elevation_cloud_ptr->width = " << elevation_cloud_ptr->width << std::endl;
//performance_supervisor_ptr_->printStatistics();
}