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); }