static void
transform_pcl_pose_to_carmen_pose(Eigen::Matrix<double, 4, 4> pcl_corrected_pose,
carmen_pose_3D_t *corrected_pose)
{
tf::Matrix3x3 rotation;
double roll, pitch, yaw;
rotation[0][0] = pcl_corrected_pose(0, 0);
rotation[0][1] = pcl_corrected_pose(0, 1);
rotation[0][2] = pcl_corrected_pose(0, 2);
rotation[1][0] = pcl_corrected_pose(1, 0);
rotation[1][1] = pcl_corrected_pose(1, 1);
rotation[1][2] = pcl_corrected_pose(1, 2);
rotation[2][0] = pcl_corrected_pose(2, 0);
rotation[2][1] = pcl_corrected_pose(2, 1);
rotation[2][2] = pcl_corrected_pose(2, 2);
rotation.getRPY(roll, pitch, yaw);
corrected_pose->orientation.roll = roll;
corrected_pose->orientation.pitch = pitch;
corrected_pose->orientation.yaw = yaw;
corrected_pose->position.x = pcl_corrected_pose(0, 3);
corrected_pose->position.y = pcl_corrected_pose(1, 3);
corrected_pose->position.z = pcl_corrected_pose(2, 3);
compute_rotation_matrix(corrected_pose_rotation,
corrected_pose->orientation);
}
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);
}
}
void ofxIcp::compute_rigid_transformation(const vector<cv::Point3d> & points_1, const cv::Mat & centroid_1, const vector<cv::Point3d> & points_2, const cv::Mat & centroid_2, cv::Mat & rotation, cv::Mat & translation){
cv::Mat covariance_matrix;
compute_covariance_matrix(points_1, centroid_1, points_2, centroid_2, covariance_matrix);
compute_rotation_matrix(covariance_matrix, rotation);
cv::Mat rot_centroid_1_mat = rotation * centroid_1.t();
translation = centroid_2 - rot_centroid_1_mat.t();
}
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;
}
int
localize_ackerman_velodyne_variable_scan(carmen_velodyne_variable_scan_message *message, sensor_parameters_t *velodyne_params, sensor_data_t *velodyne_data,
carmen_vector_3D_t *robot_velocity)
{
static rotation_matrix *r_matrix_car_to_global = NULL;
static int message_id;
int current_point_cloud_index;
int num_points = message->number_of_shots * velodyne_params->vertical_resolution;
if (velodyne_data->last_timestamp == 0.0)
{
velodyne_data->last_timestamp = message->timestamp;
message_id = -2; // correntemente sao necessarias pelo menos 2 mensagens para se ter uma volta completa de velodyne
return (0);
}
velodyne_data->current_timestamp = 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_variable_scan_update_points(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,
message->timestamp);
if (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, 0.0);
// carmen_prob_models_free_compact_map(&local_compacted_map);
// carmen_prob_models_create_compact_map(&local_compacted_map, &local_map);
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);
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);
create_binary_map(&binary_map, local_compacted_mean_remission_map);
carmen_prob_models_clear_carmen_map_using_compact_map(&local_map, &local_compacted_mean_remission_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);
carmen_prob_models_clear_carmen_map_using_compact_map(&local_map, &local_compacted_map, -1.0);
if (message_id > 1000000)
message_id = 0;
}
message_id++;
velodyne_data->last_timestamp = message->timestamp;
if (message_id >= 0)
return (1);
else
return (0);
}
int slam_icp_velodyne_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 last_globalpos_timestamp, carmen_pose_3D_t *corrected_pose_out) {
// initializations
static carmen_pose_3D_t zero_pose;
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud, target_pointcloud, source_pointcloud_leafed, target_pointcloud_leafed;
static rotation_matrix *r_matrix_car_to_global;
int converge = 0, current_point_cloud_index, before_point_cloud_index;
Eigen::Matrix<double, 4, 4> source_pointcloud_pose, target_pointcloud_pose;
Eigen::Matrix<double, 4, 4> pcl_corrected_pose, correction_transform;
static int i = 1;
static int icp_lag = 1;
if (first_iteraction) {
memset(&zero_pose, 0, sizeof(carmen_pose_3D_t));
robot_pose->orientation.pitch = robot_pose->orientation.roll =
robot_pose->position.z = 0.0;
corrected_pose = (carmen_pose_3D_t *) calloc(1,
sizeof(carmen_pose_3D_t));
*corrected_pose = *robot_pose;
first_iteraction = 0;
source_pointcloud =
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >(
new pcl::PointCloud<pcl::PointXYZRGB>);
target_pointcloud =
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >(
new pcl::PointCloud<pcl::PointXYZRGB>);
r_matrix_car_to_global = compute_rotation_matrix(r_matrix_car_to_global,
zero_pose.orientation);
}
*corrected_pose = *robot_pose;
accumulate_partial_scan(velodyne_message, velodyne_params, velodyne_data,
robot_pose, robot_velocity, phi, last_globalpos_timestamp);
if (two_complete_clouds_have_been_acquired()) {
current_point_cloud_index = velodyne_data->point_cloud_index;
before_point_cloud_index = ((velodyne_data->point_cloud_index - icp_lag)
+ NUM_VELODYNE_POINT_CLOUDS) % NUM_VELODYNE_POINT_CLOUDS;
// if (i < 3)
// {
// before_point_cloud_index = ((velodyne_data->point_cloud_index - i)
// + NUM_VELODYNE_POINT_CLOUDS) % NUM_VELODYNE_POINT_CLOUDS;
// i++;
// }
add_velodyne_spherical_points_to_pointcloud(target_pointcloud,
&(velodyne_data->points[before_point_cloud_index]),
velodyne_data->intensity[before_point_cloud_index],
r_matrix_car_to_global, &zero_pose, velodyne_params,
velodyne_data);
target_pointcloud_leafed = LeafSize(target_pointcloud, 0.2);
add_velodyne_spherical_points_to_pointcloud(source_pointcloud,
&(velodyne_data->points[current_point_cloud_index]),
velodyne_data->intensity[current_point_cloud_index],
r_matrix_car_to_global, &zero_pose, velodyne_params,
velodyne_data);
source_pointcloud_leafed = LeafSize(source_pointcloud, 0.2);
source_pointcloud_pose = generate_eigen_pose_from_carmen_pose(
&velodyne_data->robot_pose[current_point_cloud_index]);
target_pointcloud_pose = generate_eigen_pose_from_carmen_pose(
&velodyne_data->robot_pose[before_point_cloud_index]);
source_pointcloud_pose = //accumulated_correction_transform[before_point_cloud_index] *
//generate_eigen_pose_from_carmen_pose(&velodyne_data->robot_pose[before_point_cloud_index]).inverse() *
generate_eigen_pose_from_carmen_pose(&velodyne_data->robot_pose[current_point_cloud_index]);
// before_point_cloud_index = ((velodyne_data->point_cloud_index - 1)
// + NUM_VELODYNE_POINT_CLOUDS) % NUM_VELODYNE_POINT_CLOUDS;
target_pointcloud_pose =
generate_eigen_pose_from_carmen_pose(&velodyne_data->robot_pose[before_point_cloud_index]);//.inverse() *
//generate_eigen_pose_from_carmen_pose(&velodyne_data->robot_pose[current_point_cloud_index]);
converge = runGeneralizedICP(source_pointcloud_leafed, target_pointcloud_leafed,
source_pointcloud_pose, target_pointcloud_pose,
&correction_transform,
current_point_cloud_index,
before_point_cloud_index);
//
// before_point_cloud_index = ((velodyne_data->point_cloud_index - 1)
// + NUM_VELODYNE_POINT_CLOUDS) % NUM_VELODYNE_POINT_CLOUDS;
//
// target_pointcloud_pose =
// generate_eigen_pose_from_carmen_pose(&velodyne_data->robot_pose[before_point_cloud_index]);//.inverse() *
// * generate_eigen_pose_from_carmen_pose(
// &velodyne_data->robot_pose[current_point_cloud_index]);
source_pointcloud->clear();
target_pointcloud->clear();
source_pointcloud_leafed->clear();
target_pointcloud_leafed->clear();
if (converge)
{
accumulated_correction_transform[current_point_cloud_index] =
accumulated_correction_transform[before_point_cloud_index] *
correction_transform * target_pointcloud_pose.inverse() * source_pointcloud_pose;
pcl_corrected_pose = accumulated_correction_transform[current_point_cloud_index];
transform_pcl_pose_to_carmen_pose(pcl_corrected_pose,
corrected_pose);
}else
{
velodyne_data->point_cloud_index = ((velodyne_data->point_cloud_index - 1)
+ NUM_VELODYNE_POINT_CLOUDS) % NUM_VELODYNE_POINT_CLOUDS;
// accumulated_correction_transform[current_point_cloud_index] =
// accumulated_correction_transform[before_point_cloud_index] *
// target_pointcloud_pose.inverse() * source_pointcloud_pose;
}
}
corrected_pose->orientation.pitch = corrected_pose->orientation.roll =
corrected_pose->position.z = 0.0;
*corrected_pose_out = *corrected_pose;
return converge;
}
void
draw_tracking_moving_objects(moving_objects_tracking_t *moving_objects_tracking, int current_num_point_clouds,
carmen_vector_3D_t offset, int draw_particles_flag)
{
/*** MOVING OBJECTS MODULE ***/
int i;
rotation_matrix *rotate = NULL;
for (i = 0; i < current_num_point_clouds; i++)
{
if (moving_objects_tracking[i].geometric_model == -1)
continue;
carmen_pose_3D_t pos;
pos = moving_objects_tracking[i].moving_objects_pose;
carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
double correction_wheel_height = 0.28;
double W, L, H;
pos.position.x = pos.position.x - offset.x;
pos.position.y = pos.position.y - offset.y;
pos.position.z = 0.0;
W = moving_objects_tracking[i].model_features.geometry.width;
L = moving_objects_tracking[i].model_features.geometry.length;
H = moving_objects_tracking[i].model_features.geometry.height;
// W = moving_objects_tracking[i].width;
// L = moving_objects_tracking[i].length;
// H = moving_objects_tracking[i].height;
rotate = compute_rotation_matrix(NULL, pos.orientation);
glColor3d(moving_objects_tracking[i].model_features.red,
moving_objects_tracking[i].model_features.green,
moving_objects_tracking[i].model_features.blue);
p1.x = - L/2.0;
p1.y = - W/2.0;
p1.z = 0.0 - correction_wheel_height;
p2.x = L/2.0;
p2.y = - W/2.0;
p2.z = 0.0 - correction_wheel_height;
p3.x = L/2.0;
p3.y = W/2.0;
p3.z = 0.0 - correction_wheel_height;
p4.x = - L/2.0;
p4.y = W/2.0;
p4.z = 0.0 - correction_wheel_height;
p5.x = - L/2.0;
p5.y = - W/2.0;
p5.z = H - correction_wheel_height;
p6.x = L/2.0;
p6.y = - W/2.0;
p6.z = H - correction_wheel_height;
p7.x = L/2.0;
p7.y = W/2.0;
p7.z = H - correction_wheel_height;
p8.x = - L/2.0;
p8.y = W/2.0;
p8.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, p1);
p1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p2);
p2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p3);
p3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p4);
p4 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p5);
p5 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p6);
p6 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p7);
p7 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p8);
p8 = add_vectors(t, pos.position);
glBegin (GL_LINES);
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p1.x, p1.y, p1.z);
//////////////////////////////
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p5.x, p5.y, p5.z);
//////////////////////////////
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p8.x, p8.y, p8.z);
glEnd ();
/* Moving Object direction arrow */
s1.x = 0.0;
s1.y = 0.0;
s1.z = H - correction_wheel_height;
s2.x = L/2.0;
s2.y = 0.0;
s2.z = H - correction_wheel_height;
s3.x = (L/2.0) - 0.3;
s3.y = 0.2;
s3.z = H - correction_wheel_height;
s4.x = (L/2.0) - 0.3;
s4.y = -0.2;
s4.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, s1);
s1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s2);
s2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s3);
s3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s4);
s4 = add_vectors(t, pos.position);
glBegin(GL_LINES);
/* Part of arrow: | */
glVertex3d(s1.x, s1.y, s1.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: / */
glVertex3d(s3.x, s3.y, s3.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: \ */
glVertex3d(s4.x, s4.y, s4.z);
glVertex3d(s2.x, s2.y, s2.z);
glEnd();
//center of object
glPushAttrib(GL_POINT_BIT);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(s1.x, s1.y, s1.z);
glEnd();
glPopAttrib();
/* has to drawn after stuff above, so that it appears on top */
draw_number_associated(pos.position.x, pos.position.y, moving_objects_tracking[i].num_associated,
moving_objects_tracking[i].model_features.model_name);
draw_linear_velocity(pos.position.x, pos.position.y, moving_objects_tracking[i].linear_velocity,
moving_objects_tracking[i].model_features.geometry.height);
destroy_rotation_matrix(rotate);
if (draw_particles_flag == 1) {
// //todo para visualizar as particulas apenas
// for(j = 0; j < 10; j++){
// carmen_pose_3D_t pos;
// pos = moving_objects_tracking[i].moving_objects_pose;
// carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
// carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
// double correction_wheel_height = 0.28;
// double W, L, H;
//
// pos.position.x = moving_objects_tracking[i].particulas[j].pose.x - offset.x;
// pos.position.y = moving_objects_tracking[i].particulas[j].pose.y - offset.y;
// pos.position.z = 0.0;
// pos.orientation.yaw = moving_objects_tracking[i].particulas[j].pose.theta;
//
// W = moving_objects_tracking[i].particulas[j].geometry.width;
// L = moving_objects_tracking[i].particulas[j].geometry.length;
// H = moving_objects_tracking[i].particulas[j].geometry.height;
//
// // W = moving_objects_tracking[i].width;
// // L = moving_objects_tracking[i].length;
// // H = moving_objects_tracking[i].height;
//
// rotate = compute_rotation_matrix(NULL, pos.orientation);
//
// glColor3d(moving_objects_tracking[i].model_features.red -0.5,
// moving_objects_tracking[i].model_features.green -0.5,
// moving_objects_tracking[i].model_features.blue -0.5);
//
// p1.x = - L/2.0;
// p1.y = - W/2.0;
// p1.z = 0.0 - correction_wheel_height;
//
// p2.x = L/2.0;
// p2.y = - W/2.0;
// p2.z = 0.0 - correction_wheel_height;
//
// p3.x = L/2.0;
// p3.y = W/2.0;
// p3.z = 0.0 - correction_wheel_height;
//
// p4.x = - L/2.0;
// p4.y = W/2.0;
// p4.z = 0.0 - correction_wheel_height;
//
// p5.x = - L/2.0;
// p5.y = - W/2.0;
// p5.z = H - correction_wheel_height;
//
// p6.x = L/2.0;
// p6.y = - W/2.0;
// p6.z = H - correction_wheel_height;
//
// p7.x = L/2.0;
// p7.y = W/2.0;
// p7.z = H - correction_wheel_height;
//
// p8.x = - L/2.0;
// p8.y = W/2.0;
// p8.z = H - correction_wheel_height;
//
// t = multiply_matrix_vector(rotate, p1);
// p1 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p2);
// p2 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p3);
// p3 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p4);
// p4 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p5);
// p5 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p6);
// p6 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p7);
// p7 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, p8);
// p8 = add_vectors(t, pos.position);
//
// glBegin (GL_LINES);
//
// glVertex3d (p1.x, p1.y, p1.z);
// glVertex3d (p2.x, p2.y, p2.z);
//
// glVertex3d (p2.x, p2.y, p2.z);
// glVertex3d (p3.x, p3.y, p3.z);
//
// glVertex3d (p3.x, p3.y, p3.z);
// glVertex3d (p4.x, p4.y, p4.z);
//
// glVertex3d (p4.x, p4.y, p4.z);
// glVertex3d (p1.x, p1.y, p1.z);
// //////////////////////////////
//
// glVertex3d (p5.x, p5.y, p5.z);
// glVertex3d (p6.x, p6.y, p6.z);
//
// glVertex3d (p6.x, p6.y, p6.z);
// glVertex3d (p7.x, p7.y, p7.z);
//
// glVertex3d (p7.x, p7.y, p7.z);
// glVertex3d (p8.x, p8.y, p8.z);
//
// glVertex3d (p8.x, p8.y, p8.z);
// glVertex3d (p5.x, p5.y, p5.z);
// //////////////////////////////
//
// glVertex3d (p1.x, p1.y, p1.z);
// glVertex3d (p5.x, p5.y, p5.z);
//
// glVertex3d (p2.x, p2.y, p2.z);
// glVertex3d (p6.x, p6.y, p6.z);
//
// glVertex3d (p3.x, p3.y, p3.z);
// glVertex3d (p7.x, p7.y, p7.z);
//
// glVertex3d (p4.x, p4.y, p4.z);
// glVertex3d (p8.x, p8.y, p8.z);
//
// glEnd ();
//
// /* Moving Object direction arrow */
// s1.x = 0.0;
// s1.y = 0.0;
// s1.z = H - correction_wheel_height;
//
// s2.x = L/2.0;
// s2.y = 0.0;
// s2.z = H - correction_wheel_height;
//
// s3.x = (L/2.0) - 0.3;
// s3.y = 0.2;
// s3.z = H - correction_wheel_height;
//
// s4.x = (L/2.0) - 0.3;
// s4.y = -0.2;
// s4.z = H - correction_wheel_height;
//
// t = multiply_matrix_vector(rotate, s1);
// s1 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s2);
// s2 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s3);
// s3 = add_vectors(t, pos.position);
//
// t = multiply_matrix_vector(rotate, s4);
// s4 = add_vectors(t, pos.position);
//
// glBegin(GL_LINES);
// /* Part of arrow: | */
// glVertex3d(s1.x, s1.y, s1.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// /* Part of arrow: / */
// glVertex3d(s3.x, s3.y, s3.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// /* Part of arrow: \ */
// glVertex3d(s4.x, s4.y, s4.z);
// glVertex3d(s2.x, s2.y, s2.z);
//
// glEnd();
//
// //center of object
// glPushAttrib(GL_POINT_BIT);
// glPointSize(5);
// glBegin(GL_POINTS);
// glVertex3d(s1.x, s1.y, s1.z);
// glEnd();
// glPopAttrib();
//
// /* has to drawn after stuff above, so that it appears on top */
// // draw_number_associated(pos.position.x, pos.position.y, moving_objects_tracking[i].num_associated,
// // moving_objects_tracking[i].model_features.model_name);
//// draw_linear_velocity(pos.position.x, pos.position.y, moving_objects_tracking[i].particulas[j].velocity,
//// moving_objects_tracking[i].particulas[j].geometry.height);
//
// destroy_rotation_matrix(rotate);
// }
}
}
}
void
draw_ldmrs_objects(carmen_laser_ldmrs_object *ldmrs_objects_tracking, int num_ldmrs_objects, double min_velocity)
{
int i;
rotation_matrix *rotate = NULL;
for (i = 0; i < num_ldmrs_objects; i++)
{
if (ldmrs_objects_tracking[i].velocity < min_velocity)
continue;
carmen_pose_3D_t pos;
carmen_vector_3D_t p1, p2, p3 , p4, p5, p6, p7, p8, t;
carmen_vector_3D_t s1, s2, s3, s4; /* moving object direction arrow */
double correction_wheel_height = 0.28;
double W, L, H;
pos.position.x = ldmrs_objects_tracking[i].x;
pos.position.y = ldmrs_objects_tracking[i].y;
pos.position.z = 0.0;
pos.orientation.yaw = ldmrs_objects_tracking[i].orientation;
pos.orientation.roll = 0.0;
pos.orientation.pitch = 0.0;
W = ldmrs_objects_tracking[i].width;
L = ldmrs_objects_tracking[i].lenght;
H = 1.0;
// W = moving_objects_tracking[i].width;
// L = moving_objects_tracking[i].length;
// H = moving_objects_tracking[i].height;
rotate = compute_rotation_matrix(NULL, pos.orientation);
glColor3d(0.0,1.0,1.0);
p1.x = - L/2.0;
p1.y = - W/2.0;
p1.z = 0.0 - correction_wheel_height;
p2.x = L/2.0;
p2.y = - W/2.0;
p2.z = 0.0 - correction_wheel_height;
p3.x = L/2.0;
p3.y = W/2.0;
p3.z = 0.0 - correction_wheel_height;
p4.x = - L/2.0;
p4.y = W/2.0;
p4.z = 0.0 - correction_wheel_height;
p5.x = - L/2.0;
p5.y = - W/2.0;
p5.z = H - correction_wheel_height;
p6.x = L/2.0;
p6.y = - W/2.0;
p6.z = H - correction_wheel_height;
p7.x = L/2.0;
p7.y = W/2.0;
p7.z = H - correction_wheel_height;
p8.x = - L/2.0;
p8.y = W/2.0;
p8.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, p1);
p1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p2);
p2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p3);
p3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p4);
p4 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p5);
p5 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p6);
p6 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p7);
p7 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, p8);
p8 = add_vectors(t, pos.position);
glBegin (GL_LINES);
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p1.x, p1.y, p1.z);
//////////////////////////////
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p8.x, p8.y, p8.z);
glVertex3d (p5.x, p5.y, p5.z);
//////////////////////////////
glVertex3d (p1.x, p1.y, p1.z);
glVertex3d (p5.x, p5.y, p5.z);
glVertex3d (p2.x, p2.y, p2.z);
glVertex3d (p6.x, p6.y, p6.z);
glVertex3d (p3.x, p3.y, p3.z);
glVertex3d (p7.x, p7.y, p7.z);
glVertex3d (p4.x, p4.y, p4.z);
glVertex3d (p8.x, p8.y, p8.z);
glEnd ();
/* Moving Object direction arrow */
s1.x = 0.0;
s1.y = 0.0;
s1.z = H - correction_wheel_height;
s2.x = L/2.0;
s2.y = 0.0;
s2.z = H - correction_wheel_height;
s3.x = (L/2.0) - 0.3;
s3.y = 0.2;
s3.z = H - correction_wheel_height;
s4.x = (L/2.0) - 0.3;
s4.y = -0.2;
s4.z = H - correction_wheel_height;
t = multiply_matrix_vector(rotate, s1);
s1 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s2);
s2 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s3);
s3 = add_vectors(t, pos.position);
t = multiply_matrix_vector(rotate, s4);
s4 = add_vectors(t, pos.position);
glBegin(GL_LINES);
/* Part of arrow: | */
glVertex3d(s1.x, s1.y, s1.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: / */
glVertex3d(s3.x, s3.y, s3.z);
glVertex3d(s2.x, s2.y, s2.z);
/* Part of arrow: \ */
glVertex3d(s4.x, s4.y, s4.z);
glVertex3d(s2.x, s2.y, s2.z);
glEnd();
//center of object
glPushAttrib(GL_POINT_BIT);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(s1.x, s1.y, s1.z);
glEnd();
glPopAttrib();
// char class_name[50];
//
// switch (ldmrs_objects_tracking[i].classId)
// {
// case 0:
// strcpy(class_name,"Unclassified");
// break;
// case 1:
// strcpy(class_name,"Small");
// break;
// case 2:
// strcpy(class_name,"Big");
// break;
// case 3:
// strcpy(class_name,"Pedestrian");
// break;
// case 4:
// strcpy(class_name,"Bike");
// break;
// case 5:
// strcpy(class_name,"Car");
// break;
// case 6:
// strcpy(class_name,"Truck");
// break;
// default:
// strcpy(class_name,"Unknown");
// break;
// }
/* has to drawn after stuff above, so that it appears on top */
//draw_number_associated(pos.position.x, pos.position.y, ldmrs_objects_tracking[i].id,"");
draw_linear_velocity(pos.position.x, pos.position.y, ldmrs_objects_tracking[i].velocity,
1.0);
destroy_rotation_matrix(rotate);
}
}