int
atrans_transform_vec(ATrans *atrans, const char *from_frame,
const char *to_frame, const double src[3], double dst[3])
{
BotTrans rbtrans;
if(!atrans_get_trans(atrans, from_frame, to_frame, &rbtrans))
return 0;
bot_trans_apply_vec(&rbtrans, src, dst);
return 1;
}
void GlobalToCameraFrame(double point_in[], double point_out[]) {
// figure out what this point (which is currently expressed in global coordinates
// will be in local opencv coordinates
BotTrans global_to_camera_trans;
// we must update this every time because the transforms could change as the aircraft
// moves
bot_frames_get_trans(bot_frames_, "local", "opencvFrame", &global_to_camera_trans);
bot_trans_apply_vec(&global_to_camera_trans, point_in, point_out);
}
void CameraToGlobalFrame(double point_in[], double point_out[]) {
BotTrans camera_to_global_trans;
bot_frames_get_trans(bot_frames_, "opencvFrame", "local", &camera_to_global_trans);
bot_trans_apply_vec(&camera_to_global_trans, point_in, point_out);
}
void CameraToGlobalFrame(double point_in[], double point_out[]) {
bot_trans_apply_vec(&camera_to_global_trans_, point_in, point_out);
}
void GlobalToCameraFrame(double point_in[], double point_out[]) {
// figure out what this point (which is currently expressed in global coordinates
// will be in local opencv coordinates
bot_trans_apply_vec(&global_to_camera_trans_, point_in, point_out);
}
