From 0bfbcee82354b266d2b84a114fb9d325f53f9368 Mon Sep 17 00:00:00 2001 From: Bakin Denis Date: Tue, 2 Apr 2024 01:48:08 +0300 Subject: [PATCH] fix: fixed index out of rabge and other minor bugs --- packages/camera/include/calibration.h | 5 ++- packages/camera/src/calibration.cpp | 54 +++++++++++++++------------ 2 files changed, 34 insertions(+), 25 deletions(-) diff --git a/packages/camera/include/calibration.h b/packages/camera/include/calibration.h index 6d95fc6..f6feea5 100644 --- a/packages/camera/include/calibration.h +++ b/packages/camera/include/calibration.h @@ -129,8 +129,9 @@ class CalibrationNode : public rclcpp::Node { struct State { std::optional frame_size = std::nullopt; - std::vector>>> detected_ids_all; - std::vector>>> + // map< timestamp of pairs > + std::vector>>> detected_ids_all; + std::vector>>> detectected_corners_all; std::vector> polygons_all; std::vector board_markers_array; diff --git a/packages/camera/src/calibration.cpp b/packages/camera/src/calibration.cpp index 263d0a8..a95f114 100644 --- a/packages/camera/src/calibration.cpp +++ b/packages/camera/src/calibration.cpp @@ -112,7 +112,7 @@ CalibrationNode::CalibrationNode() : Node("calibration_node") { // intrinsics_.push_back(CameraIntrinsicParameters::loadFromYaml( // param_.path_to_params, param_.cameras_to_calibrate[i])); // state_.is_mono_calibrated.push_back(intrinsics_.back().isCalibrated()); - state_.is_mono_calibrated.push_back(true); + state_.is_mono_calibrated.push_back(false); intrinsics_.emplace_back(); state_.board_markers_array.emplace_back(); state_.board_corners_array.emplace_back(); @@ -120,7 +120,7 @@ CalibrationNode::CalibrationNode() : Node("calibration_node") { state_.detectected_corners_all.emplace_back(); state_.polygons_all.emplace_back(); } - state_.global_calibration_state = kStereoCapturing; + state_.global_calibration_state = kNotCalibrated; timer_.calibration_state = this->create_wall_timer( std::chrono::milliseconds(250), [this] { publishCalibrationState(); }); @@ -257,7 +257,6 @@ void CalibrationNode::handleFrame( if (!checkMaxSimilarity(current_corners, camera_idx)) { return; } - RCLCPP_INFO(this->get_logger(), "adding detection for idx=%d", camera_idx); if (param_.publish_preview_markers) { state_.board_markers_array[camera_idx].markers.push_back( @@ -267,12 +266,15 @@ void CalibrationNode::handleFrame( std::unique_ptr new_ids(new int[current_ids.size()]); std::copy(current_ids.begin(), current_ids.end(), new_ids.get()); - state_.detected_ids_all[camera_idx][image_ptr->header.stamp.nanosec] = + // timestamp is stored in millisecond + const size_t frame_timestamp_ms = + 1000ull * image_ptr->header.stamp.sec + image_ptr->header.stamp.nanosec / 1000000ull; + state_.detected_ids_all[camera_idx][frame_timestamp_ms] = std::make_pair(current_ids.size(), std::move(new_ids)); std::unique_ptr new_corners(new cv::Point2f[current_corners.size()]); std::copy(current_corners.begin(), current_corners.end(), new_corners.get()); - state_.detectected_corners_all[camera_idx][image_ptr->header.stamp.nanosec] = + state_.detectected_corners_all[camera_idx][frame_timestamp_ms] = std::make_pair(current_corners.size(), std::move(new_corners)); state_.polygons_all[camera_idx].push_back(convertToBoostPolygon(current_corners)); @@ -416,7 +418,7 @@ void CalibrationNode::fillImageObjectPoints( for (auto id_iter = state_.detectected_corners_all[camera_idx].begin(); id_iter != state_.detectected_corners_all[camera_idx].end(); ++id_iter) { - const uint32_t timestamp = id_iter->first; + const size_t timestamp = id_iter->first; auto& [size, data_ptr] = id_iter->second; // CV_32FC2 -- 4 bytes, 2 channels -- x and y coordinates @@ -514,8 +516,10 @@ void CalibrationNode::stereoCalibrate() { publishCalibrationState(); RCLCPP_INFO(this->get_logger(), "Stereo calibration inialized"); - std::vector>> image_points_all{}; - std::vector>> obj_points_all{}; + std::vector>> image_points_all( + param_.cameras_to_calibrate.size()); + std::vector>> obj_points_all( + param_.cameras_to_calibrate.size()); int chosen_detections_cnt = 0; @@ -524,12 +528,12 @@ void CalibrationNode::stereoCalibrate() { state_.detectected_corners_all.end(), [&](const auto& elem) { return !elem.empty() && chosen_detections_cnt < elem.size(); })) { int idx_of_min_timestamp = 0; - uint32_t min_timestamp = std::numeric_limits::max(); - uint32_t max_timestamp = 0; + size_t min_timestamp = std::numeric_limits::max(); + size_t max_timestamp = 0; for (int i = 0; i < state_.detectected_corners_all.size(); ++i) { auto current_map_elem = state_.detectected_corners_all[i].begin(); std::advance(current_map_elem, chosen_detections_cnt); - uint32_t current_timestamp = current_map_elem->second.first; + size_t current_timestamp = current_map_elem->first; max_timestamp = std::max(max_timestamp, current_timestamp); if (current_timestamp < min_timestamp) { @@ -537,7 +541,9 @@ void CalibrationNode::stereoCalibrate() { idx_of_min_timestamp = i; } } - if (max_timestamp - min_timestamp < 10e9 / param_.fps) { // found simultanious snap + + if (max_timestamp - min_timestamp < + static_cast(10e3 / param_.fps)) { // found simultaneous snapshot ++chosen_detections_cnt; } else { auto corners_iter_to_delete = @@ -569,19 +575,23 @@ void CalibrationNode::stereoCalibrate() { } // for now we assume that we have 2 cameras - std::vector>> image_points_common(image_points_all.size()); - std::vector> obj_points_common(obj_points_all.size()); - for (int detection_idx = 0; detection_idx < image_points_all.size(); ++detection_idx) { - obj_points_common.emplace_back(); - image_points_common[0].emplace_back(); - image_points_common[1].emplace_back(); + std::vector>> image_points_common( + param_.cameras_to_calibrate.size()); + std::vector> obj_points_common; + for (int detection_idx = 0; detection_idx < image_points_all[0].size(); ++detection_idx) { + if (obj_points_common.empty() || obj_points_common.back().empty()) { + obj_points_common.emplace_back(); + image_points_common[0].emplace_back(); + image_points_common[1].emplace_back(); + } + fillCommonImageObjectPoints( image_points_all[0][detection_idx], obj_points_all[0][detection_idx], - image_points_all[0][detection_idx], - obj_points_all[0][detection_idx], - image_points_common[0].back(), + image_points_all[1][detection_idx], + obj_points_all[1][detection_idx], image_points_common[0].back(), + image_points_common[1].back(), obj_points_common.back()); } @@ -600,8 +610,6 @@ void CalibrationNode::stereoCalibrate() { T, cv::noArray(), cv::noArray(), - cv::noArray(), - cv::noArray(), per_view_errors, cv::CALIB_FIX_INTRINSIC, cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON));