From 772e17f4d6adad8949c6c2406c53bb18605483ed Mon Sep 17 00:00:00 2001 From: Matt Boler Date: Wed, 25 Aug 2021 13:43:28 -0700 Subject: [PATCH] Adjust optical axis used in laser range finder measurement to use camera parameters --- src/x/vio/vio.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/x/vio/vio.cpp b/src/x/vio/vio.cpp index 1f18d1b..2d8ade1 100644 --- a/src/x/vio/vio.cpp +++ b/src/x/vio/vio.cpp @@ -175,8 +175,8 @@ State VIO::processMatchesMeasurement(double timestamp, // Compute 2D image coordinates of the LRF impact point on the ground x::Feature lrf_img_pt; - lrf_img_pt.setXDist(320.5); - lrf_img_pt.setYDist(240.5); + lrf_img_pt.setXDist((camera_.getWidth()+1) / 2); + lrf_img_pt.setYDist((camera_.getHeight()+1) / 2); camera_.undistort(lrf_img_pt); last_range_measurement_.img_pt = lrf_img_pt; last_range_measurement_.img_pt_n = camera_.normalize(lrf_img_pt);