Fixed apriltags z translation.

This commit is contained in:
Kwabena W. Agyeman 2017-03-21 21:01:52 -04:00
parent fb25fcee5e
commit c6009ae05d
4 changed files with 8 additions and 8 deletions

View File

@ -12633,7 +12633,7 @@ void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_
lnk_data.z_translation = MATD_EL(pose, 2, 3); lnk_data.z_translation = MATD_EL(pose, 2, 3);
lnk_data.x_rotation = fast_atan2f(MATD_EL(pose, 2, 1), MATD_EL(pose, 2, 2)); lnk_data.x_rotation = fast_atan2f(MATD_EL(pose, 2, 1), MATD_EL(pose, 2, 2));
lnk_data.y_rotation = fast_atan2f(-MATD_EL(pose, 2, 0), fast_sqrtf(sq(MATD_EL(pose, 2, 1)) + sq(MATD_EL(pose, 2, 2)))); lnk_data.y_rotation = fast_atan2f(-MATD_EL(pose, 2, 0), fast_sqrtf(sq(MATD_EL(pose, 2, 1)) + sq(MATD_EL(pose, 2, 2))));
lnk_data.z_rotation = fast_atan2f(MATD_EL(pose, 1, 0), MATD_EL(pose, 0, 0)); lnk_data.z_rotation = -fast_atan2f(MATD_EL(pose, 1, 0), MATD_EL(pose, 0, 0));
matd_destroy(pose); matd_destroy(pose);

View File

@ -2126,9 +2126,9 @@ static mp_obj_t py_image_find_apriltags(uint n_args, const mp_obj_t *args, mp_ma
apriltag_families_t families = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_families), TAG36H11); apriltag_families_t families = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_families), TAG36H11);
// 2.8mm Focal Length w/ OV7725 sensor for reference. // 2.8mm Focal Length w/ OV7725 sensor for reference.
float fx = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_fx), (2.8 / 3.984) * 656); float fx = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_fx), (2.8 / 3.984) * arg_img->w);
// 2.8mm Focal Length w/ OV7725 sensor for reference. // 2.8mm Focal Length w/ OV7725 sensor for reference.
float fy = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_fy), (2.8 / 2.952) * 488); float fy = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_fy), (2.8 / 2.952) * arg_img->h);
// Use the image versus the roi here since the image should be projected from the camera center. // Use the image versus the roi here since the image should be projected from the camera center.
float cx = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_cx), arg_img->w * 0.5); float cx = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_cx), arg_img->w * 0.5);
// Use the image versus the roi here since the image should be projected from the camera center. // Use the image versus the roi here since the image should be projected from the camera center.

View File

@ -26,18 +26,18 @@ clock = time.clock()
# translation the units are dimensionless and you must apply a conversion function. # translation the units are dimensionless and you must apply a conversion function.
# f_x is the x focal length of the camera. It should be equal to the lens focal length in mm # f_x is the x focal length of the camera. It should be equal to the lens focal length in mm
# divided by the x sensor size in mm times the number of pixels for the sensor length. # divided by the x sensor size in mm times the number of pixels in the image.
# The below values are for the OV7725 camera with a 2.8 mm lens. # The below values are for the OV7725 camera with a 2.8 mm lens.
# f_y is the y focal length of the camera. It should be equal to the lens focal length in mm # f_y is the y focal length of the camera. It should be equal to the lens focal length in mm
# divided by the y sensor size in mm times the number of pixels for the sensor length. # divided by the y sensor size in mm times the number of pixels in the image.
# The below values are for the OV7725 camera with a 2.8 mm lens. # The below values are for the OV7725 camera with a 2.8 mm lens.
# c_x is the image x center position in pixels. # c_x is the image x center position in pixels.
# c_y is the image y center position in pixels. # c_y is the image y center position in pixels.
f_x = (2.8 / 3.984) * 656 # find_apriltags defaults to this if not set f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
f_y = (2.8 / 2.952) * 488 # find_apriltags defaults to this if not set f_y = (2.8 / 2.952) * 120 # find_apriltags defaults to this if not set
c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5) c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5) c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)

View File

@ -51,7 +51,7 @@ h_fov = 2 * math.atan((sensor_w_mm / 2) / lens_mm)
v_fov = 2 * math.atan((sensor_h_mm / 2) / lens_mm) v_fov = 2 * math.atan((sensor_h_mm / 2) / lens_mm)
def z_to_mm(z_translation, tag_size): # z_translation is in decimeters... def z_to_mm(z_translation, tag_size): # z_translation is in decimeters...
return ((((-z_translation) * 100) * tag_size) / 165) - lens_to_camera_mm return (((z_translation * 100) * tag_size) / 165) - lens_to_camera_mm
# Link Setup # Link Setup