Update CameraModel

chutsu · Sep 5, 2024 · 45a876f · 45a876f
1 parent 62cbb66
commit 45a876f
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 9 deletions.
diff --git a/lib/CameraModel.cpp b/lib/CameraModel.cpp
@@ -81,15 +81,6 @@ mat2_t radtan4_point_jacobian(const vec4_t &dist_params, const vec2_t &p) {
   J_point(1, 1) += y * (2.0 * k1 * y + 4.0 * k2 * y * r2);
   // Above is generated using sympy
 
-  // const auto radtan = k1 * r2 + k2 * r2 * r2;
-  // J_point(0, 0) = 1 + radtan + k1 * 2.0 * x2 + k2 * r2 * 4 * x2 +
-  //                 2.0 * p1 * p.y() + 6 * p2 * p.x();
-  // J_point(1, 0) = k1 * 2.0 * p.x() * p.y() + k2 * 4 * r2 * p.x() * p.y() +
-  //                 p1 * 2.0 * p.x() + 2.0 * p2 * p.y();
-  // J_point(0, 1) = J_point(1, 0);
-  // J_point(1, 1) = 1 + radtan + k1 * 2.0 * y2 + k2 * r2 * 4 * y2 +
-  //                 6 * p1 * p.y() + 2.0 * p2 * p.x();
-
   return J_point;
 }
 
@@ -311,12 +302,16 @@ mat_t<2, 4> pinhole_params_jacobian(const vec4_t &proj_params,
   return J;
 }
 
+/***************************** PINHOLE-RADTAN4 ********************************/
+
 PINHOLE_PROJECT(pinhole_radtan4_project, radtan4_distort);
 PINHOLE_PROJECT_J(pinhole_radtan4_project_jacobian, radtan4_point_jacobian);
 PINHOLE_PARAMS_J(pinhole_radtan4_params_jacobian, radtan4_params_jacobian);
 PINHOLE_BACK_PROJECT(pinhole_radtan4_back_project, radtan4_undistort);
 PINHOLE_UNDISTORT(pinhole_radtan4_undistort, radtan4_undistort);
 
+/****************************** PINHOLE-EQUI4 *********************************/
+
 PINHOLE_PROJECT(pinhole_equi4_project, equi4_distort);
 PINHOLE_PROJECT_J(pinhole_equi4_project_jacobian, equi4_point_jacobian);
 PINHOLE_PARAMS_J(pinhole_equi4_params_jacobian, equi4_params_jacobian);

diff --git a/lib/CameraModel.hpp b/lib/CameraModel.hpp
@@ -139,6 +139,8 @@ mat_t<2, 4> pinhole_params_jacobian(const vec4_t &proj_params, const vec2_t &p);
     return z_undist;                                                           \
   }
 
+/***************************** PINHOLE-RADTAN4 ********************************/
+
 int pinhole_radtan4_project(const int res[2],
                             const vecx_t &params,
                             const vec3_t &p_C,
@@ -151,6 +153,8 @@ int pinhole_radtan4_back_project(const vecx_t &params,
                                  vec3_t &ray);
 vec2_t pinhole_radtan4_undistort(const vecx_t &params, const vec2_t &z);
 
+/****************************** PINHOLE-EQUI4 *********************************/
+
 int pinhole_equi4_project(const int res[2],
                           const vecx_t &params,
                           const vec3_t &p_C,