fix: fixed index out of rabge and other minor bugs

packages/camera/include/calibration.h

@@ -129,8 +129,9 @@ class CalibrationNode : public rclcpp::Node {
     struct State {
         std::optional<cv::Size> frame_size = std::nullopt;
 
-        std::vector<std::map<uint32_t, std::pair<size_t, std::unique_ptr<int[]>>>> detected_ids_all;
-        std::vector<std::map<uint32_t, std::pair<size_t, std::unique_ptr<cv::Point2f[]>>>>
+        // map< timestamp of pairs<num_of_elements, smart_ptr_to_data> >
+        std::vector<std::map<size_t, std::pair<size_t, std::unique_ptr<int[]>>>> detected_ids_all;
+        std::vector<std::map<size_t, std::pair<size_t, std::unique_ptr<cv::Point2f[]>>>>
             detectected_corners_all;
         std::vector<std::vector<Polygon>> polygons_all;
         std::vector<foxglove_msgs::msg::ImageMarkerArray> board_markers_array;

packages/camera/src/calibration.cpp

@@ -112,15 +112,15 @@ 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();
         state_.detected_ids_all.emplace_back();
         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<int[]> 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<cv::Point2f[]> 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<std::vector<std::vector<cv::Point2f>>> image_points_all{};
-    std::vector<std::vector<std::vector<cv::Point3f>>> obj_points_all{};
+    std::vector<std::vector<std::vector<cv::Point2f>>> image_points_all(
+        param_.cameras_to_calibrate.size());
+    std::vector<std::vector<std::vector<cv::Point3f>>> obj_points_all(
+        param_.cameras_to_calibrate.size());
 
     int chosen_detections_cnt = 0;
 
@@ -524,20 +528,22 @@ 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<uint32_t>::max();
-        uint32_t max_timestamp = 0;
+        size_t min_timestamp = std::numeric_limits<size_t>::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) {
                 min_timestamp = current_timestamp;
                 idx_of_min_timestamp = i;
             }
         }
-        if (max_timestamp - min_timestamp < 10e9 / param_.fps) {  // found simultanious snap
+
+        if (max_timestamp - min_timestamp <
+            static_cast<size_t>(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<std::vector<std::vector<cv::Point2f>>> image_points_common(image_points_all.size());
-    std::vector<std::vector<cv::Point3f>> 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<std::vector<std::vector<cv::Point2f>>> image_points_common(
+        param_.cameras_to_calibrate.size());
+    std::vector<std::vector<cv::Point3f>> 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));