mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Merge pull request #200 from kwagyeman/master
Fixed apriltags z translation.
This commit is contained in:
commit
2a3e7efcef
@ -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.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.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);
|
||||
|
||||
|
||||
@ -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);
|
||||
// 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.
|
||||
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.
|
||||
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.
|
||||
|
||||
@ -26,18 +26,18 @@ clock = time.clock()
|
||||
# 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
|
||||
# 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.
|
||||
|
||||
# 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.
|
||||
|
||||
# c_x is the image x 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_y = (2.8 / 2.952) * 488 # 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) * 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_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
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
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user