void lapbe_colorizer(float *outhue, float *outint,
float *metric, float *color, int w, int h, int pd)
{
float tstep = TSTEP();
int niter = NITER();
int nscales = NSCALES();
float *tmpi = xmalloc(w*h*sizeof*tmpi);
float *tmpo = xmalloc(w*h*sizeof*tmpo);
for (int l = 0; l < pd; l++)
{
for (int i = 0; i < w*h; i++)
tmpi[i] = grayP(color + i*pd, pd) ? NAN : color[i*pd+l];
lapbediag_rec(tmpo, metric, tmpi, w, h, tstep, niter, nscales);
for (int i = 0; i < w*h; i++)
outhue[i*pd+l] = tmpo[i];
}
free(tmpo);
free(tmpi);
for (int i = 0; i < w*h; i++)
normalize_intensity(outint+i*pd, outhue+i*pd, color+i*pd, pd);
}
void
velodyne_intensity_drawer_add_velodyne_message(velodyne_intensity_drawer* v_drawer, carmen_velodyne_partial_scan_message* velodyne_message, carmen_pose_3D_t car_fused_pose, carmen_vector_3D_t car_fused_velocity, double car_fused_time)
{
static double vertical_correction[32] = { -30.67, -9.3299999, -29.33, -8.0, -28.0, -6.6700001, -26.67, -5.3299999, -25.33, -4.0,
-24.0, -2.6700001, -22.67, -1.33, -21.33, 0.0, -20.0, 1.33, -18.67, 2.6700001, -17.33, 4.0, -16.0, 5.3299999, -14.67, 6.6700001,
-13.33, 8.0, -12.0, 9.3299999, -10.67, 10.67 };
static point_cloud velodyne_points = {NULL, NULL, {0.0, 0.0, 0.0}, 0, 0.0};
static double last_timestamp = 0.0;
if (last_timestamp == 0.0)
{
last_timestamp = velodyne_message->timestamp;
return;
}
int num_points = velodyne_message->number_of_32_laser_shots*(32);
if(num_points > velodyne_points.num_points)
{
velodyne_points.points = realloc(velodyne_points.points, num_points*sizeof(carmen_vector_3D_t));
velodyne_points.point_color = realloc(velodyne_points.point_color, num_points*sizeof(carmen_vector_3D_t));
}
velodyne_points.num_points = num_points;
velodyne_points.car_position = car_fused_pose.position;
velodyne_points.timestamp = velodyne_message->timestamp;
rotation_matrix* velodyne_to_board_matrix = create_rotation_matrix(v_drawer->velodyne_pose.orientation);
rotation_matrix* board_to_car_matrix = create_rotation_matrix(v_drawer->sensor_board_pose.orientation);
rotation_matrix* r_matrix_car_to_global = create_rotation_matrix(car_fused_pose.orientation);
int k = 0;
int i;
for(i = 0; i < velodyne_message->number_of_32_laser_shots; i++)
{
double shot_angle = -carmen_degrees_to_radians(velodyne_message->partial_scan[i].angle);
if(check_angle_in_range(shot_angle, v_drawer->horizontal_angle, v_drawer->horizontal_range))
{
int j;
for(j = 0; j < 32; j++)
{
if( j == v_drawer->vertical_position )
{
carmen_vector_3D_t point_position = get_velodyne_point_car_reference( shot_angle,
carmen_degrees_to_radians(vertical_correction[j]),
velodyne_message->partial_scan[i].distance[j]/500.0,
velodyne_to_board_matrix, board_to_car_matrix,
v_drawer->velodyne_pose, v_drawer->sensor_board_pose);
double shot_time = last_timestamp + ((velodyne_message->timestamp - last_timestamp)*((double)i)/((double)velodyne_message->number_of_32_laser_shots));
carmen_vector_3D_t car_interpolated_position = carmen_get_interpolated_robot_position_at_time(car_fused_pose, car_fused_velocity, car_fused_time, shot_time, r_matrix_car_to_global);
carmen_vector_3D_t point_global_position = get_point_position_global_reference(car_interpolated_position, point_position, r_matrix_car_to_global);
velodyne_points.points[k] = point_global_position;
velodyne_points.point_color[k] = create_point_colors_intensity(velodyne_message->partial_scan[i].intensity[j]);
k++;
}
}
}
}
velodyne_points.num_points = k;
normalize_intensity(velodyne_points);
destroy_rotation_matrix(velodyne_to_board_matrix);
destroy_rotation_matrix(board_to_car_matrix);
destroy_rotation_matrix(r_matrix_car_to_global);
add_point_cloud(v_drawer->pcloud_drawer, velodyne_points);
last_timestamp = velodyne_message->timestamp;
}
