Compute the image undistort map only once
Reusing the map is almost 10 times faster than recomputing it each
iteration.
Also cleaned up some reused code between camera reader and viewer.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I2c1e6b6b5533054e356eca077778f01f9e8456a1
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index 7128890..8317c09 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -29,12 +29,73 @@
// TODO<jim>: Probably need to break out LED control to separate process
class CameraReader {
public:
+ static const calibration::CameraCalibration *FindCameraCalibration(
+ const calibration::CalibrationData *calibration_data,
+ std::string_view node_name, int team_number);
+
+ static cv::Mat CameraIntrinsics(
+ const calibration::CameraCalibration *camera_calibration) {
+ cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->intrinsics()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+ return result;
+ }
+
+ static cv::Mat CameraExtrinsics(
+ const calibration::CameraCalibration *camera_calibration) {
+ // TODO(james): What's the principled way to handle non-z-axis turrets?
+ const frc971::vision::calibration::TransformationMatrix *transform =
+ camera_calibration->has_turret_extrinsics()
+ ? camera_calibration->turret_extrinsics()
+ : camera_calibration->fixed_extrinsics();
+
+ cv::Mat result(4, 4, CV_32F,
+ const_cast<void *>(
+ static_cast<const void *>(transform->data()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), transform->data()->size());
+ return result;
+ }
+
+ static cv::Mat CameraDistCoeffs(
+ const calibration::CameraCalibration *camera_calibration) {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
+ return result;
+ }
+
+ static std::pair<cv::Mat, cv::Mat> ComputeUndistortMaps(
+ const cv::Mat intrinsics, const cv::Mat dist_coeffs) {
+ std::pair<cv::Mat, cv::Mat> undistort_maps;
+ static const cv::Size kImageSize = {640, 480};
+ cv::initUndistortRectifyMap(intrinsics, dist_coeffs, cv::Mat(), intrinsics,
+ kImageSize, CV_16SC2, undistort_maps.first,
+ undistort_maps.second);
+ return undistort_maps;
+ }
+
+ static cv::Mat UndistortImage(cv::Mat image_distorted,
+ std::pair<cv::Mat, cv::Mat> undistort_maps) {
+ cv::Mat image;
+ cv::remap(image_distorted, image, undistort_maps.first,
+ undistort_maps.second, cv::INTER_LINEAR);
+ return image;
+ }
+
CameraReader(aos::ShmEventLoop *event_loop,
const calibration::CalibrationData *calibration_data,
V4L2Reader *reader)
: event_loop_(event_loop),
calibration_data_(calibration_data),
- camera_calibration_(FindCameraCalibration()),
+ camera_calibration_(FindCameraCalibration(
+ calibration_data_, event_loop_->node()->name()->string_view(),
+ aos::network::GetTeamNumber())),
+ undistort_maps_(
+ ComputeUndistortMaps(CameraIntrinsics(), CameraDistCoeffs())),
reader_(reader),
image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
target_estimator_(CameraIntrinsics(), CameraExtrinsics()),
@@ -60,8 +121,6 @@
double GetDutyCycle() { return duty_cycle_; }
private:
- const calibration::CameraCalibration *FindCameraCalibration() const;
-
// Processes an image (including sending the results).
void ProcessImage(cv::Mat image_mat_distorted,
int64_t image_monotonic_timestamp_ns);
@@ -70,40 +129,21 @@
void ReadImage();
cv::Mat CameraIntrinsics() const {
- cv::Mat result(3, 3, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration_->intrinsics()->data())));
- result.convertTo(result, CV_64F);
- CHECK_EQ(result.total(), camera_calibration_->intrinsics()->size());
- return result;
+ return CameraIntrinsics(camera_calibration_);
}
cv::Mat CameraExtrinsics() const {
- // TODO(james): What's the principled way to handle non-z-axis turrets?
- const frc971::vision::calibration::TransformationMatrix *transform =
- camera_calibration_->has_turret_extrinsics()
- ? camera_calibration_->turret_extrinsics()
- : camera_calibration_->fixed_extrinsics();
-
- cv::Mat result(4, 4, CV_32F,
- const_cast<void *>(
- static_cast<const void *>(transform->data()->data())));
- result.convertTo(result, CV_64F);
- CHECK_EQ(result.total(), transform->data()->size());
- return result;
+ return CameraExtrinsics(camera_calibration_);
}
cv::Mat CameraDistCoeffs() const {
- const cv::Mat result(5, 1, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration_->dist_coeffs()->data())));
- CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
- return result;
+ return CameraDistCoeffs(camera_calibration_);
}
aos::ShmEventLoop *const event_loop_;
const calibration::CalibrationData *const calibration_data_;
const calibration::CameraCalibration *const camera_calibration_;
+ std::pair<cv::Mat, cv::Mat> undistort_maps_;
V4L2Reader *const reader_;
aos::Sender<CameraImage> image_sender_;
TargetEstimator target_estimator_;