Triangulation and visualisation

packages/camera/CMakeLists.txt

@@ -4,7 +4,6 @@ project(camera)
 add_compile_options(-Wall -Wextra -Werror=unused-variable -Wpedantic)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
 
-# find_package(cv_bridge REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(ament_cmake REQUIRED)
@@ -17,6 +16,7 @@ add_executable(camera src/camera.cpp src/camera_status.cpp)
 add_executable(
   calibration src/calibration_main.cpp src/calibration.cpp src/params.cpp
 )
+add_executable(triangulation src/triangulation.cpp src/params.cpp)
 
 target_include_directories(
   camera PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -27,13 +27,20 @@ target_include_directories(
   calibration PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                      "/usr/include"
 )
+target_include_directories(
+  triangulation PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+                       "/usr/include"
+)
 
 ament_target_dependencies(camera yaml_cpp_vendor)
 ament_target_dependencies(calibration yaml_cpp_vendor Boost OpenCV)
+ament_target_dependencies(triangulation OpenCV yaml_cpp_vendor)
 
 target_link_libraries(camera mvsdk)
 
-install(TARGETS camera DESTINATION lib/${PROJECT_NAME})
+install(TARGETS camera calibration triangulation
+        DESTINATION lib/${PROJECT_NAME}
+)
 
 if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/params")
   file(MAKE_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/params")

packages/camera/include/params.h

@@ -26,6 +26,7 @@ struct CameraIntrinsicParameters {
     int camera_id = 0;
     cv::Size image_size{};
     cv::Mat camera_matrix{};
+    cv::Mat camera_matrix_inv{};
     cv::Vec<double, 5> dist_coefs{};
 
     struct Cached {

packages/camera/include/triangulation.h

@@ -0,0 +1,29 @@
+#include "params.h"
+#include "params.h"
+
+#include <opencv2/core/core.hpp>
+#include <string>
+#include <vector>
+
+namespace handy {
+// the following interface counts on 2 cameras only
+
+struct StereoParameters {
+    cv::Mat rotation_matrix;
+    cv::Mat translation_vector;
+};
+
+class TriangulationNode {
+  public:
+    TriangulationNode(std::string& params_file_path, std::vector<int> camera_ids);
+
+    cv::Point3f triangulatePosition(std::vector<cv::Point2f> image_points);
+
+  private:
+    struct Params {
+        std::vector<CameraIntrinsicParameters> cameras_intrinsics = {};
+        std::vector<StereoParameters> camera_stereo_params = {};
+        std::vector<cv::Mat> projection_matrices = {};
+    } param_{};
+};
+}  // namespace handy

packages/camera/scripts/mask_to_centroid.py

@@ -0,0 +1,63 @@
+import argparse
+import json
+import os
+from typing import List
+
+import cv2
+import numpy as np
+
+
+def get_bbox(mask: np.ndarray) -> List[float]:
+    # x_min, y_min, x_max, y_max
+    horizontal_indicies = np.where(np.any(mask, axis=0))[0]
+    vertical_indicies = np.where(np.any(mask, axis=1))[0]
+    x1, x2 = horizontal_indicies[[0, -1]]
+    y1, y2 = vertical_indicies[[0, -1]]
+    bbox = list(map(float, [x1, y1, x2, y2]))
+    return bbox
+
+
+def read_and_convert(sources: str, output_file: str, width=1920, height=1200) -> None:
+    if not all([os.path.exists(source) for source in sources]):
+        raise ValueError
+
+    common_filenames = set(os.listdir(sources[0]))
+    for source in sources[1:]:
+        common_filenames.intersection_update(os.listdir(source))
+    common_filenames = sorted(common_filenames)
+
+    for i in range(len(sources)):
+        source = sources[i]
+        json_data = {}
+        for filename in common_filenames:
+            path_to_file = os.path.join(source, filename)
+            image = cv2.imread(path_to_file, cv2.IMREAD_GRAYSCALE)
+            bbox = get_bbox(image)
+            centroid_x = (bbox[0] + bbox[2]) / 2
+            centroid_y = (bbox[1] + bbox[3]) / 2
+            json_data[filename] = {}
+            json_data[filename]["centroid"] = [centroid_x, centroid_y]
+        with open(f"{output_file}_{i + 1}.json", mode="w", encoding="utf-8") as file:
+            json.dump(json_data, file)
+
+
+def init_parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument(
+        "--width", help="Width of a single frame", type=int, default=1920
+    )
+    parser.add_argument(
+        "--height", help="height of a single frame", type=int, default=1200
+    )
+    parser.add_argument("--sources", help="folder with masks", required=True, nargs="*")
+    parser.add_argument("--export", help="just_filename_no_extension", required=True)
+
+    return parser
+
+
+if __name__ == "__main__":
+    parser = init_parser()
+    args = parser.parse_args()
+
+    read_and_convert(args.sources, args.export, args.width, args.height)