void accumulate_partial_scan(
carmen_velodyne_partial_scan_message *velodyne_message,
sensor_parameters_t *velodyne_params, sensor_data_t *velodyne_data,
carmen_pose_3D_t *robot_pose, carmen_vector_3D_t *robot_velocity,
double phi, double robot_timestamp) {
static int velodyne_message_id;
int num_points = velodyne_message->number_of_32_laser_shots
* velodyne_params->vertical_resolution;
if (velodyne_data->last_timestamp == 0.0) {
velodyne_data->last_timestamp = velodyne_message->timestamp;
velodyne_message_id = -2;// correntemente sao necessarias pelo menos 2 mensagens para se ter uma volta completa de velodyne
return;
}
velodyne_data->current_timestamp = velodyne_message->timestamp;
build_sensor_point_cloud(&(velodyne_data->points),
(velodyne_data->intensity), &(velodyne_data->point_cloud_index),
num_points, NUM_VELODYNE_POINT_CLOUDS);
carmen_velodyne_partial_scan_update_points(velodyne_message,
velodyne_params->vertical_resolution,
&(velodyne_data->points[velodyne_data->point_cloud_index]),
(velodyne_data->intensity[velodyne_data->point_cloud_index]),
velodyne_params->ray_order, velodyne_params->vertical_correction,
velodyne_params->range_max);
velodyne_data->robot_pose[velodyne_data->point_cloud_index] = *robot_pose;
velodyne_data->robot_phi[velodyne_data->point_cloud_index] = phi;
velodyne_data->robot_velocity[velodyne_data->point_cloud_index] =
*robot_velocity;
velodyne_data->robot_timestamp[velodyne_data->point_cloud_index] =
robot_timestamp;
velodyne_message_id++;
velodyne_data->last_timestamp = velodyne_message->timestamp;
// fill the partial scans array
if (current_point_cloud_partial_scan_index >= 1)
{
received_enough_pointclouds_partial_scans = 1;
} else
{
accumulated_correction_transform[velodyne_data->point_cloud_index] = generate_eigen_pose_from_carmen_pose(robot_pose);
current_point_cloud_partial_scan_index++;
}
}
// Computes local_compacted_map and local_compacted_mean_remission_map maps
int
localize_ackerman_velodyne_partial_scan(carmen_velodyne_partial_scan_message *velodyne_message, sensor_parameters_t *velodyne_params, sensor_data_t *velodyne_data,
carmen_vector_3D_t *robot_velocity, double phi)
{
static rotation_matrix *r_matrix_car_to_global = NULL;
static int velodyne_message_id;
int current_point_cloud_index;
int num_points = velodyne_message->number_of_32_laser_shots * velodyne_params->vertical_resolution;
if (velodyne_data->last_timestamp == 0.0)
{
velodyne_data->last_timestamp = velodyne_message->timestamp;
velodyne_message_id = -2; // correntemente sao necessarias pelo menos 2 mensagens para se ter uma volta completa de velodyne
return (0);
}
velodyne_data->current_timestamp = velodyne_message->timestamp;
build_sensor_point_cloud(&(velodyne_data->points), velodyne_data->intensity, &(velodyne_data->point_cloud_index), num_points, NUM_VELODYNE_POINT_CLOUDS);
carmen_velodyne_partial_scan_update_points(velodyne_message,
velodyne_params->vertical_resolution,
&(velodyne_data->points[velodyne_data->point_cloud_index]),
velodyne_data->intensity[velodyne_data->point_cloud_index],
velodyne_params->ray_order,
velodyne_params->vertical_correction,
velodyne_params->range_max,
velodyne_message->timestamp);
//if (velodyne_viewer)
//debug_remission_map_velodyne(velodyne_message, velodyne_params);
if (velodyne_message_id >= 0)
{
carmen_pose_3D_t local_pose;
local_pose.position.x = (local_map.config.x_size * local_map.config.resolution) / 2.0;
local_pose.position.y = (local_map.config.x_size * local_map.config.resolution) / 2.0;
local_pose.position.z = 0;
local_pose.orientation.pitch = local_pose.orientation.roll = local_pose.orientation.yaw = 0.0;
r_matrix_car_to_global = compute_rotation_matrix(r_matrix_car_to_global, local_pose.orientation);
current_point_cloud_index = velodyne_data->point_cloud_index;
compute_laser_rays_from_velodyne_and_create_a_local_map(velodyne_params, velodyne_data, r_matrix_car_to_global, &local_pose, robot_velocity, 0.0, 0.0, current_point_cloud_index, phi);
carmen_prob_models_calc_mean_and_variance_remission_map(&local_mean_remission_map, &local_variance_remission_map,
&local_sum_remission_map, &local_sum_sqr_remission_map, &local_count_remission_map);
carmen_prob_models_free_compact_map(&local_compacted_map);
carmen_prob_models_free_compact_map(&local_compacted_mean_remission_map);
// carmen_prob_models_free_compact_map(&local_compacted_variance_remission_map);
// Build local_compacted_map from local_map computed by compute_laser_rays_from_velodyne_and_create_a_local_map()
carmen_prob_models_create_compact_map(&local_compacted_map, &local_map, -1.0);
carmen_prob_models_create_compact_map(&local_compacted_mean_remission_map, &local_mean_remission_map, -1.0);
// carmen_prob_models_create_compact_map(&local_compacted_variance_remission_map, &local_variance_remission_map, -1.0);
// Clear maps for next point cloud
carmen_prob_models_clear_carmen_map_using_compact_map(&local_map, &local_compacted_map, -1.0);
carmen_prob_models_clear_carmen_map_using_compact_map(&local_mean_remission_map, &local_compacted_mean_remission_map, -1.0);
// carmen_prob_models_clear_carmen_map_using_compact_map(&local_variance_remission_map, &local_compacted_variance_remission_map, -1.0);
carmen_prob_models_clear_carmen_map_using_compact_map(&local_sum_remission_map, &local_compacted_mean_remission_map, -1.0);
// carmen_prob_models_clear_carmen_map_using_compact_map(&local_sum_sqr_remission_map, &local_compacted_variance_remission_map, -1.0);
carmen_prob_models_clear_carmen_map_using_compact_map(&local_count_remission_map, &local_compacted_mean_remission_map, -1.0);
if (velodyne_message_id > 1000000)
velodyne_message_id = 0;
}
velodyne_message_id++;
velodyne_data->last_timestamp = velodyne_message->timestamp;
if (velodyne_message_id >= 0)
return (1);
else
return (0);
}
int
localalize_using_map_velodyne_partial_scan(carmen_velodyne_partial_scan_message *velodyne_message)
{
static int velodyne_message_id;
static IplImage *img_gray = NULL;
int ok_to_publish;
int num_points = velodyne_message->number_of_32_laser_shots * spherical_sensor_params[0].vertical_resolution;
ok_to_publish = 0;
if (!globalpos_initialized)
return (ok_to_publish);
if (spherical_sensor_data[0].last_timestamp == 0.0)
{
spherical_sensor_data[0].last_timestamp = velodyne_message->timestamp;
velodyne_message_id = -2; // correntemente sao necessarias pelo menos 2 mensagens para ter uma volta completa de velodyne
return (ok_to_publish);
}
spherical_sensor_data[0].current_timestamp = velodyne_message->timestamp;
build_sensor_point_cloud(&spherical_sensor_data[0].points, spherical_sensor_data[0].intensity, &spherical_sensor_data[0].point_cloud_index, num_points, NUM_VELODYNE_POINT_CLOUDS);
carmen_velodyne_partial_scan_update_points(velodyne_message, spherical_sensor_params[0].vertical_resolution,
&spherical_sensor_data[0].points[spherical_sensor_data[0].point_cloud_index], spherical_sensor_data[0].intensity[spherical_sensor_data[0].point_cloud_index],
spherical_sensor_params[0].ray_order,
spherical_sensor_params[0].vertical_correction,
spherical_sensor_params[0].range_max,
velodyne_message->timestamp);
spherical_sensor_data[0].robot_pose[spherical_sensor_data[0].point_cloud_index] = robot_pose;
spherical_sensor_data[0].robot_velocity[spherical_sensor_data[0].point_cloud_index] = robot_velocity;
spherical_sensor_data[0].robot_timestamp[spherical_sensor_data[0].point_cloud_index] = robot_pose_timestamp;
spherical_sensor_data[0].robot_phi[spherical_sensor_data[0].point_cloud_index] = robot_phi;
if (velodyne_message_id >= 0)
{
ok_to_publish = run_localalize_using_map(&spherical_sensor_params[0], &spherical_sensor_data[0], r_matrix_car_to_global);
if (velodyne_message_id > 1000000)
velodyne_message_id = 0;
}
velodyne_message_id++;
spherical_sensor_data[0].last_timestamp = velodyne_message->timestamp;
if (img_gray == NULL)
{
img_gray = cvCreateImage(cvSize(current_mean_remission_map.config.x_size, current_mean_remission_map.config.y_size), IPL_DEPTH_8U, 3);
}
// for (int i = 0; i < current_mean_remission_map.config.x_size; i++)
// {
// for(int j = 0; j < current_mean_remission_map.config.y_size; j++)
// {
// img_gray->imageData[i * img_gray->widthStep + 3 * j] = 255;
// img_gray->imageData[i * img_gray->widthStep + 3 * j + 1] = 0;
// img_gray->imageData[i * img_gray->widthStep + 3 * j + 2] = 0;
// }
// }
// for (int i = 0; i < local_compacted_mean_remission_map.number_of_known_points_on_the_map; i++)
// {
// if (local_compacted_mean_remission_map.coord_x[i] < 0 || local_compacted_mean_remission_map.coord_y[i] < 0 ||
// local_compacted_mean_remission_map.coord_x[i] > current_mean_remission_map.config.x_size ||
// local_compacted_mean_remission_map.coord_y[i] > current_mean_remission_map.config.y_size)
// continue;
//
// img_gray->imageData[local_compacted_mean_remission_map.coord_x[i] * img_gray->widthStep + 3 * local_compacted_mean_remission_map.coord_y[i]] = (unsigned char)(2550 * local_compacted_mean_remission_map.value[i]);
// img_gray->imageData[local_compacted_mean_remission_map.coord_x[i] * img_gray->widthStep + 3 * local_compacted_mean_remission_map.coord_y[i] + 1] = (unsigned char)(2550 * local_compacted_mean_remission_map.value[i]);
// img_gray->imageData[local_compacted_mean_remission_map.coord_x[i] * img_gray->widthStep + 3 * local_compacted_mean_remission_map.coord_y[i] + 2] = (unsigned char)(2550 * local_compacted_mean_remission_map.value[i]);
// }
for (int i = 0; i < current_mean_remission_map.config.x_size; i++)
{
for(int j = 0; j < current_mean_remission_map.config.y_size; j++)
{
if (current_mean_remission_map.map[i][j] < 0.0)
{
img_gray->imageData[i * img_gray->widthStep + 3 * j] = 255;
img_gray->imageData[i * img_gray->widthStep + 3 * j + 1] = 0;
img_gray->imageData[i * img_gray->widthStep + 3 * j + 2] = 0;
continue;
}
img_gray->imageData[i * img_gray->widthStep + 3 * j] = (unsigned char)(2550 * current_mean_remission_map.map[i][j]);
img_gray->imageData[i * img_gray->widthStep + 3 * j + 1] = (unsigned char)(2550 * current_mean_remission_map.map[i][j]);
img_gray->imageData[i * img_gray->widthStep + 3 * j + 2] = (unsigned char)(2550 * current_mean_remission_map.map[i][j]);
}
}
cvShowImage("1", img_gray);
cvWaitKey(33);
return (ok_to_publish);
}
