From c6009ae05db1f5f6078016b7680fb17fae8694ec Mon Sep 17 00:00:00 2001 From: "Kwabena W. Agyeman" Date: Tue, 21 Mar 2017 21:01:52 -0400 Subject: [PATCH] Fixed apriltags z translation. --- src/omv/img/apriltag.c | 2 +- src/omv/py/py_image.c | 4 ++-- usr/examples/16-Codes/find_apriltags_3d_pose.py | 8 ++++---- .../18-MAVLink/mavlink_apriltags_landing_target.py | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/omv/img/apriltag.c b/src/omv/img/apriltag.c index effd4ab40..64b2e3ead 100644 --- a/src/omv/img/apriltag.c +++ b/src/omv/img/apriltag.c @@ -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); diff --git a/src/omv/py/py_image.c b/src/omv/py/py_image.c index d4bddbcff..03f2f1086 100644 --- a/src/omv/py/py_image.c +++ b/src/omv/py/py_image.c @@ -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. diff --git a/usr/examples/16-Codes/find_apriltags_3d_pose.py b/usr/examples/16-Codes/find_apriltags_3d_pose.py index 71f141dba..8879f53f5 100644 --- a/usr/examples/16-Codes/find_apriltags_3d_pose.py +++ b/usr/examples/16-Codes/find_apriltags_3d_pose.py @@ -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) diff --git a/usr/examples/18-MAVLink/mavlink_apriltags_landing_target.py b/usr/examples/18-MAVLink/mavlink_apriltags_landing_target.py index f78475d74..6e8aa0e2b 100644 --- a/usr/examples/18-MAVLink/mavlink_apriltags_landing_target.py +++ b/usr/examples/18-MAVLink/mavlink_apriltags_landing_target.py @@ -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