diff --git a/constants/camera_constants.json b/constants/camera_constants.json index 077ccd60..6336b0af 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -25,6 +25,7 @@ "frame_width": 1280, "frame_height": 800, "fps": 100.0, + "serial_id": "devorin", "max_payload_size": 1024, "stream_ratio": 0.5, "port": 5801, diff --git a/scripts/build.sh b/scripts/build.sh index 2703ae55..868d5df8 100755 --- a/scripts/build.sh +++ b/scripts/build.sh @@ -21,6 +21,13 @@ else BUILD_DIR="${NAME}-build" fi -git submodule update --init --progress --depth 1 +if [ "$(pwd)" != "/bos" ]; then + mkdir -p /bos/constants 2>/dev/null || sudo mkdir -p /bos/constants + if [ -w /bos ]; then + cp -r constants/. /bos/constants + else + sudo cp -r constants/. /bos/constants + fi +fi cmake -Wno-dev -DENABLE_CLANG_TIDY=OFF -DCMAKE_BUILD_TYPE=Release -B "$BUILD_DIR" -G Ninja . cmake --build "$BUILD_DIR" diff --git a/scripts/cmake-4.3.2-linux-aarch64.sh b/scripts/cmake-4.3.2-linux-aarch64.sh new file mode 100755 index 00000000..56cee90e Binary files /dev/null and b/scripts/cmake-4.3.2-linux-aarch64.sh differ diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index ef546bdc..040b0343 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -65,6 +65,15 @@ target_link_libraries(MultiCameraSource PRIVATE utils ) +find_library(NVJPEG_LIBRARY + NAMES nvjpeg + PATHS + /usr/lib/aarch64-linux-gnu/tegra + /usr/lib/aarch64-linux-gnu + /usr/lib + REQUIRED +) + add_library(UVCCamera) target_sources(UVCCamera PRIVATE @@ -74,6 +83,20 @@ target_link_libraries(UVCCamera PRIVATE uvc absl::status utils + ${NVJPEG_LIBRARY} +) +set(JETSON_API /usr/src/jetson_multimedia_api) +target_sources(UVCCamera PRIVATE + ${JETSON_API}/samples/common/classes/NvJpegDecoder.cpp + ${JETSON_API}/samples/common/classes/NvBuffer.cpp + ${JETSON_API}/samples/common/classes/NvElement.cpp + ${JETSON_API}/samples/common/classes/NvElementProfiler.cpp + ${JETSON_API}/samples/common/classes/NvLogging.cpp +) +target_include_directories(UVCCamera PRIVATE + ${JETSON_API}/include + ${JETSON_API}/include/libjpeg-8b + ${JETSON_API}/samples/common/classes ) add_library(camera INTERFACE) diff --git a/src/camera/camera_constants.cc b/src/camera/camera_constants.cc index 5e567824..50ca05a9 100644 --- a/src/camera/camera_constants.cc +++ b/src/camera/camera_constants.cc @@ -1,4 +1,5 @@ #include "src/camera/camera_constants.h" +#include #include #include "absl/flags/flag.h" @@ -6,10 +7,26 @@ ABSL_FLAG(std::string, camera_constants_path, // NOLINT "/bos/constants/camera_constants.json", // NOTLINT "Path to the json file of camera constants"); //NOLINT +namespace { +constexpr std::string_view kDefaultCameraConstantsPath = + "/bos/constants/camera_constants.json"; +constexpr std::string_view kLocalCameraConstantsPath = + "constants/camera_constants.json"; + +auto ResolveCameraConstantsPath(const std::string& path) -> std::string { + if (path == kDefaultCameraConstantsPath && + std::filesystem::exists(kLocalCameraConstantsPath)) { + return std::string(kLocalCameraConstantsPath); + } + return path; +} +} // namespace + namespace camera { auto GetCameraConstants() -> camera_constants_t { - return GetCameraConstants(absl::GetFlag(FLAGS_camera_constants_path)); + return GetCameraConstants( + ResolveCameraConstantsPath(absl::GetFlag(FLAGS_camera_constants_path))); } template diff --git a/src/camera/cscore_streamer.cc b/src/camera/cscore_streamer.cc index ec5a7d7d..3b2983c6 100644 --- a/src/camera/cscore_streamer.cc +++ b/src/camera/cscore_streamer.cc @@ -1,6 +1,36 @@ #include "cscore_streamer.h" namespace camera { +auto NormalizeForStreaming(const cv::Mat& mat) -> cv::Mat { + if (mat.empty()) { + return {}; + } + + switch (mat.channels()) { + case 1: { + cv::Mat bgr; + cv::cvtColor(mat, bgr, cv::COLOR_GRAY2BGR); + return bgr; + } + case 2: { + cv::Mat bgr; + cv::cvtColor(mat, bgr, cv::COLOR_YUV2BGR_YUY2); + return bgr; + } + case 3: + return mat; + case 4: { + cv::Mat bgr; + cv::cvtColor(mat, bgr, cv::COLOR_BGRA2BGR); + return bgr; + } + default: + std::cerr << "Unsupported frame channel count for CSCore: " + << mat.channels(); + return {}; + } +} + CscoreStreamer::CscoreStreamer(const std::string& name, uint port, uint fps, uint width, uint height, bool verbose) : width_(width), height_(height) { @@ -25,11 +55,17 @@ CscoreStreamer::CscoreStreamer(const std::string& name, uint port, uint fps, verbose) {} void CscoreStreamer::WriteFrame(const cv::Mat& mat) { - cv::Mat frame; - cv::resize(mat, frame, cv::Size(width_, height_)); - if (frame.channels() == 4) { - cv::cvtColor(frame, frame, cv::COLOR_BGRA2BGR); + std::cout << "WRITING FRAME"; + cv::Mat normalized = NormalizeForStreaming(mat); + if (normalized.empty()) { + return; } + + cv::Mat frame; + cv::resize(normalized, frame, cv::Size(width_, height_)); source_.PutFrame(frame); + std::cout << "WROTE FRAME"; + std::this_thread::sleep_for(std::chrono::duration(1)); + std::cout << "End sleep"; } } // namespace camera diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index 91ecc930..7f073f59 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -1,67 +1,216 @@ #include "src/camera/uvc_camera.h" -#include -#include +#include "/usr/src/jetson_multimedia_api/include/NvBuffer.h" #include "absl/status/status.h" #include "src/utils/pch.h" namespace camera { -const cv::Mat UVCCamera::backup_image_ = - cv::imread("/bos/constants/dont_worry_about_it.jpg"); +constexpr std::array kJpegStartMarker = {0xFF, 0xD8}; +constexpr std::array kJpegEndMarker = {0xFF, 0xD9}; + +// Necessary because sometimes the size of the end sequence is variable, usually 5-7 bytes +// Example on home orin with usb camera: Img end was shifted0xffff60106574 vs 0xffff60106578 +// Doesn't seem to happen for image start but it is left in for safety +auto RemoveImagePackaging(const unsigned char* data, size_t size) + -> std::vector { + if (data == nullptr || size < 2) { + return {}; + } + + const unsigned char* full_frame_begin = data; + const unsigned char* full_frame_end = data + size; + + const unsigned char* start_of_image = + std::search(full_frame_begin, full_frame_end, + std::begin(kJpegStartMarker), std::end(kJpegStartMarker)); + if (start_of_image == full_frame_end) { + return {}; + } + + const unsigned char* end_of_image = full_frame_end; + for (const unsigned char* current = full_frame_end - 2; + current >= start_of_image; --current) { + if (current[0] == kJpegEndMarker[0] && current[1] == kJpegEndMarker[1]) { + end_of_image = current + 2; + break; + } + if (current == start_of_image) { + break; + } + } + + std::vector normalized(start_of_image, end_of_image); + if (normalized.size() < 2) { + return {}; + } + + return normalized; +} + +void FreeNvBuffer(NvBuffer* buff) { + if (buff != nullptr) { + delete buff; + } +} + +void LogPlaneFormat(const NvBuffer* buffer) { + for (int i = 0; i < buffer->n_planes; i++) { + std::cout << buffer->planes[0].fmt.height << " " + << buffer->planes[0].fmt.width << " " + << buffer->planes[0].fmt.stride << " " + << buffer->planes[0].fmt.bytesperpixel << std::endl; + } +} + +void LogPotentialBufferFormat(const NvBuffer* buffer, + NvBuffer::NvBufferPlaneFormat& format, + uint32_t raw_pixfmt, uint32_t width, + uint32_t height) { + uint32_t num_planes; + buffer->fill_buffer_plane_format(&num_planes, &format, width, height, + raw_pixfmt); + std::cout << format.height << " " << format.width << " " << format.stride + << " " << format.bytesperpixel << " with fmt: " << raw_pixfmt + << std::endl; +} void callback(uvc_frame_t* frame, void* ptr) { + LOG(INFO) << "Hallo cb"; auto ptr_ = static_cast(ptr); - if (ptr_->mutex_.try_lock()) { - switch (frame->frame_format) { - case UVC_COLOR_FORMAT_MJPEG: { - char* data = static_cast(frame->data); - std::vector buffer(data, data + frame->data_bytes); - ptr_->frame_buffer.frame = cv::imdecode(buffer, UVCCamera::read_type); - break; + std::unique_lock lock(ptr_->mutex_, std::try_to_lock); + if (!lock.owns_lock()) { + return; + } + + switch (frame->frame_format) { + case UVC_COLOR_FORMAT_MJPEG: { + NvBuffer* decoded_frame_buffer = nullptr; + uint32_t pixfmt, width, height; + std::vector jpeg = RemoveImagePackaging( + reinterpret_cast(frame->data), frame->data_bytes); + if (jpeg.empty()) { + LOG(WARNING) << "Failed to normalize MJPEG frame for camera " + << ptr_->camera_constant_.name; + return; + } + + int ret = + ptr_->decoder_->decodeToBuffer(&decoded_frame_buffer, jpeg.data(), + jpeg.size(), &pixfmt, &width, &height); + + std::cout << "Pixfmt: " << pixfmt << std::endl; + + if (ret != 0 || decoded_frame_buffer == nullptr) { + LOG(WARNING) << "Decode failed for camera " + << ptr_->camera_constant_.name << " with code: " << ret; + FreeNvBuffer(decoded_frame_buffer); + return; } - case UVC_COLOR_FORMAT_YUYV: { - uvc_frame_t* bgr = uvc_allocate_frame(frame->width * frame->height * 3); - if (!bgr) { - LOG(WARNING) << "Camera " << ptr_->camera_constant_.name - << " failed to allocate "; - ptr_->mutex_.unlock(); + + if (ptr_->read_type == cv::IMREAD_COLOR) { + // TODO support a reasonable number of formats other than yuyv422M + if (pixfmt != V4L2_PIX_FMT_YUV422M) { + LOG(WARNING) << "Didn't receive multiplanar format, instead got " + << pixfmt; + std::cout << "Possible Formats: " << V4L2_PIX_FMT_NV12M << " " + << V4L2_PIX_FMT_YUV32 << " " << V4L2_PIX_FMT_YUV420 << " " + << V4L2_PIX_FMT_YUV420M << " " + << " " << V4L2_PIX_FMT_YUV422P << " " << std::endl; return; } - uvc_error_t ret = uvc_yuyv2bgr(frame, bgr); - if (ret != 0) { - LOG(WARNING) << "YUYV failed to convert to BGR"; + + const NvBuffer::NvBufferPlane& y_plane = + decoded_frame_buffer->planes[0]; + const NvBuffer::NvBufferPlane& u_plane = + decoded_frame_buffer->planes[1]; + const NvBuffer::NvBufferPlane& v_plane = + decoded_frame_buffer->planes[2]; + const uint32_t y_stride = + y_plane.fmt.stride * y_plane.fmt.bytesperpixel; + const uint32_t uv_stride = y_stride / 2; + cv::Mat yuyv422_interleaved(height, width, CV_8UC2); + for (size_t row = 0; row < height; row++) { + cv::Vec2b* row_ptr = yuyv422_interleaved.ptr(row); + uint8_t* y_ptr = y_plane.data + row * y_stride; + uint8_t* u_ptr = u_plane.data + row * uv_stride; + uint8_t* v_ptr = v_plane.data + row * uv_stride; + for (size_t col = 0; col < width; col += 2) { + row_ptr[col][0] = y_ptr[col]; + row_ptr[col][1] = u_ptr[col / 2]; + row_ptr[col + 1][0] = y_ptr[col + 1]; + row_ptr[col + 1][1] = v_ptr[col / 2]; + } } - IplImage* ipl_image; - ipl_image = cvCreateImageHeader(cvSize(bgr->width, bgr->height), - IPL_DEPTH_8U, 3); - cvSetData(ipl_image, bgr->data, bgr->width * 3); - ptr_->frame_buffer.frame = cv::cvarrToMat(ipl_image, true); - uvc_free_frame(bgr); - break; + + // cv::Mat rgb; + // std::cout << "1" << std::endl; + // cv::cvtColor(yuyv422_interleaved, rgb, cv::COLOR_YUV2BGR_YUY2); + // std::cout << "2" << std::endl; + // cv::imwrite("frame" + std::to_string(frame->sequence) + ".jpg", rgb); + // std::cout << "3" << std::endl; + ptr_->frame_buffer.frame = std::move(yuyv422_interleaved); + // std::cout << "4" << std::endl; + } else { + NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; + ptr_->frame_buffer.frame = + cv::Mat(height, width, CV_8UC1, luminance.data, + luminance.fmt.stride * luminance.fmt.bytesperpixel) + .clone(); } - default: - LOG(WARNING) << "Unknown frame format"; - break; + FreeNvBuffer(decoded_frame_buffer); + break; } - if (ptr_->frame_buffer.frame.empty()) { - LOG(WARNING) << "Failed to decode frame from camera " - << ptr_->camera_constant_.name; - ptr_->mutex_.unlock(); - return; + case UVC_COLOR_FORMAT_YUYV: { + uvc_frame_t* bgr = uvc_allocate_frame(frame->width * frame->height * 3); + if (!bgr) { + LOG(WARNING) << "Camera " << ptr_->camera_constant_.name + << " failed to allocate "; + return; + } + uvc_error_t ret = uvc_yuyv2bgr(frame, bgr); + if (ret != 0) { + LOG(WARNING) << "YUYV failed to convert to BGR"; + uvc_free_frame(bgr); + return; + } + ptr_->frame_buffer.frame = + cv::Mat(bgr->height, bgr->width, CV_8UC3, bgr->data, bgr->width * 3) + .clone(); + uvc_free_frame(bgr); + break; } - ptr_->frame_buffer.invalid = false; - ptr_->frame_buffer.timestamp = - frc::Timer::GetFPGATimestamp() - .to(); // TODO: Use more accurate timestamp - ptr_->frame_index_ = frame->sequence; - ptr_->mutex_.unlock(); + default: + LOG(WARNING) << "Unknown frame format"; + break; } + if (ptr_->frame_buffer.frame.empty()) { + LOG(WARNING) << "Failed to decode frame from camera " + << ptr_->camera_constant_.name; + return; + } + ptr_->frame_buffer.invalid = false; + ptr_->frame_buffer.timestamp = + frc::Timer::GetFPGATimestamp() + .to(); // TODO: Use more accurate timestamp + ptr_->frame_index_ = frame->sequence; } UVCCamera::UVCCamera(const CameraConstant& camera_constant, absl::Status& status, std::optional log_path) - : camera_constant_(camera_constant), log_path_(std::move(log_path)) { - + : camera_constant_(camera_constant), + log_path_(std::move(log_path)), + decoder_( + NvJPEGDecoder::createJPEGDecoder(camera_constant_.name.c_str())) { + if (decoder_ == nullptr) { + status = absl::InternalError("Failed to create JPEG decoder for " + + camera_constant_.name); + return; + } + if (!camera_constant.serial_id.has_value()) { + status = absl::InvalidArgumentError(fmt::format( + "Must provide a serial id for uvc camera {}", camera_constant.name)); + return; + } int res = uvc_init(&context_, nullptr); if (res != 0) { status = absl::AbortedError( @@ -69,16 +218,8 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, camera_constant.name, res)); return; } - if (!camera_constant.serial_id.has_value()) { - LOG(WARNING) << "Was not provided with serial id. This shuold only be done " - "if there is only one uvc camera connected"; - - res = uvc_find_device(context_, &device_, 0, 0, nullptr); - } else { - res = uvc_find_device(context_, &device_, 0, 0, - camera_constant_.serial_id->c_str()); - LOG(INFO) << "Serial id: " << camera_constant_.serial_id.value(); - } + res = uvc_find_device(context_, &device_, 0, 0, + camera_constant_.serial_id->c_str()); if (res != 0) { status = absl::AbortedError( fmt::format("Unable to find device for camera {} with error code {}", @@ -98,8 +239,9 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, camera_constant_.frame_width.value(), camera_constant_.frame_height.value(), camera_constant_.fps.value()); if (res != 0) { - status = absl::AbortedError("Unable to get stream format for camera: " + - camera_constant.name); + status = absl::AbortedError(fmt::format( + "Unable to get stream control for camera {} with error code {}", + camera_constant.name, res)); return; } uvc_print_stream_ctrl(&ctrl_, stderr); @@ -109,8 +251,9 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, camera_constant.max_frame_size.value_or(ctrl_.dwMaxVideoFrameSize); res = uvc_start_streaming(device_handle_, &ctrl_, callback, this, 0); if (res != 0) { - status = absl::AbortedError("Unable to start streaming for camera: " + - camera_constant.name); + status = absl::AbortedError( + fmt::format("Unable to start stream for camera {} with error code {}", + camera_constant.name, res)); return; } } @@ -120,9 +263,8 @@ auto UVCCamera::GetFrame() -> timestamped_frame_t { while (frame_index_ == previous_frame_index_) { std::this_thread::yield(); } - mutex_.lock(); + std::lock_guard lock(mutex_); if (frame_buffer.frame.empty()) { - backup_image_.copyTo(copied_timestamped_frame.frame); copied_timestamped_frame.invalid = true; copied_timestamped_frame.timestamp = frc::Timer::GetFPGATimestamp().to(); @@ -131,7 +273,6 @@ auto UVCCamera::GetFrame() -> timestamped_frame_t { copied_timestamped_frame.invalid = frame_buffer.invalid; copied_timestamped_frame.timestamp = frame_buffer.timestamp; } - mutex_.unlock(); previous_frame_index_ = frame_index_; return copied_timestamped_frame; } @@ -141,10 +282,8 @@ auto UVCCamera::Restart() -> void { uvc_close(device_handle_); uvc_unref_device(device_); - const char* serial_id = camera_constant_.serial_id.has_value() - ? camera_constant_.serial_id.value().c_str() - : nullptr; - uvc_find_device(context_, &device_, 0, 0, serial_id); + uvc_find_device(context_, &device_, 0, 0, + camera_constant_.serial_id->c_str()); uvc_open(device_, &device_handle_); LOG(INFO) << "Restarting device UVC Camera. Device ctrl: "; @@ -154,11 +293,17 @@ auto UVCCamera::Restart() -> void { } UVCCamera::~UVCCamera() { - uvc_stop_streaming(device_handle_); - uvc_close(device_handle_); - uvc_unref_device(device_); - uvc_exit(context_); - LOG(INFO) << camera_constant_.name << " has been destructed"; + if (device_handle_ != nullptr) { + uvc_stop_streaming(device_handle_); + uvc_close(device_handle_); + } + if (device_ != nullptr) { + uvc_unref_device(device_); + } + if (context_ != nullptr) { + uvc_exit(context_); + } + delete decoder_; } auto UVCCamera::GetCameraConstant() const -> camera_constant_t { diff --git a/src/camera/uvc_camera.h b/src/camera/uvc_camera.h index 34dafe2a..cf2836b9 100644 --- a/src/camera/uvc_camera.h +++ b/src/camera/uvc_camera.h @@ -1,5 +1,6 @@ #pragma once #include +#include "/usr/src/jetson_multimedia_api/include/NvJpegDecoder.h" #include "camera_constants.h" #include "libuvc/libuvc.h" #include "src/camera/camera.h" @@ -20,16 +21,16 @@ class UVCCamera : public ICamera { public: const camera_constant_t camera_constant_; std::optional log_path_; - static const cv::Mat backup_image_; - uvc_context_t* context_; - uvc_device_t* device_; - uvc_device_handle_t* device_handle_; + uvc_context_t* context_ = nullptr; + uvc_device_t* device_ = nullptr; + uvc_device_handle_t* device_handle_ = nullptr; timestamped_frame_t frame_buffer; uvc_stream_ctrl_t ctrl_; std::mutex mutex_; - int frame_index_; - int previous_frame_index_; - static constexpr cv::ImreadModes read_type = cv::IMREAD_GRAYSCALE; + int frame_index_ = 0; + int previous_frame_index_ = 0; + NvJPEGDecoder* decoder_ = nullptr; + static constexpr cv::ImreadModes read_type = cv::IMREAD_COLOR; private: auto StartCamera(uvc_stream_ctrl_t ctrl) -> void; diff --git a/src/localization/gpu_apriltag_detector.cc b/src/localization/gpu_apriltag_detector.cc index 5902a025..07c0069b 100644 --- a/src/localization/gpu_apriltag_detector.cc +++ b/src/localization/gpu_apriltag_detector.cc @@ -53,16 +53,30 @@ auto GPUAprilTagDetector::GetTagDetections( CHECK(!timestamped_frame.frame.empty()); try { absl::Status detection_status; - if (image_format == vision::ImageFormat::MONO8) { - CHECK(timestamped_frame.frame.channels() == 1); - detection_status = - gpu_detector_->Detect(timestamped_frame.frame.data, nullptr); - } else { // TODO allow other formats than YUY2 - CHECK(timestamped_frame.frame.channels() == 3); - cv::Mat gray; - cv::cvtColor(timestamped_frame.frame, gray, cv::COLOR_BGR2YUV_YUY2); - cv::Mat mat_ = ToMat(gray); - detection_status = gpu_detector_->Detect(mat_.data, nullptr); + switch (image_format) { + case vision::ImageFormat::MONO8: { + CHECK(timestamped_frame.frame.channels() == 1); + detection_status = + gpu_detector_->Detect(timestamped_frame.frame.data, nullptr); + break; + } + case vision::ImageFormat::BGR8: { // TODO allow other formats than YUY2 + CHECK(timestamped_frame.frame.channels() == 3); + cv::Mat yuyv_mat; + cv::cvtColor(timestamped_frame.frame, yuyv_mat, cv::COLOR_BGR2YUV_YUY2); + cv::Mat mat_ = ToMat(yuyv_mat); + detection_status = gpu_detector_->Detect(mat_.data, nullptr); + break; + } + case vision::ImageFormat::YUYV422: { + LOG(INFO) << "Ch: " << timestamped_frame.frame.channels(); + CHECK(timestamped_frame.frame.channels() == 2); + detection_status = + gpu_detector_->Detect(timestamped_frame.frame.data, nullptr); + break; + } + default: + LOG(WARNING) << "Unsupported image format"; } if (!detection_status.ok()) { // LOG(WARNING) << "Gpu detector failed! Error: " diff --git a/src/localization/gpu_apriltag_detector.h b/src/localization/gpu_apriltag_detector.h index 088cbfa7..3cf26487 100644 --- a/src/localization/gpu_apriltag_detector.h +++ b/src/localization/gpu_apriltag_detector.h @@ -24,6 +24,6 @@ class GPUAprilTagDetector : public IAprilTagDetector { apriltag_detector_t* apriltag_detector_; std::unique_ptr gpu_detector_; static constexpr vision::ImageFormat image_format = - vision::ImageFormat::MONO8; + vision::ImageFormat::YUYV422; }; } // namespace localization diff --git a/src/test/integration_test/apriltag_detect_test.cc b/src/test/integration_test/apriltag_detect_test.cc index a3e41839..24ffdc13 100644 --- a/src/test/integration_test/apriltag_detect_test.cc +++ b/src/test/integration_test/apriltag_detect_test.cc @@ -57,7 +57,11 @@ auto main(int argc, char* argv[]) -> int { LOG(INFO) << position_estimate; } + LOG(INFO) << "Pre conversion"; timestamped_frame.frame.copyTo(display_frame); + cv::cvtColor(display_frame, display_frame, cv::COLOR_YUV2BGR_YUY2); + LOG(INFO) << "Post conversion"; + for (auto& tag_detection : tag_detections) { for (auto& corner : tag_detection.corners) { cv::circle(display_frame, corner, 10, cv::Scalar(0, 0, 255)); diff --git a/src/unambiguous_second.cc b/src/unambiguous_second.cc index d995a690..b981fb17 100644 --- a/src/unambiguous_second.cc +++ b/src/unambiguous_second.cc @@ -22,7 +22,7 @@ auto main() -> int { std::vector cameras{ camera_constants.at("second_bot_left"), camera_constants.at("second_bot_right"), - camera_constants.at("second_bot_front"), + // camera_constants.at("second_bot_front"), }; std::jthread thread([cameras](const std::stop_token& stop_token) { diff --git a/src/utils/transform.h b/src/utils/transform.h index d573be1d..4db5a998 100644 --- a/src/utils/transform.h +++ b/src/utils/transform.h @@ -85,12 +85,14 @@ auto SeparateTranslationAndRotationMatrices(const TransformValues& combined) -> TransformDecomposition; inline auto PoseOffField(frc::Pose3d pose) -> bool { - constexpr double kerror_margin = 0.2; + return false; + /*constexpr double kerror_margin = 0.2; return pose.X().value() < 0 - kerror_margin || pose.X().value() > 16.54 + kerror_margin || pose.Y().value() < 0 - kerror_margin || pose.Y().value() > - 8 + kerror_margin /* || pose.Z().value() > kerror_margin*/; + 8 + kerror_margin /* || pose.Z().value() > kerror_margin*/ + ; } } // namespace utils