From 9f4d3e9b9838a17934b32706b0e87ee4999de938 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 6 Jul 2021 12:14:11 -0400 Subject: [PATCH] Fix radtan norm intinsic jacob (fixes #169) --- ov_core/src/cam/CamRadtan.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ov_core/src/cam/CamRadtan.h b/ov_core/src/cam/CamRadtan.h index b312bc69e..0ee16d101 100644 --- a/ov_core/src/cam/CamRadtan.h +++ b/ov_core/src/cam/CamRadtan.h @@ -164,12 +164,12 @@ class CamRadtan : public CamBase { double x_2 = uv_norm(0) * uv_norm(0); double y_2 = uv_norm(1) * uv_norm(1); double x_y = uv_norm(0) * uv_norm(1); - H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r) + 2 * cam_d(6) * y + - (2 * cam_d(7) * x + 4 * cam_d(7) * x)); - H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r + 2 * cam_d(6) * x + 2 * cam_d(7) * y); - H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r + 2 * cam_d(6) * x + 2 * cam_d(7) * y); - H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r) + 2 * cam_d(7) * x + - (2 * cam_d(6) * y + 4 * cam_d(6) * y)); + H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) + + 2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x)); + H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y); + H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y); + H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) + + 2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y)); // Calculate distorted coordinates for radtan double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +