示例#1
0
/* ================ 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);
}