Merge pull request #119 from qsisi/master

fix PinholeFullCamera Undistortion
PJLab-ADG · Oct 18, 2023 · 826f6fc · 826f6fc
2 parents 20d0d76 + 51e87a3
commit 826f6fc
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/online_calib/vins_camera2imu/src/camodocal/camera_models/PinholeFullCamera.cc b/online_calib/vins_camera2imu/src/camodocal/camera_models/PinholeFullCamera.cc
@@ -596,8 +596,8 @@ PinholeFullCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P
             xd0     = x * cdist * icdist2 + p1 * a1 + p2 * a2;
             yd0     = y * cdist * icdist2 + p1 * a3 + p2 * a1;
 
-            double x_proj = xd * fx + cx;
-            double y_proj = yd * fy + cy;
+            double x_proj = xd0 * fx + cx;
+            double y_proj = yd0 * fy + cy;
 
             error = sqrt( pow( x_proj - u, 2 ) + pow( y_proj - v, 2 ) );
         }