diff --git a/include/structs_definitions.h b/include/structs_definitions.h index 5263d124b4b8300858ec0558726c206c14e0da5d..6461a52c7ff747fba69526bafb59bb51d41614d6 100644 --- a/include/structs_definitions.h +++ b/include/structs_definitions.h @@ -17,8 +17,9 @@ const int KATA_EVALUATION = 2; const float OUT_OF_RANGE = 1000.0; const float MAX_RANGE = 100.0; +const int DATA_N_0_ELEVATION_CLOUD_INDEX_OF_POINT_IN_ORIGINAL_CLOUD = 0; + const int DATA_N_0_INTENSITY = 0; -const int DATA_C_2_MEAN_INTENSITY = 2; const int DATA_N_1_INTENSITY_VARIANCE = 1; const int DATA_N_2_Z_MEAN = 2; const int DATA_N_3_Z_VARIANCE = 3; @@ -147,9 +148,9 @@ const uint8_t R_VERTEX_NOT_CONNECTED_TO_ROOT = 255; const uint8_t G_VERTEX_NOT_CONNECTED_TO_ROOT = 0; const uint8_t B_VERTEX_NOT_CONNECTED_TO_ROOT = 0; -const uint8_t R_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 255; -const uint8_t G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 0; -const uint8_t B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 0; +const uint8_t R_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 0; +const uint8_t G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 255; +const uint8_t B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 255; namespace kf_based_terrain_analysis_lib { diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp index d8ab35d0a83ba8629a6f37b44cf251433de9b737..b4647309d6aa7c9f80113c7c050df9a1db8cf0bd 100644 --- a/src/kf_based_terrain_analysis.cpp +++ b/src/kf_based_terrain_analysis.cpp @@ -202,12 +202,12 @@ float CKf_Based_Terrain_Analysis::mahalanobisDistance(const pcl::PointXYZRGBNorm { Eigen::Vector3d partial_derivatives; partial_derivatives(0) = 1.0; - partial_derivatives(1) = (point_in_sensor_frame.y - reference_in_sensor_frame.y); - partial_derivatives(2) = (point_in_sensor_frame.x - reference_in_sensor_frame.x); + partial_derivatives(1) = (point_in_sensor_frame.x - reference_in_sensor_frame.x); + partial_derivatives(2) = (point_in_sensor_frame.y - reference_in_sensor_frame.y); - prediction_std_fast = partial_derivatives.transpose() + prediction_std_fast = sqrt(partial_derivatives.transpose() * references_covariance_matrix_vector_[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]] - * partial_derivatives; + * partial_derivatives); } float mahalanobis_distance_fast = fabs(point_in_sensor_frame.z - prediction_fast) / prediction_std_fast; @@ -489,6 +489,7 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints( // because of the POV (one thing can be seem as obstacle or ground depending on the POV) { // Now we compute the score + // ERROR!! Check this! it should be 1.0 - maha / thresh, isnt it? float score = (mahalanobis_distance / filtering_configuration.mahalanobis_threshold) - 1.0; if (score > 0.99) score = 0.99; @@ -774,33 +775,14 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud( elevation_cloud_point.b = B_CLASS_UNKNOWN; } - float sum = std::accumulate(elevation_cell_vector[i].z_coordinates.begin(), - elevation_cell_vector[i].z_coordinates.end(), 0.0); - float z_mean = sum / (float)elevation_cell_vector[i].z_coordinates.size(); - float var = 0; - for (int n = 0; n < elevation_cell_vector[i].z_coordinates.size(); ++n) - var += ((elevation_cell_vector[i].z_coordinates[n] - z_mean) - * (elevation_cell_vector[i].z_coordinates[n] - z_mean)); - - var /= (float)elevation_cell_vector[i].z_coordinates.size(); - - float sum_intensity = std::accumulate(elevation_cell_vector[i].intensities.begin(), - elevation_cell_vector[i].intensities.end(), 0.0); - float intensity_mean = sum_intensity / (float)elevation_cell_vector[i].intensities.size(); - float var_intensity = 0; - for (int n = 0; n < elevation_cell_vector[i].intensities.size(); ++n) - var_intensity += ((elevation_cell_vector[i].intensities[n] - intensity_mean) - * (elevation_cell_vector[i].intensities[n] - intensity_mean)); - - var_intensity /= (float)elevation_cell_vector[i].intensities.size(); - //std::cout << "Intensity of elevation point: mean = " << intensity_mean << " std = " << sqrt(var_intensity) << std::endl; // Filling remaining fields - elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = intensity_mean; - elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE] = var_intensity; - elevation_cloud_point.data_n[DATA_N_2_Z_MEAN] = z_mean; - elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = var; + elevation_cloud_point.data_n[DATA_N_0_ELEVATION_CLOUD_INDEX_OF_POINT_IN_ORIGINAL_CLOUD] = + elevation_cell_vector[i].index_of_lowest_point; + elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE] = 0.0; + elevation_cloud_point.data_n[DATA_N_2_Z_MEAN] = 0.0; + elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = 0.0; elevation_cloud_point.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = INDEX_UNKNOWN; elevation_cloud_point.data_c[DATA_C_3_ORIGINAL_INDEX] = i; //storing the index for later in labelling step @@ -844,9 +826,12 @@ void CKf_Based_Terrain_Analysis::generateRootVertex( root_vertex.y = 0.0; root_vertex.z = -1.0 * lidar_configuration.sensor_height; - root_vertex.r = 0; - root_vertex.g = 0; - root_vertex.b = 0; + // We will keep the correspondence between ground references and original points in the lidar cloud to take advantage of the segmentation + // for planning + root_vertex.data_c[GRND_REF_DATA_C_0_RGB_CAST_INTO_FLOAT] = (float)INDEX_UNKNOWN; +// root_vertex.r = 0; +// root_vertex.g = 0; +// root_vertex.b = 0; root_vertex.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] = VERTEX_CONNECTED_TO_ROOT; @@ -950,8 +935,8 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud( if (std_dev > max_std_dev) max_std_dev = std_dev; - if (std_dev > MAX_STD_DEV) - std_dev = MAX_STD_DEV; + //if (std_dev > MAX_STD_DEV) + // std_dev = MAX_STD_DEV; float red = 0.0; float green = 255.0 * std_dev / MAX_STD_DEV; @@ -961,8 +946,11 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud( point.g = (int)green; point.b = (int)blue; - //std::cout << "Storing the point!" << std::endl; - pcl_dense_cloud_ptr->push_back(point); + if (std_dev < MAX_STD_DEV) + { + //std::cout << "Storing the point!" << std::endl; + pcl_dense_cloud_ptr->push_back(point); + } } } } @@ -1293,7 +1281,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( break; default: - assert("ERROR in CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel: Unrecognized number of outputs in Shallow Neural Network" && false); + assert( + "ERROR in CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel: Unrecognized number of outputs in Shallow Neural Network" + && false); break; } } @@ -1409,14 +1399,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] = elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE]; - // TODO: we copy the mean intensity value in the channel reserved for the color, we do this because - // we have detected a problem with pcl conversions: when passing the point cloud through a ROS topic, - // channels c2 and c3 get corrupted, so we avoid to use them, we will investigate this problem in the - // future (hopefully) - // pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] = - // elevation_cloud_point.data_n[DATA_N_0_INTENSITY]; -// pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_0_RGB_CAST_INTO_FLOAT] = -// elevation_cloud_point.data_n[DATA_N_0_INTENSITY]; if (filtering_configuration.classify_not_labeled_points_as_obstacles) { if ((pcl_cloud_ptr->points[*point_iterator].z - elevation_cloud_point.z) @@ -1473,14 +1455,22 @@ void CKf_Based_Terrain_Analysis::ensureThatConnectionsAreReflectedInBothEnds( { for (int vertex_iterator = 0; vertex_iterator < ground_reference_cloud_ptr->points.size(); ++vertex_iterator) { +// if (ground_reference_cloud_ptr->points[vertex_iterator].data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] +// != VERTEX_NOT_CONNECTED_TO_ROOT) +// { for (int connected_vertices_iterator = 0; connected_vertices_iterator < edges[vertex_iterator].size(); ++connected_vertices_iterator) { int connected_vertex = edges[vertex_iterator][connected_vertices_iterator]; +// if (ground_reference_cloud_ptr->points[connected_vertex].data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] +// != VERTEX_NOT_CONNECTED_TO_ROOT) +// { if (std::find(edges[connected_vertex].begin(), edges[connected_vertex].end(), vertex_iterator) == edges[connected_vertex].end()) edges[connected_vertex].push_back(vertex_iterator); +// } } +// } } } @@ -1756,13 +1746,25 @@ void CKf_Based_Terrain_Analysis::groundSegmentation( // std::cout << "Total number of covariance matrices = " << references_covariance_matrix_vector_.size() << std::endl; //getchar(); -// We can discard ground references if they are not connected with any other, to test this, we need -// to be sure that connections are reflected in both ends - ensureThatConnectionsAreReflectedInBothEnds(ground_reference_cloud_ptr, edges); - fastLabelPointcloudUsingGroundModel(filtering_configuration, elevation_cloud_ptr, correspondence_indexes, ground_reference_cloud_ptr, edges, pcl_cloud_ptr); + for (int vertex_iterator = 0; vertex_iterator < ground_reference_cloud_ptr->points.size(); ++vertex_iterator) + { + // If the point in the original cloud has been classified as obstacle we will want to exclude it from the graph search that might be used for planning by other algorithms + if (pcl_cloud_ptr->points[ground_reference_cloud_ptr->points[vertex_iterator].data_c[DATA_C_0_RGB_CAST_INTO_FLOAT]].data_c[DATA_C_1_ID_CLASS] + == CLASS_OBSTACLE) + { + ground_reference_cloud_ptr->points[vertex_iterator].data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] = + VERTEX_NOT_CONNECTED_TO_ROOT; + //edges[vertex_iterator].clear(); + } + } + + // We can discard ground references if they are not connected with any other, to test this, we need + // to be sure that connections are reflected in both ends + ensureThatConnectionsAreReflectedInBothEnds(ground_reference_cloud_ptr, edges); + // Dense reconstruction aims to serve as debugging or visualizing tool, it generates an XY regular grid with predicted // Z values using the closest reference point if (filtering_configuration.show_dense_reconstruction) diff --git a/src/observation_downsampler.cpp b/src/observation_downsampler.cpp index e6b4a813b490935ea8b6901a6cdfaa14ec266cb6..c49960f5215db441bd191154660b2bd4760044d0 100644 --- a/src/observation_downsampler.cpp +++ b/src/observation_downsampler.cpp @@ -98,6 +98,7 @@ void CObservationDownsampler::addGroundObservationInSensorFrame( num_points_[index]++; pcl::PointXYZRGBNormal aux = ground_observation_in_sensor_frame; + // TODO: rename this to clarify the process aux.data_c[GRND_REF_DATA_C_3_Z_VARIANCE] = azimuth_angle; observations_[index].push_back(aux); } @@ -149,9 +150,10 @@ void CObservationDownsampler::generateNewGroundReferences( // << " Centroid with accums = " << z_centroid << " variance = " << z_centroid_variance << std::endl; //assert(z_centroid == point.z && "something goes wrong!"); - point.r = 0; // just to not leave uninitialized values - point.g = 0; - point.b = 0; + point.data_c[GRND_REF_DATA_C_0_RGB_CAST_INTO_FLOAT] = (float)median_point.data_n[DATA_N_0_ELEVATION_CLOUD_INDEX_OF_POINT_IN_ORIGINAL_CLOUD]; +// point.r = 0; // just to not leave uninitialized values +// point.g = 0; +// point.b = 0; // Warning! the index is computed as size() because we haven't added the point yet! point.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] = (float)ground_reference_cloud_ptr->points.size();