static void
compute_laser_rays_from_velodyne_and_create_a_local_map(sensor_parameters_t *velodyne_params, sensor_data_t *velodyne_data, rotation_matrix *r_matrix_car_to_global,
carmen_pose_3D_t *robot_pose, carmen_vector_3D_t *robot_velocity,
double x_origin, double y_origin, int point_cloud_index, double phi)
{
spherical_point_cloud v_zt = velodyne_data->points[point_cloud_index];
int N = v_zt.num_points / velodyne_params->vertical_resolution;
double dt = 1.0 / (1808.0 * 12.0);
carmen_pose_3D_t robot_interpolated_position = *robot_pose;
// Ray-trace the grid
int jump = filter->param->jump_size;
for (int k = 0; k < N; k += jump)
{
robot_interpolated_position = carmen_ackerman_interpolated_robot_position_at_time(*robot_pose, (double) k * dt, robot_velocity->x, phi, car_config.distance_between_front_and_rear_axles);
r_matrix_car_to_global = compute_rotation_matrix(r_matrix_car_to_global, robot_interpolated_position.orientation);
carmen_prob_models_compute_relevant_map_coordinates(velodyne_data, velodyne_params, k * velodyne_params->vertical_resolution, robot_interpolated_position.position, sensor_board_1_pose,
r_matrix_car_to_global, sensor_board_1_to_car_matrix, robot_wheel_radius, x_origin, y_origin, &car_config, 0, 0);
carmen_prob_models_get_occuppancy_log_odds_via_unexpeted_delta_range(velodyne_data, velodyne_params, k * velodyne_params->vertical_resolution, highest_sensor, safe_range_above_sensors, 0, 0);
carmen_prob_models_upgrade_log_odds_of_cells_hit_by_rays(&local_map, velodyne_params, velodyne_data, 0);
carmen_prob_models_update_intensity_of_cells_hit_by_rays(&local_sum_remission_map, &local_sum_sqr_remission_map, &local_count_remission_map, velodyne_params, velodyne_data, highest_sensor, safe_range_above_sensors, NULL, 0);
}
}
static void
update_cells_in_the_velodyne_perceptual_field(sensor_parameters_t *sensor_params, sensor_data_t *sensor_data, rotation_matrix *r_matrix_robot_to_global,
int point_cloud_index)
{
int i, j;
spherical_point_cloud v_zt = sensor_data->points[point_cloud_index];
carmen_pose_3D_t robot_interpolated_position;
double v = sensor_data->robot_velocity[point_cloud_index].x;
double phi = sensor_data->robot_phi[point_cloud_index];
double scan_time = (v_zt.num_points / sensor_params->vertical_resolution) * sensor_params->time_spent_by_each_scan;
double dt = (sensor_data->current_timestamp - sensor_data->robot_timestamp[point_cloud_index]) - scan_time;
// Ray-trace the grid
for (i = 0, j = 0; i < v_zt.num_points; i = i + sensor_params->vertical_resolution, j++, dt += sensor_params->time_spent_by_each_scan)
{
robot_interpolated_position = carmen_ackerman_interpolated_robot_position_at_time(sensor_data->robot_pose[point_cloud_index], dt, v, phi, car_config.distance_between_front_and_rear_axles);
r_matrix_robot_to_global = compute_rotation_matrix(r_matrix_car_to_global, robot_interpolated_position.orientation);
change_sensor_rear_range_max(sensor_params, v_zt.sphere_points[i].horizontal_angle);
carmen_prob_models_compute_relevant_map_coordinates(sensor_data, sensor_params, i, robot_interpolated_position.position, sensor_board_1_pose,
r_matrix_robot_to_global, sensor_board_1_to_car_matrix, robot_wheel_radius, g_map_origin.x, g_map_origin.y, &car_config, 0, 0);
carmen_prob_models_get_occuppancy_log_odds_via_unexpeted_delta_range(sensor_data, sensor_params, i,
highest_sensor, safe_range_above_sensors, 0, 0);
carmen_prob_models_update_intensity_of_cells_hit_by_rays(&sum_remission_map, &sum_sqr_remission_map, &count_remission_map, sensor_params, sensor_data, highest_sensor, safe_range_above_sensors, NULL, 0);
}
}
static void add_velodyne_spherical_points_to_pointcloud(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud,
spherical_point_cloud *v_zt, unsigned char *intensity,
rotation_matrix *r_matrix_car_to_global, carmen_pose_3D_t *robot_pose,
sensor_parameters_t *velodyne_params, sensor_data_t *velodyne_data) {
int i, j, k;
double dt = 0;
carmen_pose_3D_t robot_interpolated_position;
double v = velodyne_data->robot_velocity[velodyne_data->point_cloud_index].x;
double phi = velodyne_data->robot_phi[velodyne_data->point_cloud_index];
carmen_robot_ackerman_config_t car_config;
car_config.distance_between_front_and_rear_axles = distance_between_front_and_rear_axles;
car_config.distance_between_rear_wheels = robot_width;
for (i = 0, j = 0; i < v_zt->num_points;
i = i + 1 * velodyne_params->vertical_resolution, j += 1) {
dt = j * velodyne_params->time_spent_by_each_scan;
robot_interpolated_position =
carmen_ackerman_interpolated_robot_position_at_time(*robot_pose,
dt, v, phi, distance_between_front_and_rear_axles);
r_matrix_car_to_global = compute_rotation_matrix(r_matrix_car_to_global,
robot_interpolated_position.orientation);
carmen_prob_models_compute_relevant_map_coordinates(velodyne_data, velodyne_params, i, robot_interpolated_position.position, sensor_board_1_pose,
r_matrix_car_to_global, sensor_board_1_to_car_matrix, robot_wheel_radius, 0.0, 0.0, &car_config, 0);
carmen_prob_models_get_occuppancy_log_odds_via_unexpeted_delta_range(velodyne_data, velodyne_params, i,
2.0, 10.0, velodyne_params->delta_difference_mean, velodyne_params->delta_difference_stddev);
for (k = 0; k < velodyne_params->vertical_resolution; k++)
{
carmen_vector_3D_t point = get_global_point_from_velodyne_point(
v_zt->sphere_points[i + k], velodyne_params,
r_matrix_car_to_global,
robot_interpolated_position.position);
if (velodyne_data->occupancy_log_odds_of_each_ray_target[k] > velodyne_params->log_odds.log_odds_l0)
{
// printf("aqui\n");
if (point_is_valid(v_zt->sphere_points[i + k], velodyne_params))
add_velodyne_point_to_pointcloud(pointcloud, intensity[i + k],
point);
}
}
}
}
void
localalize_using_map_set_robot_pose_into_the_map(double v, double phi, double timestamp)
{
static double last_timestamp = timestamp;
globalpos_initialized = 1;
robot_pose = carmen_ackerman_interpolated_robot_position_at_time(robot_pose, timestamp - last_timestamp, v, phi, car_config.distance_between_front_and_rear_axles);
r_matrix_car_to_global = compute_rotation_matrix(r_matrix_car_to_global, robot_pose.orientation);
last_timestamp = timestamp;
}
