void
bot_roll_pitch_yaw_to_angle_axis (const double rpy[3], double *theta,
double axis[3])
{
double q[4];
bot_roll_pitch_yaw_to_quat(rpy, q);
bot_quat_to_angle_axis (q, theta, axis);
}
int
bot_quaternion_test()
{
#define FAIL_TEST { fprintf(stderr, "bot_quaternion_test failed at line %d\n", \
__LINE__); return 0; }
fprintf(stderr, "running quaternion test\n");
double theta = 0;
double rvec[] = { 0, 0, 1 };
double q[4];
double rpy[3];
double roll, pitch, yaw;
bot_angle_axis_to_quat(theta, rvec, q);
if (! qeq (q, 1, 0, 0, 0)) FAIL_TEST;
bot_quat_to_roll_pitch_yaw (q, rpy);
roll = rpy[0]; pitch = rpy[1]; yaw = rpy[2];
if (! rpyeq (roll,pitch,yaw, 0,0,0)) FAIL_TEST;
// quat_from_angle_axis
theta = M_PI;
bot_angle_axis_to_quat(theta, rvec, q);
fprintf(stderr,"<%.3f, %.3f, %.3f, %.3f>\n", q[0], q[1], q[2], q[3]);
if (! qeq (q, 0, 0, 0, 1)) FAIL_TEST;
// bot_quat_to_angle_axis
bot_quat_to_angle_axis (q, &theta, rvec);
if (!feq (theta, M_PI)) FAIL_TEST;
if (!feq(rvec[0], 0) || !feq(rvec[1], 0) || !feq(rvec[2], 1)) FAIL_TEST;
bot_quat_to_roll_pitch_yaw (q, rpy);
if (! rpyeq (roll,pitch,yaw, 0,0,M_PI)) FAIL_TEST;
double q2[4];
double q3[4];
double axis1[] = { 0, 1, 0 };
double axis2[] = { 0, 0, 1 };
bot_angle_axis_to_quat (M_PI/2, axis1, q);
bot_angle_axis_to_quat (M_PI/2, axis2, q);
bot_quat_mult (q3, q, q2);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
bot_quat_rotate (q, rvec);
fprintf(stderr, "by q: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
bot_quat_rotate (q2, rvec);
fprintf(stderr, "by q2: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
bot_quat_rotate (q3, rvec);
fprintf(stderr, "by q*q2: [ %.2f, %.2f, %.2f ]\n",
rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
bot_quat_mult (q3, q2, q);
bot_quat_rotate (q3, rvec);
fprintf(stderr, "by q2*q: [ %.2f, %.2f, %.2f ]\n",
rvec[0], rvec[1], rvec[2]);
// TODO
#undef FAIL_TEST
fprintf(stderr, "bot_quaternion_test complete\n");
return 1;
}
static void on_laser(const lcm_recv_buf_t *rbuf, const char *channel, const bot_core_planar_lidar_t *msg, void *user)
{
RendererLaser *self = (RendererLaser*) user;
g_assert(self);
/* get the laser channel object based on channel name */
laser_channel *lchan = g_hash_table_lookup(self->channels_hash, channel);
if (lchan == NULL) {
/* allocate and initialize laser channel structure */
lchan = (laser_channel*) calloc(1, sizeof(laser_channel));
g_assert(lchan);
lchan->enabled = 1;
lchan->name = bot_param_get_planar_lidar_name_from_lcm_channel(self->bot_param, channel);
lchan->channel = strdup(channel);
lchan->projector = laser_projector_new(self->bot_param, self->bot_frames, lchan->name, 1);
char param_prefix[1024];
bot_param_get_planar_lidar_prefix(NULL, lchan->name, param_prefix, sizeof(param_prefix));
char color_key[1024];
sprintf(color_key, "%s.viewer_color", param_prefix);
double color[3];
int color_size = bot_param_get_double_array(self->bot_param, color_key, color, 3);
if (color_size != 3) {
ERR("Error: Missing or funny color for planar LIDAR "
"configuration key: '%s'\n", color_key);
lchan->color[0] = 1;
lchan->color[1] = 1;
lchan->color[2] = 1;
}
else {
lchan->color[0] = color[0];
lchan->color[1] = color[1];
lchan->color[2] = color[2];
}
lchan->scans = bot_ptr_circular_new(self->param_max_buffer_size, laser_scan_destroy, NULL);
g_assert(lchan->scans);
/* add laser channel to hash table and array */
g_hash_table_insert(self->channels_hash, lchan->channel, lchan);
g_ptr_array_add(self->channels, lchan);
/* add check box */
if (self->viewer)
bot_gtk_param_widget_add_booleans(self->pw, 0, lchan->name, lchan->enabled, NULL);
}
/* TODO: Optimization - allocate space for local points from a
circular buffer instead of calling calloc for each scan */
laser_projected_scan *lscan = laser_create_projected_scan_from_planar_lidar(lchan->projector, msg,
bot_frames_get_root_name(self->bot_frames));
if (lscan == NULL)
return; //probably didn't have a pose message yet...
if(lchan->scans->capacity != self->param_max_buffer_size){
bot_ptr_circular_resize(lchan->scans, self->param_max_buffer_size);
}
if (bot_ptr_circular_size(lchan->scans) > 0) {
laser_projected_scan *last_scan = bot_ptr_circular_index(lchan->scans, 0);
/* check for a large time difference between scans (typical when
jumping around an LCM log file) */
gboolean time_jump = FALSE;
int64_t dt = msg->utime - last_scan->utime;
if (dt < -OLD_HISTORY_THRESHOLD || dt > OLD_HISTORY_THRESHOLD)
time_jump = TRUE;
/* spacial decimation */
gboolean stationary = FALSE;
BotTrans delta;
if (self->param_spacial_decimate && self->param_scan_memory > 10) {
bot_trans_invert_and_compose(&lscan->origin, &last_scan->origin, &delta);
double dist = bot_vector_magnitude_3d(delta.trans_vec);
double rot;
double axis[3];
bot_quat_to_angle_axis(delta.rot_quat, &rot, axis);
if (dist < SPACIAL_DECIMATION_LIMIT && rot < ANGULAR_DECIMATION_LIMIT) {
stationary = TRUE;
}
}
if (stationary) {
laser_scan_destroy(NULL, lscan);
return;
}
}
bot_ptr_circular_add(lchan->scans, lscan);
if (self->viewer)
bot_viewer_request_redraw(self->viewer);
return;
}
