44 sprintf(key,
"calibration.%s.orientation", name);
51 sprintf(key,
"calibration.%s.rpy", name);
57 for (i = 0; i < 3; i++)
63 sprintf(key,
"calibration.%s.angleaxis", name);
70 double s = sin(theta/2);
72 quat[0] = cos(theta/2);
74 for (i = 1; i < 4; i++)
91 sprintf(key,
"calibration.%s.position", name);
120 const char *name,
double m[16],
123 double body_to_local[16];
127 double sensor_to_calibration[16];
133 sprintf(key,
"calibration.%s.relative_to", name);
136 if (!strcmp(calib_frame,
"body")) {
141 double calibration_to_body[16];
146 double sensor_to_body[16];
convenience functions for small linear algebra operations
int config_util_get_matrix(Config *cfg, const char *name, double m[16])
int config_util_get_pos(Config *cfg, const char *name, double pos[3])
char * config_get_str_or_fail(Config *conf, const char *key)
int config_has_key(Config *conf, const char *key)
static int _config_util_get_quat(Config *cfg, const char *name, double quat[4])
int rot_quat_pos_to_matrix(const double quat[4], const double pose[3], double m[16])
int config_util_get_quat(Config *cfg, const char *name, double quat[4])
header file for rotations.cpp Transformation utilities
int config_get_double_array(Config *conf, const char *key, double *vals, int len)
header file for config_util.cpp Check the LCM instructions for more info
void rot_roll_pitch_yaw_to_quat(const double rpy[3], double q[4])
int config_util_sensor_to_local_with_pose(Config *config, const char *name, double m[16], lcmtypes_pose_t *p)
static void matrix_multiply_4x4_4x4(const double a[16], const double b[16], double r[16])
header file for math utilities