more cleaning

include/structs_definitions.h

@@ -14,10 +14,8 @@ const float OUT_OF_RANGE = 1000.0;
 const float MAX_RANGE = 100.0;
 
 const int DATA_N_0_INTENSITY = 0;
-
-const int DATA_N_1_Z_GROUND = 1;
-const int DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE = 1;
-
+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;
 
@@ -25,12 +23,9 @@ const int DATA_C_0_RGB_CAST_INTO_FLOAT = 0;
 const int DATA_C_1_ID_CLASS = 1;
 
 const int DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL = 2;
-const int DATA_C_2_MEAN_INTENSITY = 2;
 
 const int DATA_C_3_ORIGINAL_INDEX = 3;
 
-const int DATA_C_2_CAR_PROB = 2; // TODO: remove this
-
 const int GRND_REF_DATA_C_0_RGB_CAST_INTO_FLOAT = 0;
 const int GRND_REF_DATA_C_1_ROLL = 1;
 const int GRND_REF_DATA_C_2_PITCH = 2;
@@ -47,12 +42,12 @@ const int INDEX_UNKNOWN = -1;
 const float VERTEX_CONNECTED_TO_ROOT = 0.0;
 const float VERTEX_NOT_CONNECTED_TO_ROOT = 100.0;
 
-const float Z_VARIANCE_UNKNOWN = -1.0;
-const float Z_MEAN_UNKNOWN = -1.0;
+const float UNKNOWN_MEAN = -1000000.0;
+const float UNKOWN_VARIANCE = -1.0;
 
-const float INTENSITY_UNKNOWN = -1.0;
-const float PROB_UNKNOWN = -1.0;
-const float Z_GROUND_UNKNOWN = -1000000.0;
+const float UNKNOWN_INTENSITY = -1.0;
+const float UNKNOWN_PROB = -1.0;
+const float UNKNOWN_Z_GROUND = -1000000.0;
 
 const int KITTI_NUMBER_OF_PRECISION_RECALL_INTERVALES = 40;
 

src/kf_based_terrain_analysis.cpp

@@ -462,13 +462,6 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
 
           if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND)
           {
-//          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
-
-            // this is redundant, so we now use the data_n_1 channel to store intensity variance.
-//            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
             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;
@@ -503,13 +496,6 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
             elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] =
                 reference_index + score;
 
-          //Deprecated, we now use the data_n_1 channel to store intensity variance
-//          // 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 + score; //0.99;
@@ -806,14 +792,14 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
 
     // Filling remaining fields
     elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = intensity_mean;
-    elevation_cloud_point.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] = var_intensity;
+    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_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
 
-    //std::cout << "Intensity of elevation point AFTER storing it: mean = " << elevation_cloud_point.data_n[DATA_N_0_INTENSITY] << "    std = " << sqrt( elevation_cloud_point.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE]) << std::endl;
+    //std::cout << "Intensity of elevation point AFTER storing it: mean = " << elevation_cloud_point.data_n[DATA_N_0_INTENSITY] << "    std = " << sqrt( elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE]) << std::endl;
 
     //std::cout << "var z = " << it->data_n[DATA_N_3_Z_VARIANCE] << std::endl;
     //std::cout << "elevation_cell_vector[i].z_coordinates.size() = " << z_coordinates.size() << std::endl;
@@ -1028,18 +1014,18 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
 
         pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] =
             point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
-        pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] =
-            point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE];
+        pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_INTENSITY_VARIANCE] =
+            point_in_sensor_frame.data_n[DATA_N_1_INTENSITY_VARIANCE];
 
 //        std::cout << "Intensity in elevation cloud: mean = " << point_in_sensor_frame.data_n[DATA_N_0_INTENSITY]
 //            << "    var = "
-//            << point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE]
+//            << point_in_sensor_frame.data_n[DATA_N_1_INTENSITY_VARIANCE]
 //            << std::endl;
 //
 //        std::cout << "Intensity in pcl cloud: point = "
 //            << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_INTENSITY] << "    mean = "
 //            << pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] << "    var = "
-//            << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] << std::endl;
+//            << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_INTENSITY_VARIANCE] << std::endl;
 
         pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] = point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
         pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
@@ -1061,10 +1047,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
         }
         else
         {
-          //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
-          //pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = point_in_sensor_frame.z;
-
-          //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
           if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
               < filtering_configuration.robot_height)
           {
@@ -1102,8 +1084,8 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
         {
           pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] =
               point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
-          pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] =
-              point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE];
+          pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_INTENSITY_VARIANCE] =
+              point_in_sensor_frame.data_n[DATA_N_1_INTENSITY_VARIANCE];
 
           pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] =
               point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];