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