carmen_vector_3D_t carmen_get_sensor_sphere_point_in_robot_cartesian_reference(carmen_sphere_coord_t sphere_point, carmen_pose_3D_t sensor_pose, carmen_pose_3D_t sensor_board_pose, rotation_matrix* sensor_to_board_matrix, rotation_matrix* board_to_car_matrix) { carmen_vector_3D_t sensor_reference, board_reference, car_reference; // **************************************** // TODO: Colocar TF nessas transformadas!!! // **************************************** sensor_reference = carmen_covert_sphere_to_cartesian_coord(sphere_point); board_reference = carmen_change_sensor_reference(sensor_pose.position, sensor_reference, sensor_to_board_matrix); car_reference = carmen_change_sensor_reference(sensor_board_pose.position, board_reference, board_to_car_matrix); return car_reference; }
// @@@ Esta funcao nao considera a velocidade angular carmen_vector_3D_t carmen_get_interpolated_robot_position_at_time(carmen_pose_3D_t robot_pose, carmen_vector_3D_t robot_velocity, double robot_time, double interpolated_time, rotation_matrix* r_matrix_robot_to_global) { double dt = interpolated_time - robot_time; carmen_vector_3D_t displacement_robot_reference; displacement_robot_reference.x = robot_velocity.x * dt; // @@@ a variacao angular nao ee considerada... displacement_robot_reference.y = robot_velocity.y * dt; displacement_robot_reference.z = robot_velocity.z * dt; return carmen_change_sensor_reference(robot_pose.position, displacement_robot_reference, r_matrix_robot_to_global); }
carmen_vector_3D_t get_global_point_from_velodyne_point( carmen_sphere_coord_t sphere_point, sensor_parameters_t *velodyne_params, rotation_matrix *r_matrix_car_to_global, carmen_vector_3D_t robot_position) { carmen_vector_3D_t point_position_in_the_robot = carmen_get_sensor_sphere_point_in_robot_cartesian_reference( sphere_point, velodyne_params->pose, sensor_board_1_pose, velodyne_params->sensor_to_board_matrix, sensor_board_1_to_car_matrix); carmen_vector_3D_t global_point_position_in_the_world = carmen_change_sensor_reference(robot_position, point_position_in_the_robot, r_matrix_car_to_global); return global_point_position_in_the_world; }
static void get_sensors_param(int argc, char **argv) { int i, j; int flipped; int horizontal_resolution; char stereo_velodyne_string[256]; int stereo_velodyne_vertical_roi_ini; int stereo_velodyne_vertical_roi_end; int stereo_velodyne_horizontal_roi_ini; int stereo_velodyne_horizontal_roi_end; int roi_ini, roi_end; spherical_sensor_params[0].pose = velodyne_pose; spherical_sensor_params[0].sensor_robot_reference = carmen_change_sensor_reference(sensor_board_1_pose.position, spherical_sensor_params[0].pose.position, sensor_board_1_to_car_matrix); spherical_sensor_params[0].height = spherical_sensor_params[0].sensor_robot_reference.z + robot_wheel_radius; if (spherical_sensor_params[0].height > highest_sensor) highest_sensor = spherical_sensor_params[0].height; if (spherical_sensor_params[0].alive && !strcmp(spherical_sensor_params[0].name,"velodyne")) { spherical_sensor_params[0].ray_order = carmen_velodyne_get_ray_order(); spherical_sensor_params[0].vertical_correction = carmen_velodyne_get_vertical_correction(); spherical_sensor_params[0].delta_difference_mean = carmen_velodyne_get_delta_difference_mean(); spherical_sensor_params[0].delta_difference_stddev = carmen_velodyne_get_delta_difference_stddev(); carmen_param_t param_list[] = { {spherical_sensor_params[0].name, (char*)"vertical_resolution", CARMEN_PARAM_INT, &spherical_sensor_params[0].vertical_resolution, 0, NULL}, {(char *)"localize_ackerman", (char*)"velodyne_range_max", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[0].range_max, 0, NULL}, {spherical_sensor_params[0].name, (char*)"time_spent_by_each_scan", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[0].time_spent_by_each_scan, 0, NULL}, }; carmen_param_install_params(argc, argv, param_list, sizeof(param_list) / sizeof(param_list[0])); init_velodyne_points(&spherical_sensor_data[0].points, &spherical_sensor_data[0].intensity, &spherical_sensor_data[0].robot_pose, &spherical_sensor_data[0].robot_velocity, &spherical_sensor_data[0].robot_timestamp, &spherical_sensor_data[0].robot_phi); spherical_sensor_params[0].sensor_to_support_matrix = create_rotation_matrix(spherical_sensor_params[0].pose.orientation); spherical_sensor_data[0].point_cloud_index = 0; carmen_prob_models_alloc_sensor_data(&spherical_sensor_data[0], spherical_sensor_params[0].vertical_resolution, number_of_threads); if (max_range < spherical_sensor_params[0].range_max) { max_range = spherical_sensor_params[0].range_max; } spherical_sensor_params[0].current_range_max = spherical_sensor_params[0].range_max; } for (i = 1; i < number_of_sensors; i++) { if (spherical_sensor_params[i].alive) { spherical_sensor_params[i].pose = get_stereo_velodyne_pose_3D(argc, argv, i); spherical_sensor_params[i].sensor_robot_reference = carmen_change_sensor_reference(sensor_board_1_pose.position, spherical_sensor_params[i].pose.position, sensor_board_1_to_car_matrix); spherical_sensor_params[i].height = spherical_sensor_params[i].sensor_robot_reference.z + robot_wheel_radius; if (spherical_sensor_params[i].height > highest_sensor) highest_sensor = spherical_sensor_params[i].height; sprintf(stereo_velodyne_string, "%s%d", "stereo", i); carmen_param_t param_list[] = { {spherical_sensor_params[i].name, (char*) "vertical_resolution", CARMEN_PARAM_INT, &spherical_sensor_params[i].vertical_resolution, 0, NULL}, {spherical_sensor_params[i].name, (char*) "horizontal_resolution", CARMEN_PARAM_INT, &horizontal_resolution, 0, NULL}, {spherical_sensor_params[i].name, (char*) "flipped", CARMEN_PARAM_ONOFF, &flipped, 0, NULL}, {spherical_sensor_params[i].name, (char*) "range_max", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[i].range_max, 0, NULL}, {spherical_sensor_params[i].name, (char*) "vertical_roi_ini", CARMEN_PARAM_INT, &stereo_velodyne_vertical_roi_ini, 0, NULL }, {spherical_sensor_params[i].name, (char*) "vertical_roi_end", CARMEN_PARAM_INT, &stereo_velodyne_vertical_roi_end, 0, NULL }, {spherical_sensor_params[i].name, (char*) "horizontal_roi_ini", CARMEN_PARAM_INT, &stereo_velodyne_horizontal_roi_ini, 0, NULL }, {spherical_sensor_params[i].name, (char*) "horizontal_roi_end", CARMEN_PARAM_INT, &stereo_velodyne_horizontal_roi_end, 0, NULL } }; carmen_param_install_params(argc, argv, param_list, sizeof(param_list) / sizeof(param_list[0])); if (flipped) { spherical_sensor_params[i].vertical_resolution = horizontal_resolution; roi_ini = stereo_velodyne_horizontal_roi_ini; roi_end = stereo_velodyne_horizontal_roi_end; } else { roi_ini = stereo_velodyne_vertical_roi_ini; roi_end = stereo_velodyne_vertical_roi_end; } if (spherical_sensor_params[i].vertical_resolution > (roi_end - roi_ini)) { carmen_die("The stereo_velodyne_vertical_resolution is bigger than stereo point cloud height"); } if (max_range < spherical_sensor_params[i].range_max) { max_range = spherical_sensor_params[i].range_max; } spherical_sensor_params[i].current_range_max = spherical_sensor_params[i].range_max; spherical_sensor_params[i].range_max_factor = 1.0; spherical_sensor_params[i].ray_order = generates_ray_order(spherical_sensor_params[i].vertical_resolution); spherical_sensor_params[i].vertical_correction = get_variable_velodyne_correction(); // spherical_sensor_params[i].vertical_correction = get_stereo_velodyne_correction(flipped, i, spherical_sensor_params[i].vertical_resolution, roi_ini, roi_end, 0, 0); init_velodyne_points(&spherical_sensor_data[i].points, &spherical_sensor_data[i].intensity, &spherical_sensor_data[i].robot_pose, &spherical_sensor_data[i].robot_velocity, &spherical_sensor_data[i].robot_timestamp, &spherical_sensor_data[i].robot_phi); spherical_sensor_params[i].sensor_to_support_matrix = create_rotation_matrix(spherical_sensor_params[i].pose.orientation); spherical_sensor_data[i].point_cloud_index = 0; carmen_prob_models_alloc_sensor_data(&spherical_sensor_data[i], spherical_sensor_params[i].vertical_resolution, number_of_threads); //TODO : tem que fazer esta medida para as cameras igual foi feito para o velodyne if(i == 8) { spherical_sensor_params[i].delta_difference_mean = carmen_velodyne_get_delta_difference_mean(); spherical_sensor_params[i].delta_difference_stddev = carmen_velodyne_get_delta_difference_stddev(); } else { spherical_sensor_params[i].delta_difference_mean = (double *)calloc(50, sizeof(double)); spherical_sensor_params[i].delta_difference_stddev = (double *)calloc(50, sizeof(double)); for (j = 0; j < 50; j++) spherical_sensor_params[i].delta_difference_stddev[j] = 1.0; } } } }