fix PinholeFullCamera Undistortion

guanhaining · guanhaining · commit 51e87a36e040 · 2023-10-18T15:31:31.000+08:00
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 ) );
         }

Original file line number	Diff line number	Diff line change
`@@ -596,8 +596,8 @@ PinholeFullCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P`
`596`	`596`	`xd0 = x * cdist * icdist2 + p1 * a1 + p2 * a2;`
`597`	`597`	`yd0 = y * cdist * icdist2 + p1 * a3 + p2 * a1;`
`598`	`598`
`599`		`- double x_proj = xd * fx + cx;`
`600`		`- double y_proj = yd * fy + cy;`
	`599`	`+ double x_proj = xd0 * fx + cx;`
	`600`	`+ double y_proj = yd0 * fy + cy;`
`601`	`601`
`602`	`602`	`error = sqrt( pow( x_proj - u, 2 ) + pow( y_proj - v, 2 ) );`
`603`	`603`	`}`