local_planner/src/nodes/local_planner.cpp
// get 3D points from old histogram
get 3D points from old histogram
get 3D points from old histogram
void LocalPlanner::reprojectPoints(Histogram histogram) {
  double n_points = 0;
  double dist, age;
  geometry_msgs::Point temp_array[4];
  reprojected_points_age_.clear();
  reprojected_points_dist_.clear();
  reprojected_points_.points.clear();
  reprojected_points_.header.stamp = final_cloud_.header.stamp;
  reprojected_points_.header.frame_id = "local_origin";
  for (int e = 0; e < GRID_LENGTH_E; e++) {
    for (int z = 0; z < GRID_LENGTH_Z; z++) {
      if (histogram.get_bin(e, z) != 0) {
        n_points++;
        double beta_e = elevationIndexToAngle(e, ALPHA_RES);
        double beta_z = azimuthIndexToAngle(z, ALPHA_RES);
        temp_array[0] =
            fromPolarToCartesian(beta_e + ALPHA_RES / 2, beta_z + ALPHA_RES / 2,
                                 histogram.get_dist(e, z), position_old_);
        temp_array[1] =
            fromPolarToCartesian(beta_e - ALPHA_RES / 2, beta_z + ALPHA_RES / 2,
                                 histogram.get_dist(e, z), position_old_);
        temp_array[2] =
            fromPolarToCartesian(beta_e + ALPHA_RES / 2, beta_z - ALPHA_RES / 2,
                                 histogram.get_dist(e, z), position_old_);
        temp_array[3] =
            fromPolarToCartesian(beta_e - ALPHA_RES / 2, beta_z - ALPHA_RES / 2,
                                 histogram.get_dist(e, z), position_old_);
        for (int i = 0; i < 4; i++) {
          dist = distance3DCartesian(pose_.pose.position, temp_array[i]);
          age = histogram.get_age(e, z);
          if (dist < 2 * histogram_box_size_.xmax_ && dist > 0.3 &&
              age < reproj_age_) {
            reprojected_points_.points.push_back(pcl::PointXYZ(
                temp_array[i].x, temp_array[i].y, temp_array[i].z));
            reprojected_points_age_.push_back(age);
            reprojected_points_dist_.push_back(dist);
          }
        }
      }
    }
  }
}
void LocalPlanner :: reprojectPoints ( Histogram histogram ) { double n_points = 0 ; double dist , age ; geometry_msgs :: Point temp_array [ 4 ] ; reprojected_points_age_ . clear ( ) ; reprojected_points_dist_ . clear ( ) ; reprojected_points_ . points . clear ( ) ; reprojected_points_ . header . stamp = final_cloud_ . header . stamp ; reprojected_points_ . header . frame_id = " " ; for ( int e = 0 ; e < GRID_LENGTH_E ; e ++ ) { for ( int z = 0 ; z < GRID_LENGTH_Z ; z ++ ) { if ( histogram . get_bin ( e , z ) != 0 ) { n_points ++ ; double beta_e = elevationIndexToAngle ( e , ALPHA_RES ) ; double beta_z = azimuthIndexToAngle ( z , ALPHA_RES ) ; temp_array [ 0 ] = fromPolarToCartesian ( beta_e + ALPHA_RES / 2 , beta_z + ALPHA_RES / 2 , histogram . get_dist ( e , z ) , position_old_ ) ; temp_array [ 1 ] = fromPolarToCartesian ( beta_e - ALPHA_RES / 2 , beta_z + ALPHA_RES / 2 , histogram . get_dist ( e , z ) , position_old_ ) ; temp_array [ 2 ] = fromPolarToCartesian ( beta_e + ALPHA_RES / 2 , beta_z - ALPHA_RES / 2 , histogram . get_dist ( e , z ) , position_old_ ) ; temp_array [ 3 ] = fromPolarToCartesian ( beta_e - ALPHA_RES / 2 , beta_z - ALPHA_RES / 2 , histogram . get_dist ( e , z ) , position_old_ ) ; for ( int i = 0 ; i < 4 ; i ++ ) { dist = distance3DCartesian ( pose_ . pose . position , temp_array [ i ] ) ; age = histogram . get_age ( e , z ) ; if ( dist < 2 * histogram_box_size_ . xmax_ && dist > 0.3 && age < reproj_age_ ) { reprojected_points_ . points . push_back ( pcl :: PointXYZ ( temp_array [ i ] . x , temp_array [ i ] . y , temp_array [ i ] . z ) ) ; reprojected_points_age_ . push_back ( age ) ; reprojected_points_dist_ . push_back ( dist ) ; } } } } } }
get 3D points from old histogram
get 3D points from old histogram
// transform from array index to angle // transform from Polar to Cartesian