/* ================ general ============== */
int bot_param_get_quat(BotParam *param, const char *name, double quat[4])
{
char key[2048];
sprintf(key, "%s.quat", name);
if (bot_param_has_key(param, key)) {
int sz = bot_param_get_double_array(param, key, quat, 4);
assert(sz == 4);
return 0;
}
sprintf(key, "%s.rpy", name);
if (bot_param_has_key(param, key)) {
double rpy[3];
int sz = bot_param_get_double_array(param, key, rpy, 3);
assert(sz == 3);
for (int i = 0; i < 3; i++)
rpy[i] = bot_to_radians (rpy[i]);
bot_roll_pitch_yaw_to_quat(rpy, quat);
return 0;
}
sprintf(key, "%s.rodrigues", name);
if (bot_param_has_key(param, key)) {
double rod[3];
int sz = bot_param_get_double_array(param, key, rod, 3);
assert(sz == 3);
bot_rodrigues_to_quat(rod, quat);
return 0;
}
sprintf(key, "%s.angleaxis", name);
if (bot_param_has_key(param, key)) {
double aa[4];
int sz = bot_param_get_double_array(param, key, aa, 4);
assert(sz == 4);
bot_angle_axis_to_quat(aa[0], aa + 1, quat);
return 0;
}
return -1;
}
/**
* get the measurement covariance at state
*/
void MapMeasurementFunction::getMeasurementCov(const RBIS & state, Eigen::Matrix3d & R_effective)
{
double xy[2] = { state.position()(0), state.position()(1) };
Eigen::Vector3d eulers = state.getEulerAngles();
double phipsi[2] = { eulers(0), eulers(2) };
R_effective = phi_psi_xy_cov_map->readValue(phipsi)->readValue(xy);
double theta_state = eulers(1);
double z_state = state.position()(2);
if (fabs(z_state - z_height) > 2.0) {
fprintf(stderr, "warning, querying measurement at different height than measurements were simulated, %f, %f\n",
z_state, z_height);
}
if (fabs(theta_state - theta) > bot_to_radians(20)) {
fprintf(stderr,
"warning, querying measurement at different theta (pitch) than measurements were simulated, %f, %f\n",
bot_to_degrees(theta_state), bot_to_degrees(theta));
}
}
static void on_param_widget_changed(BotGtkParamWidget *pw, const char *name, void *user)
{
RendererFrames *self = (RendererFrames*) user;
if (self->updating) {
return;
}
BotViewer *viewer = self->viewer;
int activeSensorNum = bot_gtk_param_widget_get_enum(pw, PARAM_FRAME_SELECT);
if (!strcmp(name, PARAM_FRAME_SELECT)) {
if (activeSensorNum > 0) {
self->updating = 1;
bot_viewer_set_status_bar_message(self->viewer, "Modify Calibration relative to %s", bot_frames_get_relative_to(
self->frames, self->frameNames[activeSensorNum]));
bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 1);
frames_update_handler(self->frames, self->frameNames[activeSensorNum], bot_frames_get_relative_to(self->frames,
self->frameNames[activeSensorNum]), bot_timestamp_now(), self);
}
else {
bot_viewer_set_status_bar_message(self->viewer, "");
bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 0);
}
}
else if (!strcmp(name, PARAM_SAVE) && activeSensorNum > 0) {
char save_fname[1024];
sprintf(save_fname, "manual_calib_%s.cfg", self->frameNames[activeSensorNum]);
fprintf(stderr, "saving params to: %s\n", save_fname);
FILE * f = fopen(save_fname, "w");
double pos[3];
pos[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X);
pos[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y);
pos[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z);
double rpy[3];
rpy[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL);
rpy[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH);
rpy[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_YAW);
double rpy_rad[3];
for (int i = 0; i < 3; i++)
rpy_rad[i] = bot_to_radians(rpy[i]);
double quat[4];
bot_roll_pitch_yaw_to_quat(rpy_rad, quat);
double rod[3];
bot_quat_to_rodrigues(quat, rod);
fprintf(f, ""
"%s {\n"
"position = [%f, %f, %f];\n"
"rpy = [%f, %f, %f];\n"
"relative_to = \"%s\";\n"
"}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rpy[0], rpy[1], rpy[2],
bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]));
fprintf(f, ""
"\n"
"%s {\n"
"position = [%f, %f, %f];\n"
"rodrigues = [%f, %f, %f];\n"
"relative_to = \"%s\";\n"
"}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rod[0], rod[1], rod[2],
bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]));
fclose(f);
bot_viewer_set_status_bar_message(self->viewer, "Calibration saved to %s", save_fname);
}
else if (activeSensorNum > 0) {
self->updating = 1;
BotTrans curr;
curr.trans_vec[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X);
curr.trans_vec[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y);
curr.trans_vec[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z);
double rpy[3];
rpy[0] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL));
rpy[1] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH));
rpy[2] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_YAW));
bot_roll_pitch_yaw_to_quat(rpy, curr.rot_quat);
if (fabs(rpy[0]) > M_PI || fabs(rpy[1]) > M_PI || fabs(rpy[2]) > M_PI) {
bot_gtk_param_widget_set_double(self->pw, PARAM_ROLL, bot_to_degrees(bot_mod2pi(rpy[0])));
bot_gtk_param_widget_set_double(self->pw, PARAM_PITCH, bot_to_degrees(bot_mod2pi(rpy[1])));
bot_gtk_param_widget_set_double(self->pw, PARAM_YAW, bot_to_degrees(bot_mod2pi(rpy[2])));
}
//and update the link
const char * frame_name = self->frameNames[activeSensorNum];
const char * relative_to = bot_frames_get_relative_to(self->frames, frame_name);
bot_frames_update_frame(self->frames, frame_name, relative_to, &curr, bot_timestamp_now());
}
bot_viewer_request_redraw(self->viewer);
}