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/BUILD b/y2022/vision/BUILD
index a26c507..30ab84a 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -250,6 +250,7 @@
deps = [
":blob_detector_lib",
":calibration_data",
+ ":camera_reader_lib",
":target_estimator_lib",
"//aos:init",
"//aos/events:shm_event_loop",
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index 92d3727..0af4afc 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -21,12 +21,11 @@
using namespace frc971::vision;
-const calibration::CameraCalibration *CameraReader::FindCameraCalibration()
- const {
- const std::string_view node_name = event_loop_->node()->name()->string_view();
- const int team_number = aos::network::GetTeamNumber();
+const calibration::CameraCalibration *CameraReader::FindCameraCalibration(
+ const calibration::CalibrationData *calibration_data,
+ std::string_view node_name, int team_number) {
for (const calibration::CameraCalibration *candidate :
- *calibration_data_->camera_calibrations()) {
+ *calibration_data->camera_calibrations()) {
if (candidate->node_name()->string_view() != node_name) {
continue;
}
@@ -92,9 +91,7 @@
void CameraReader::ProcessImage(cv::Mat image_mat_distorted,
int64_t image_monotonic_timestamp_ns) {
- cv::Mat image_mat;
- cv::undistort(image_mat_distorted, image_mat, CameraIntrinsics(),
- CameraDistCoeffs());
+ cv::Mat image_mat = UndistortImage(image_mat_distorted, undistort_maps_);
BlobDetector::BlobResult blob_result;
BlobDetector::ExtractBlobs(image_mat, &blob_result);
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_;
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index 26d9700..f76e0b9 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -378,16 +378,16 @@
[](const std::pair<size_t, size_t> &a,
const std::pair<size_t, size_t> &b) { return a.first < b.first; });
- size_t middle_tape_index = 1000;
+ std::optional<size_t> middle_tape_index = std::nullopt;
for (size_t i = 0; i < tape_indices.size(); ++i) {
if (tape_indices[i].second == middle_blob_index_) {
middle_tape_index = i;
}
}
- CHECK_NE(middle_tape_index, 1000) << "Failed to find middle tape";
+ CHECK(middle_tape_index.has_value()) << "Failed to find middle tape";
if (VLOG_IS_ON(2)) {
- LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
<< middle_blob_index_;
for (size_t i = 0; i < tape_indices.size(); ++i) {
const auto distance = DistanceFromTapeIndex(
@@ -400,7 +400,7 @@
{
size_t offset = 0;
- for (size_t i = middle_tape_index + 1; i < tape_indices.size(); ++i) {
+ for (size_t i = *middle_tape_index + 1; i < tape_indices.size(); ++i) {
tape_indices[i].first -= offset;
if (tape_indices[i].first > tape_indices[i - 1].first + 1) {
@@ -412,7 +412,7 @@
}
if (VLOG_IS_ON(2)) {
- LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
<< middle_blob_index_;
for (size_t i = 0; i < tape_indices.size(); ++i) {
const auto distance = DistanceFromTapeIndex(
@@ -425,7 +425,7 @@
{
size_t offset = 0;
- for (size_t i = middle_tape_index; i > 0; --i) {
+ for (size_t i = *middle_tape_index; i > 0; --i) {
tape_indices[i - 1].first -= offset;
if (tape_indices[i - 1].first + 1 < tape_indices[i].first) {
@@ -440,7 +440,7 @@
}
if (VLOG_IS_ON(2)) {
- LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
<< middle_blob_index_;
for (size_t i = 0; i < tape_indices.size(); ++i) {
const auto distance = DistanceFromTapeIndex(
@@ -566,11 +566,11 @@
size_t blob_index, const std::vector<cv::Point_<S>> &tape_points) const {
auto distance = cv::Point_<S>(std::numeric_limits<S>::infinity(),
std::numeric_limits<S>::infinity());
- size_t final_match = 255;
+ std::optional<size_t> final_match = std::nullopt;
if (blob_index == middle_blob_index_) {
// Fix the middle blob so the solver can't go too far off
final_match = tape_points.size() / 2;
- distance = DistanceFromTapeIndex(blob_index, final_match, tape_points);
+ distance = DistanceFromTapeIndex(blob_index, *final_match, tape_points);
} else {
// Give the other blob_stats some freedom in case some are split into pieces
for (auto it = tape_points.begin(); it < tape_points.end(); it++) {
@@ -585,11 +585,11 @@
}
}
- VLOG(2) << "Matched index " << blob_index << " to " << final_match
+ CHECK(final_match.has_value());
+ VLOG(2) << "Matched index " << blob_index << " to " << *final_match
<< " distance " << distance.x << " " << distance.y;
- CHECK_NE(final_match, 255);
- return final_match;
+ return *final_match;
}
void TargetEstimator::DrawProjectedHub(
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 446f1f6..e455f66 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -13,6 +13,7 @@
#include "frc971/vision/vision_generated.h"
#include "y2022/vision/blob_detector.h"
#include "y2022/vision/calibration_data.h"
+#include "y2022/vision/camera_reader.h"
#include "y2022/vision/target_estimate_generated.h"
#include "y2022/vision/target_estimator.h"
@@ -213,51 +214,24 @@
const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
CalibrationData());
- const calibration::CameraCalibration *calibration = nullptr;
- for (const calibration::CameraCalibration *candidate :
- *calibration_data.message().camera_calibrations()) {
- if ((candidate->node_name()->string_view() == FLAGS_calibration_node) &&
- (candidate->team_number() == FLAGS_calibration_team_number)) {
- calibration = candidate;
- break;
- }
- }
+ const calibration::CameraCalibration *calibration =
+ CameraReader::FindCameraCalibration(&calibration_data.message(),
+ FLAGS_calibration_node,
+ FLAGS_calibration_team_number);
+ const auto intrinsics = CameraReader::CameraIntrinsics(calibration);
+ const auto extrinsics = CameraReader::CameraExtrinsics(calibration);
+ const auto dist_coeffs = CameraReader::CameraDistCoeffs(calibration);
- CHECK(calibration) << "No calibration data found for node \""
- << FLAGS_calibration_node << "\" with team number "
- << FLAGS_calibration_team_number;
-
- const auto intrinsics_float = cv::Mat(
- 3, 3, CV_32F,
- const_cast<void *>(
- static_cast<const void *>(calibration->intrinsics()->data())));
- cv::Mat intrinsics;
- intrinsics_float.convertTo(intrinsics, CV_64F);
-
- const frc971::vision::calibration::TransformationMatrix *transform =
- calibration->has_turret_extrinsics() ? calibration->turret_extrinsics()
- : calibration->fixed_extrinsics();
-
- const auto extrinsics_float = cv::Mat(
- 4, 4, CV_32F,
- const_cast<void *>(static_cast<const void *>(transform->data()->data())));
- cv::Mat extrinsics;
- extrinsics_float.convertTo(extrinsics, CV_64F);
-
- const auto dist_coeffs_float = cv::Mat(
- 5, 1, CV_32F,
- const_cast<void *>(
- static_cast<const void *>(calibration->dist_coeffs()->data())));
- cv::Mat dist_coeffs;
- dist_coeffs_float.convertTo(dist_coeffs, CV_64F);
+ // Compute undistortion map once for efficiency
+ const auto undistort_maps =
+ CameraReader::ComputeUndistortMaps(intrinsics, dist_coeffs);
TargetEstimator estimator(intrinsics, extrinsics);
for (auto it = file_list.begin() + FLAGS_skip; it < file_list.end(); it++) {
LOG(INFO) << "Reading file " << (it - file_list.begin()) << ": " << *it;
- cv::Mat image_mat_distorted = cv::imread(it->c_str());
- cv::Mat image_mat;
- cv::undistort(image_mat_distorted, image_mat, intrinsics, dist_coeffs);
+ cv::Mat image_mat =
+ CameraReader::UndistortImage(cv::imread(it->c_str()), undistort_maps);
BlobDetector::BlobResult blob_result;
blob_result.binarized_image =
diff --git a/y2022/vision/viewer_replay.cc b/y2022/vision/viewer_replay.cc
index b2d3464..5d09d55 100644
--- a/y2022/vision/viewer_replay.cc
+++ b/y2022/vision/viewer_replay.cc
@@ -194,6 +194,8 @@
bool use_image = true;
if (FLAGS_detected_only || FLAGS_filtered_only) {
+ // TODO(milind): if adding target estimation here in the future,
+ // undistortion is needed
BlobDetector::BlobResult blob_result;
BlobDetector::ExtractBlobs(image_mat, &blob_result);