.gitignore

@@ -1,13 +1,6 @@
 # Distribution / packaging
 .Python
 build/
-models/
-confs/
-ZLM/
-ZLMediaKit/
-ffmpeg-4.2.5/
-nv-codec-headers/
-*.bz2
 develop-eggs/
 dist/
 eggs/
@@ -22,11 +15,9 @@ share/python-wheels/
 *.egg
 MANIFEST
 .idea/
-calib_webcam_1280x720.yaml
+models/
-calib_webcam_640x480.yaml
+models-converting.sh
-sv_algorithm_params.json
+models-downloading.sh
-sv_algorithm_params_coco_1280.json
-sv_algorithm_params_coco_640.json
 
 # Prerequisites
 *.d

CMakeLists.txt

@@ -24,7 +24,6 @@ else()
     option(USE_FFMPEG "BUILD WITH FFMPEG." ON)
   elseif(PLATFORM STREQUAL "X86_INTEL")
     add_definitions(-DPLATFORM_X86_INTEL)
-    option(USE_INTEL "BUILD WITH INTEL." ON)
     option(USE_FFMPEG "BUILD WITH FFMPEG." ON)
   else()
     message(FATAL_ERROR "UNSUPPORTED PLATFORM!")
@@ -39,10 +38,6 @@ if(USE_CUDA)
   message(STATUS "CUDA: ON")
 endif()
 
-if(USE_INTEL)
-  add_definitions(-DWITH_INTEL)
-  message(STATUS "INTEL: ON")
-endif()
 
 if(USE_GSTREAMER)
   add_definitions(-DWITH_GSTREAMER)
@@ -72,22 +67,15 @@ include_directories(
   ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/IOs/serial/include
   ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/FIFO
   ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G1
-  ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/AT10
+  ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G2
   ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/Q10f
-  ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/GX40
   ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src
   ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl
-  ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/common_det/intel
-  ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/landing_det/intel
-  ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/veri/intel
   ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/common_det/cuda
   ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/landing_det/cuda
   ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/sot/ocv470
-  ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/sort
-  ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/bytetrack
   ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/color_line
   ${CMAKE_CURRENT_SOURCE_DIR}/video_io
-  ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/veri/cuda
   ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/ellipse_det
   ${CMAKE_CURRENT_SOURCE_DIR}/utils
 )
@@ -128,7 +116,6 @@ set(
   include/sv_sot.h
   include/sv_mot.h
   include/sv_color_line.h
-  include/sv_veri_det.h
   include/sv_video_input.h
   include/sv_video_output.h
   include/sv_world.h
@@ -142,16 +129,14 @@ list(APPEND serial_SRCS gimbal_ctrl/IOs/serial/src/impl/unix.cc)
 list(APPEND serial_SRCS gimbal_ctrl/IOs/serial/src/impl/list_ports/list_ports_linux.cc)
 
 set(driver_SRCS
-  gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp
+  gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc
 )
 file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G1/*.cpp)
 list(APPEND driver_SRCS ${DRV_LIB_FILES})
-file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/AT10/*.cpp)
+file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/G2/*.cpp)
 list(APPEND driver_SRCS ${DRV_LIB_FILES})
 file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/Q10f/*.cpp)
 list(APPEND driver_SRCS ${DRV_LIB_FILES})
-file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/GX40/*.cpp)
-list(APPEND driver_SRCS ${DRV_LIB_FILES})
 file(GLOB DRV_LIB_FILES ${CMAKE_CURRENT_SOURCE_DIR}/gimbal_ctrl/driver/src/*.cpp)
 list(APPEND driver_SRCS ${DRV_LIB_FILES})
 
@@ -170,7 +155,6 @@ set(spirecv_SRCS
   algorithm/ellipse_det/ellipse_detector.cpp
   algorithm/common_det/sv_common_det.cpp
   algorithm/landing_det/sv_landing_det.cpp
-  algorithm/veri/sv_veri_det.cpp
   algorithm/sot/sv_sot.cpp
   algorithm/mot/sv_mot.cpp
   algorithm/color_line/sv_color_line.cpp
@@ -184,10 +168,6 @@ file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/video_io/*.cpp)
 list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
 file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/utils/*.cpp)
 list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
-file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/sort/*.cpp)
-list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
-file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/bytetrack/*.cpp)
-list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
 
 if(USE_CUDA)
   list(APPEND spirecv_SRCS algorithm/common_det/cuda/yolov7/preprocess.cu)
@@ -197,27 +177,9 @@ if(USE_CUDA)
   list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
   file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/landing_det/cuda/*.cpp)
   list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
-  file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/veri/cuda/*.cpp)
-  list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
-endif()
-
-if(USE_INTEL)
-  file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/common_det/intel/*.cpp)
-  list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
-  file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/landing_det/intel/*.cpp)
-  list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
-  file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/veri/intel/*.cpp)
-  list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
 endif()
 
 if(USE_FFMPEG)
-  if(USE_INTEL)
-    file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/video_io/ffmpeg/x86_intel/*.cpp)
-    list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
-  elseif(USE_CUDA)
-    file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/video_io/ffmpeg/x86_cuda/*.cpp)
-    list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
-  endif()
   file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/video_io/ffmpeg/*.cpp)
   list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
 endif()
@@ -261,22 +223,10 @@ if(USE_CUDA)  # PLATFORM_X86_CUDA & PLATFORM_JETSON
   target_link_libraries(SpireCVSeg sv_world)
 
 elseif(PLATFORM STREQUAL "X86_INTEL")  # Links to Intel-OpenVINO libraries here
-  # Intel-Openvino
-  include_directories(
-    PUBLIC /opt/intel/openvino_2022/runtime/include/
-    PUBLIC /opt/intel/openvino_2022/runtime/include/ie/
-  )
-  link_directories(
-    ${InferenceEngine_LIBRARIES}
-    /opt/intel/openvino_2022/runtime/lib/intel64/libopenvino.so
-  )
-  
   add_library(sv_world SHARED ${spirecv_SRCS})
   target_link_libraries(
     sv_world ${OpenCV_LIBS} 
     sv_gimbal
-    ${InferenceEngine_LIBRARIES}
-    /opt/intel/openvino_2022/runtime/lib/intel64/libopenvino.so
   )
 endif()
 
@@ -305,8 +255,6 @@ add_executable(SingleObjectTracking samples/demo/single_object_tracking.cpp)
 target_link_libraries(SingleObjectTracking sv_world)
 add_executable(MultipleObjectTracking samples/demo/multiple_object_tracking.cpp)
 target_link_libraries(MultipleObjectTracking sv_world)
-add_executable(EvalMOTMetric samples/demo/eval_MOT_metric.cpp)
-target_link_libraries(EvalMOTMetric -lstdc++fs sv_world)
 add_executable(ColorLineDetection samples/demo/color_line_detect.cpp)
 target_link_libraries(ColorLineDetection sv_world)
 add_executable(UdpDetectionInfoReceiver samples/demo/udp_detection_info_receiver.cpp)
@@ -315,8 +263,6 @@ add_executable(UdpDetectionInfoSender samples/demo/udp_detection_info_sender.cpp
 target_link_libraries(UdpDetectionInfoSender sv_world)
 add_executable(VideoSaving samples/demo/video_saving.cpp)
 target_link_libraries(VideoSaving sv_world)
-add_executable(VERI samples/demo/veri.cpp)
-target_link_libraries(VERI sv_world)
 add_executable(VideoStreaming samples/demo/video_streaming.cpp)
 target_link_libraries(VideoStreaming sv_world)
 add_executable(GimbalClickedTracking samples/demo/gimbal_detection_with_clicked_tracking.cpp)
@@ -325,21 +271,20 @@ add_executable(GimbalLandingMarkerDetection samples/demo/gimbal_landing_marker_d
 target_link_libraries(GimbalLandingMarkerDetection sv_world)
 add_executable(GimbalUdpDetectionInfoSender samples/demo/gimbal_udp_detection_info_sender.cpp)
 target_link_libraries(GimbalUdpDetectionInfoSender sv_world)
+add_executable(ArucoDetectionWithSingleObjectTracking samples/demo/aruco_detection_with_single_object_tracking.cpp)
+target_link_libraries(ArucoDetectionWithSingleObjectTracking sv_world)
+add_executable(CarDetectionWithTracking samples/demo/car_detection_with_tracking.cpp)
+target_link_libraries(CarDetectionWithTracking sv_world)
 
 add_executable(EvalFpsOnVideo samples/test/eval_fps_on_video.cpp)
 target_link_libraries(EvalFpsOnVideo sv_world)
 
-add_executable(GimbalTest samples/test/gimbal_test.cpp)
-target_link_libraries(GimbalTest sv_world)
-
 add_executable(EvalModelOnCocoVal samples/test/eval_mAP_on_coco_val/eval_mAP_on_coco_val.cpp)
 target_link_libraries(EvalModelOnCocoVal sv_world)
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/samples/calib)
 add_executable(CameraCalibrarion samples/calib/calibrate_camera_charuco.cpp)
-target_link_libraries(CameraCalibrarion ${OpenCV_LIBS} sv_world)
+target_link_libraries(CameraCalibrarion ${OpenCV_LIBS})
-add_executable(CreateMarker samples/calib/create_marker.cpp)
-target_link_libraries(CreateMarker ${OpenCV_LIBS})
 
 message(STATUS "CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
 if (NOT DEFINED SV_INSTALL_PREFIX)
@@ -358,7 +303,7 @@ if(USE_CUDA)
     RUNTIME DESTINATION bin
   )
 elseif(PLATFORM STREQUAL "X86_INTEL")
-  install(TARGETS sv_gimbal sv_world
+  install(TARGETS sv_world
     LIBRARY DESTINATION lib
   )
 endif()

README.en.md

@@ -38,9 +38,9 @@ SpireCV is an **real-time edge perception SDK** built for **intelligent unmanned
  - **Platform level**：
     - [x] X86 + Nvidia GPUs (10 series, 20 series, and 30 series graphics cards recommended)
     - [x] Jetson (AGX Orin/Xavier、Orin NX/Nano、Xavier NX)
-    - [x] Intel CPU
+    - [ ] Intel CPU (coming soon)
-    - [ ] HUAWEI Ascend (coming soon)
     - [ ] Rockchip (coming soon)
+    - [ ] HUAWEI Ascend (coming soon)
 
 ## Demos
  - **QR code detection**

README.md

@@ -12,7 +12,7 @@ SpireCV是一个专为**智能无人系统**打造的**边缘实时感知SDK**
 
 ## 快速入门
 
- - 安装及使用：[SpireCV使用手册](https://docs.amovlab.com/Spire_CV_Amov/#/)（[wolai版本](https://www.wolai.com/4qWFM6aZmtpQE6jj7hnNMW)）、[SpireCV开发者套件指南](https://docs.amovlab.com/spirecvkit/#/)
+ - 安装及使用：[SpireCV使用手册](https://docs.amovlab.com/Spire_CV_Amov/#/)、[SpireCV开发者套件指南](https://docs.amovlab.com/spirecvkit/#/)
     - 需掌握C++语言基础、CMake编译工具基础。
     - 需要掌握OpenCV视觉库基础，了解CUDA、OpenVINO、RKNN和CANN等计算库。
     - 需要了解ROS基本概念及基本操作。
@@ -38,9 +38,9 @@ SpireCV是一个专为**智能无人系统**打造的**边缘实时感知SDK**
  - **平台层**：
     - [x] X86+Nvidia GPU（推荐10系、20系、30系显卡）
     - [x] Jetson（AGX Orin/Xavier、Orin NX/Nano、Xavier NX）
-    - [x] Intel CPU
+    - [ ] Intel CPU（推进中）
-    - [ ] HUAWEI Ascend（推进中）
     - [ ] Rockchip（推进中）
+    - [ ] HUAWEI Ascend（推进中）
 
 ## 功能展示
  - **二维码检测**

algorithm/common_det/cuda/common_det_cuda_impl.cpp

@@ -1,8 +1,6 @@
 #include "common_det_cuda_impl.h"
 #include <cmath>
 #include <fstream>
-#include <iostream>
-#include "sv_util.h"
 
 #define SV_MODEL_DIR "/SpireCV/models/"
 #define SV_ROOT_DIR "/SpireCV/"
@@ -105,7 +103,7 @@ void infer_seg(IExecutionContext& context, cudaStream_t& stream, void **buffers,
   CUDA_CHECK(cudaMemcpyAsync(output2, buffers[2], batchSize * kOutputSize2 * sizeof(float), cudaMemcpyDeviceToHost, stream));
   cudaStreamSynchronize(stream);
 }
-void CommonObjectDetectorCUDAImpl::_prepare_buffers(int input_h, int input_w, int batchsize) {
+void CommonObjectDetectorCUDAImpl::_prepare_buffers(int input_h, int input_w) {
   assert(this->_engine->getNbBindings() == 2);
   // In order to bind the buffers, we need to know the names of the input and output tensors.
   // Note that indices are guaranteed to be less than IEngine::getNbBindings()
@@ -114,12 +112,12 @@ void CommonObjectDetectorCUDAImpl::_prepare_buffers(int input_h, int input_w, in
   assert(inputIndex == 0);
   assert(outputIndex == 1);
   // Create GPU buffers on device
-  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[0]), batchsize * 3 * input_h * input_w * sizeof(float)));
+  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[0]), kBatchSize * 3 * input_h * input_w * sizeof(float)));
-  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[1]), batchsize * kOutputSize * sizeof(float)));
+  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[1]), kBatchSize * kOutputSize * sizeof(float)));
 
-  this->_cpu_output_buffer = new float[batchsize * kOutputSize];
+  this->_cpu_output_buffer = new float[kBatchSize * kOutputSize];
 }
-void CommonObjectDetectorCUDAImpl::_prepare_buffers_seg(int input_h, int input_w, int batchsize) {
+void CommonObjectDetectorCUDAImpl::_prepare_buffers_seg(int input_h, int input_w) {
   assert(this->_engine->getNbBindings() == 3);
   // In order to bind the buffers, we need to know the names of the input and output tensors.
   // Note that indices are guaranteed to be less than IEngine::getNbBindings()
@@ -131,13 +129,13 @@ void CommonObjectDetectorCUDAImpl::_prepare_buffers_seg(int input_h, int input_w
   assert(outputIndex2 == 2);
 
   // Create GPU buffers on device
-  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[0]), batchsize * 3 * input_h * input_w * sizeof(float)));
+  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[0]), kBatchSize * 3 * input_h * input_w * sizeof(float)));
-  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[1]), batchsize * kOutputSize1 * sizeof(float)));
+  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[1]), kBatchSize * kOutputSize1 * sizeof(float)));
-  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[2]), batchsize * kOutputSize2 * sizeof(float)));
+  CUDA_CHECK(cudaMalloc((void**)&(this->_gpu_buffers[2]), kBatchSize * kOutputSize2 * sizeof(float)));
 
   // Alloc CPU buffers
-  this->_cpu_output_buffer1 = new float[batchsize * kOutputSize1];
+  this->_cpu_output_buffer1 = new float[kBatchSize * kOutputSize1];
-  this->_cpu_output_buffer2 = new float[batchsize * kOutputSize2];
+  this->_cpu_output_buffer2 = new float[kBatchSize * kOutputSize2];
 }
 void deserialize_engine(std::string& engine_name, IRuntime** runtime, ICudaEngine** engine, IExecutionContext** context) {
   std::ifstream file(engine_name, std::ios::binary);
@@ -174,8 +172,7 @@ void CommonObjectDetectorCUDAImpl::cudaDetect(
   std::vector<float>& boxes_h_,
   std::vector<int>& boxes_label_,
   std::vector<float>& boxes_score_,
-  std::vector<cv::Mat>& boxes_seg_,
+  std::vector<cv::Mat>& boxes_seg_
-  bool input_4k_
 )
 {
 #ifdef WITH_CUDA
@@ -186,51 +183,9 @@ void CommonObjectDetectorCUDAImpl::cudaDetect(
   double thrs_nms = base_->getThrsNms();
 
   std::vector<cv::Mat> img_batch;
-  if (input_4k_)
+  img_batch.push_back(img_);
-  {
+  // Preprocess
-    if (img_.cols == 3840 && img_.rows == 2160)
+  cuda_batch_preprocess(img_batch, this->_gpu_buffers[0], input_w, input_h, this->_stream);
-    {
-      cv::Mat patch1, patch2, patch3, patch4, patch5, patch6;
-      
-      img_.colRange(200, 1480).rowRange(0, 1280).copyTo(patch1);
-      img_.colRange(1280, 2560).rowRange(0, 1280).copyTo(patch2);
-      img_.colRange(2360, 3640).rowRange(0, 1280).copyTo(patch3);
-      
-      img_.colRange(200, 1480).rowRange(880, 2160).copyTo(patch4);
-      img_.colRange(1280, 2560).rowRange(880, 2160).copyTo(patch5);
-      img_.colRange(2360, 3640).rowRange(880, 2160).copyTo(patch6);
-      
-      img_batch.push_back(patch1);
-      img_batch.push_back(patch2);
-      img_batch.push_back(patch3);
-      img_batch.push_back(patch4);
-      img_batch.push_back(patch5);
-      img_batch.push_back(patch6);
-    }
-    else
-    {
-      throw std::runtime_error("SpireCV (106) Input image is NOT 4K (3840 x 2160)!");
-    }
-    if (with_segmentation)
-    {
-      throw std::runtime_error("SpireCV (106) Resolution 4K DO NOT Support Segmentation!");
-    }
-  }
-  else
-  {
-    img_batch.push_back(img_);
-  }
-  
-  if (input_4k_)
-  {
-    // Preprocess
-    cuda_batch_preprocess(img_batch, this->_gpu_buffers[0], 1280, 1280, this->_stream);
-  }
-  else
-  {
-    // Preprocess
-    cuda_batch_preprocess(img_batch, this->_gpu_buffers[0], input_w, input_h, this->_stream);
-  }
 
   // Run inference
   if (with_segmentation)
@@ -239,14 +194,7 @@ void CommonObjectDetectorCUDAImpl::cudaDetect(
   }
   else
   {
-    if (input_4k_)
+    infer(*this->_context, this->_stream, (void**)this->_gpu_buffers, this->_cpu_output_buffer, kBatchSize);
-    {
-      infer(*this->_context, this->_stream, (void**)this->_gpu_buffers, this->_cpu_output_buffer, 6);
-    }
-    else
-    {
-      infer(*this->_context, this->_stream, (void**)this->_gpu_buffers, this->_cpu_output_buffer, kBatchSize);
-    }
   }
 
   // NMS
@@ -260,102 +208,45 @@ void CommonObjectDetectorCUDAImpl::cudaDetect(
     batch_nms(res_batch, this->_cpu_output_buffer, img_batch.size(), kOutputSize, thrs_conf, thrs_nms);
   }
 
-
+  std::vector<Detection> res = res_batch[0];
-  if (input_4k_)
+  std::vector<cv::Mat> masks;
+  if (with_segmentation)
   {
-    for (size_t k = 0; k < res_batch.size(); k++)
+    masks = process_mask(&(this->_cpu_output_buffer2[0]), kOutputSize2, res, input_h, input_w);
-    {
+  }
-      std::vector<Detection> res = res_batch[k];
-      for (size_t j = 0; j < res.size(); j++)
-      {
-        cv::Rect r = get_rect(img_batch[k], res[j].bbox, 1280, 1280);
-        if (r.x < 0) r.x = 0;
-        if (r.y < 0) r.y = 0;
-        if (r.x + r.width >= 1280)  r.width = 1280 - r.x - 1;
-        if (r.y + r.height >= 1280) r.height = 1280 - r.y - 1;
-        if (r.width > 3 && r.height > 3)
-        {
-          if (0 == k)
-          {
-            boxes_x_.push_back(r.x + 200);
-            boxes_y_.push_back(r.y);
-          }
-          else if (1 == k)
-          {
-            boxes_x_.push_back(r.x + 1280);
-            boxes_y_.push_back(r.y);
-          }
-          else if (2 == k)
-          {
-            boxes_x_.push_back(r.x + 2360);
-            boxes_y_.push_back(r.y);
-          }
-          else if (3 == k)
-          {
-            boxes_x_.push_back(r.x + 200);
-            boxes_y_.push_back(r.y + 880);
-          }
-          else if (4 == k)
-          {
-            boxes_x_.push_back(r.x + 1280);
-            boxes_y_.push_back(r.y + 880);
-          }
-          else if (5 == k)
-          {
-            boxes_x_.push_back(r.x + 2360);
-            boxes_y_.push_back(r.y + 880);
-          }
-          boxes_w_.push_back(r.width);
-          boxes_h_.push_back(r.height);
 
-          boxes_label_.push_back((int)res[j].class_id);
+
-          boxes_score_.push_back(res[j].conf);
+
-        }
+  for (size_t j = 0; j < res.size(); j++) {
+    cv::Rect r = get_rect(img_, res[j].bbox, input_h, input_w);
+    if (r.x < 0) r.x = 0;
+    if (r.y < 0) r.y = 0;
+    if (r.x + r.width >= img_.cols)  r.width = img_.cols - r.x - 1;
+    if (r.y + r.height >= img_.rows) r.height = img_.rows - r.y - 1;
+    if (r.width > 5 && r.height > 5)
+    {
+      // cv::rectangle(img_show, r, cv::Scalar(0, 0, 255), 2);
+      // cv::putText(img_show, vehiclenames[(int)res[j].class_id], cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 2.2, cv::Scalar(0, 0, 255), 2);
+      boxes_x_.push_back(r.x);
+      boxes_y_.push_back(r.y);
+      boxes_w_.push_back(r.width);
+      boxes_h_.push_back(r.height);
+
+      boxes_label_.push_back((int)res[j].class_id);
+      boxes_score_.push_back(res[j].conf);
+
+      if (with_segmentation)
+      {
+        cv::Mat mask_j = masks[j].clone();
+        boxes_seg_.push_back(mask_j);
       }
     }
   }
-  else
-  {
-
-    std::vector<Detection> res = res_batch[0];
-    std::vector<cv::Mat> masks;
-    if (with_segmentation)
-    {
-      masks = process_mask(&(this->_cpu_output_buffer2[0]), kOutputSize2, res, input_h, input_w);
-    }
-
-    for (size_t j = 0; j < res.size(); j++)
-    {
-      cv::Rect r = get_rect(img_, res[j].bbox, input_h, input_w);
-      if (r.x < 0) r.x = 0;
-      if (r.y < 0) r.y = 0;
-      if (r.x + r.width >= img_.cols)  r.width = img_.cols - r.x - 1;
-      if (r.y + r.height >= img_.rows) r.height = img_.rows - r.y - 1;
-      if (r.width > 5 && r.height > 5)
-      {
-        // cv::rectangle(img_show, r, cv::Scalar(0, 0, 255), 2);
-        // cv::putText(img_show, vehiclenames[(int)res[j].class_id], cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 2.2, cv::Scalar(0, 0, 255), 2);
-        boxes_x_.push_back(r.x);
-        boxes_y_.push_back(r.y);
-        boxes_w_.push_back(r.width);
-        boxes_h_.push_back(r.height);
-
-        boxes_label_.push_back((int)res[j].class_id);
-        boxes_score_.push_back(res[j].conf);
-
-        if (with_segmentation)
-        {
-          cv::Mat mask_j = masks[j].clone();
-          boxes_seg_.push_back(mask_j);
-        }
-      }
-    }
   
-  }
 #endif
 }
 
-bool CommonObjectDetectorCUDAImpl::cudaSetup(CommonObjectDetectorBase* base_, bool input_4k_)
+bool CommonObjectDetectorCUDAImpl::cudaSetup(CommonObjectDetectorBase* base_)
 {
 #ifdef WITH_CUDA
   std::string dataset = base_->getDataset();
@@ -364,62 +255,23 @@ bool CommonObjectDetectorCUDAImpl::cudaSetup(CommonObjectDetectorBase* base_, bo
   bool with_segmentation = base_->withSegmentation();
   double thrs_conf = base_->getThrsConf();
   double thrs_nms = base_->getThrsNms();
-  std::string model = base_->getModel();
-  int bs = base_->getBatchSize();
-  char bs_c[8];
-  sprintf(bs_c, "%d", bs);
-  std::string bs_s(bs_c);
 
   std::string engine_fn = get_home() + SV_MODEL_DIR + dataset + ".engine";
-  std::vector<std::string> files;
-  
-  _list_dir(get_home() + SV_MODEL_DIR, files, "-online.engine", "Nv-" + dataset + "-yolov5" + model + "_b" + bs_s + "_c");
-  if (files.size() > 0)
-  {
-    std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-    engine_fn = get_home() + SV_MODEL_DIR + files[0];
-  }
-
   if (input_w == 1280)
   {
-    files.clear();
+    engine_fn = get_home() + SV_MODEL_DIR + dataset + "_HD.engine";
-    _list_dir(get_home() + SV_MODEL_DIR, files, "-online.engine", "Nv-" + dataset + "-yolov5" + model + "6_b" + bs_s + "_c");
-    if (files.size() > 0)
-    {
-      std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-      engine_fn = get_home() + SV_MODEL_DIR + files[0];
-    }
-    else
-    {
-      engine_fn = get_home() + SV_MODEL_DIR + dataset + "_HD.engine";
-    }
   }
   if (with_segmentation)
   {
     base_->setInputH(640);
     base_->setInputW(640);
-    files.clear();
+    engine_fn = get_home() + SV_MODEL_DIR + dataset + "_SEG.engine";
-    _list_dir(get_home() + SV_MODEL_DIR, files, "-online.engine", "Nv-" + dataset + "-yolov5" + model + "_seg_b" + bs_s + "_c");
-    if (files.size() > 0)
-    {
-      std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-      engine_fn = get_home() + SV_MODEL_DIR + files[0];
-    }
-    else
-    {
-      engine_fn = get_home() + SV_MODEL_DIR + dataset + "_SEG.engine";
-    }
   }
   std::cout << "Load: " << engine_fn << std::endl;
   if (!is_file_exist(engine_fn))
   {
     throw std::runtime_error("SpireCV (104) Error loading the CommonObject TensorRT model (File Not Exist)");
   }
-  
-  if (input_4k_ && with_segmentation)
-  {
-    throw std::runtime_error("SpireCV (106) Resolution 4K DO NOT Support Segmentation!");
-  }
 
   deserialize_engine(engine_fn, &this->_runtime, &this->_engine, &this->_context);
   CUDA_CHECK(cudaStreamCreate(&this->_stream));
@@ -430,25 +282,29 @@ bool CommonObjectDetectorCUDAImpl::cudaSetup(CommonObjectDetectorBase* base_, bo
   if (with_segmentation)
   {
     // Prepare cpu and gpu buffers
-    this->_prepare_buffers_seg(input_h, input_w, 1);
+    this->_prepare_buffers_seg(input_h, input_w);
   }
   else
   {
-    if (input_4k_)
+    // Prepare cpu and gpu buffers
-    {
+    this->_prepare_buffers(input_h, input_w);
-      // Prepare cpu and gpu buffers
-      this->_prepare_buffers(input_h, input_w, 6);
-    }
-    else
-    {
-      // Prepare cpu and gpu buffers
-      this->_prepare_buffers(input_h, input_w, 1);
-    }
   }
   return true;
 #endif
   return false;
 }
 
+
+
+
+
+
+
+
+
+
+
+
+
 }
 

algorithm/common_det/cuda/common_det_cuda_impl.h

@@ -26,7 +26,7 @@ public:
   CommonObjectDetectorCUDAImpl();
   ~CommonObjectDetectorCUDAImpl();
 
-  bool cudaSetup(CommonObjectDetectorBase* base_, bool input_4k_);
+  bool cudaSetup(CommonObjectDetectorBase* base_);
   void cudaDetect(
     CommonObjectDetectorBase* base_,
     cv::Mat img_,
@@ -36,13 +36,12 @@ public:
     std::vector<float>& boxes_h_,
     std::vector<int>& boxes_label_,
     std::vector<float>& boxes_score_,
-    std::vector<cv::Mat>& boxes_seg_,
+    std::vector<cv::Mat>& boxes_seg_
-    bool input_4k_
   );
 
 #ifdef WITH_CUDA
-  void _prepare_buffers_seg(int input_h, int input_w, int batchsize);
+  void _prepare_buffers_seg(int input_h, int input_w);
-  void _prepare_buffers(int input_h, int input_w, int batchsize);
+  void _prepare_buffers(int input_h, int input_w);
   nvinfer1::IExecutionContext* _context;
   nvinfer1::IRuntime* _runtime;
   nvinfer1::ICudaEngine* _engine;

algorithm/common_det/intel/common_det_intel_impl.cpp

@@ -1,479 +0,0 @@
-#include "common_det_intel_impl.h"
-#include <cmath>
-#include <fstream>
-#include "sv_util.h"
-
-#define SV_MODEL_DIR "/SpireCV/models/"
-#define SV_ROOT_DIR "/SpireCV/"
-
-namespace sv
-{
-
-#ifdef WITH_INTEL
-  using namespace cv;
-  using namespace std;
-  using namespace dnn;
-#endif
-
-  float sigmoid_function(float a)
-  {
-    float b = 1. / (1. + exp(-a));
-    return b;
-  }
-
-  cv::Mat letterbox(cv::Mat &img_, std::vector<float> &paddings)
-  {
-    std::vector<int> new_shape = {640, 640};
-
-    // Get current image shape [height, width]
-    int img_h = img_.rows;
-    int img_w = img_.cols;
-
-    // Compute scale ratio(new / old) and target resized shape
-    float scale = std::min(new_shape[1] * 1.0 / img_h, new_shape[0] * 1.0 / img_w);
-    int resize_h = int(round(img_h * scale));
-    int resize_w = int(round(img_w * scale));
-    paddings[0] = scale;
-
-    // Compute padding
-    int pad_h = new_shape[1] - resize_h;
-    int pad_w = new_shape[0] - resize_w;
-
-    // Resize and pad image while meeting stride-multiple constraints
-    cv::Mat resized_img;
-    cv::resize(img_, resized_img, cv::Size(resize_w, resize_h));
-
-    // divide padding into 2 sides
-    float half_h = pad_h * 1.0 / 2;
-    float half_w = pad_w * 1.0 / 2;
-    paddings[1] = half_h;
-    paddings[2] = half_w;
-
-    // Compute padding boarder
-    int top = int(round(half_h - 0.1));
-    int bottom = int(round(half_h + 0.1));
-    int left = int(round(half_w - 0.1));
-    int right = int(round(half_w + 0.1));
-
-    // Add border
-    cv::copyMakeBorder(resized_img, resized_img, top, bottom, left, right, 0, cv::Scalar(114, 114, 114));
-
-    return resized_img;
-  }
-
-  CommonObjectDetectorIntelImpl::CommonObjectDetectorIntelImpl()
-  {
-  }
-
-  CommonObjectDetectorIntelImpl::~CommonObjectDetectorIntelImpl()
-  {
-  }
-
-  void CommonObjectDetectorIntelImpl::intelDetect(
-      CommonObjectDetectorBase *base_,
-      cv::Mat img_,
-      std::vector<float> &boxes_x_,
-      std::vector<float> &boxes_y_,
-      std::vector<float> &boxes_w_,
-      std::vector<float> &boxes_h_,
-      std::vector<int> &boxes_label_,
-      std::vector<float> &boxes_score_,
-      std::vector<cv::Mat> &boxes_seg_,
-      bool input_4k_)
-  {
-#ifdef WITH_INTEL
-    int input_h = base_->getInputH();
-    int input_w = base_->getInputW();
-    bool with_segmentation = base_->withSegmentation();
-    double thrs_conf = base_->getThrsConf();
-    double thrs_nms = base_->getThrsNms();
-
-    if (with_segmentation)
-    {
-      std::vector<float> paddings(3); // scale, half_h, half_w
-      this->preprocess_img_seg(img_, paddings);
-
-      infer_request.start_async();
-      infer_request.wait();
-
-      // Postprocess
-      this->postprocess_img_seg(img_, paddings, boxes_x_, boxes_y_, boxes_w_, boxes_h_, boxes_label_, boxes_score_, boxes_seg_, thrs_conf, thrs_nms);
-    }
-    else
-    {
-      // Preprocess
-      this->preprocess_img(img_);
-
-      // Run inference
-      infer_request.start_async();
-      infer_request.wait();
-
-      // Postprocess
-      this->postprocess_img(boxes_x_, boxes_y_, boxes_w_, boxes_h_, boxes_label_, boxes_score_, thrs_conf, thrs_nms);
-    }
-
-#endif
-  }
-
-  bool CommonObjectDetectorIntelImpl::intelSetup(CommonObjectDetectorBase *base_, bool input_4k_)
-  {
-#ifdef WITH_INTEL
-    ov::Core core;
-    std::string dataset = base_->getDataset();
-    double thrs_conf = base_->getThrsConf();
-    double thrs_nms = base_->getThrsNms();
-    inpHeight = base_->getInputH();
-    inpWidth = base_->getInputW();
-    with_segmentation = base_->withSegmentation();
-    std::string model = base_->getModel();
-
-    std::string openvino_fn = get_home() + SV_MODEL_DIR + dataset + ".onnx";
-    std::vector<std::string> files;
-    _list_dir(get_home() + SV_MODEL_DIR, files, "-online.onnx", "Int-" + dataset + "-yolov5" + model + "_c");
-    if (files.size() > 0)
-    {
-      std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-      openvino_fn = get_home() + SV_MODEL_DIR + files[0];
-    }
-
-    if (inpWidth == 1280)
-    {
-      files.clear();
-      _list_dir(get_home() + SV_MODEL_DIR, files, "-online.onnx", "Int-" + dataset + "-yolov5" + model + "6_c");
-      if (files.size() > 0)
-      {
-        std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-        openvino_fn = get_home() + SV_MODEL_DIR + files[0];
-      }
-      else
-      {
-        openvino_fn = get_home() + SV_MODEL_DIR + dataset + "_HD.onnx";
-      }
-    }
-    if (with_segmentation)
-    {
-      base_->setInputH(640);
-      base_->setInputW(640);
-      files.clear();
-      _list_dir(get_home() + SV_MODEL_DIR, files, "-online.onnx", "Int-" + dataset + "-yolov5" + model + "_seg_c");
-      if (files.size() > 0)
-      {
-        std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-        openvino_fn = get_home() + SV_MODEL_DIR + files[0];
-      }
-      else
-      {
-        openvino_fn = get_home() + SV_MODEL_DIR + dataset + "_SEG.onnx";
-      }
-    }
-    std::cout << "Load: " << openvino_fn << std::endl;
-    if (!is_file_exist(openvino_fn))
-    {
-      throw std::runtime_error("SpireCV (104) Error loading the CommonObject OpenVINO model (File Not Exist)");
-    }
-
-    if (input_4k_ && with_segmentation)
-    {
-      throw std::runtime_error("SpireCV (106) Resolution 4K DO NOT Support Segmentation!");
-    }
-
-    if (with_segmentation)
-    {
-      this->compiled_model = core.compile_model(openvino_fn, "GPU");
-      this->infer_request = compiled_model.create_infer_request();
-    }
-    else
-    {
-      std::shared_ptr<ov::Model> model_ = core.read_model(openvino_fn);
-      ov::preprocess::PrePostProcessor Pre_P = ov::preprocess::PrePostProcessor(model_);
-      Pre_P.input().tensor().set_element_type(ov::element::u8).set_layout("NHWC").set_color_format(ov::preprocess::ColorFormat::RGB);
-      Pre_P.input().preprocess().convert_element_type(ov::element::f32).convert_color(ov::preprocess::ColorFormat::RGB).scale({255, 255, 255}); // .scale({ 112, 112, 112 });
-      Pre_P.input().model().set_layout("NCHW");
-      Pre_P.output().tensor().set_element_type(ov::element::f32);
-      model_ = Pre_P.build();
-      this->compiled_model = core.compile_model(model_, "GPU");
-      this->infer_request = compiled_model.create_infer_request();
-    }
-    return true;
-#endif
-    return false;
-  }
-
-  void CommonObjectDetectorIntelImpl::preprocess_img(cv::Mat &img_)
-  {
-#ifdef WITH_INTEL
-    float width = img_.cols;
-    float height = img_.rows;
-    cv::Size new_shape = cv::Size(inpHeight, inpWidth);
-    float r = float(new_shape.width / max(width, height));
-    int new_unpadW = int(round(width * r));
-    int new_unpadH = int(round(height * r));
-
-    cv::resize(img_, resize.resized_image, cv::Size(new_unpadW, new_unpadH), 0, 0, cv::INTER_AREA);
-    resize.resized_image = resize.resized_image;
-    resize.dw = new_shape.width - new_unpadW;
-    resize.dh = new_shape.height - new_unpadH;
-    cv::Scalar color = cv::Scalar(100, 100, 100);
-    cv::copyMakeBorder(resize.resized_image, resize.resized_image, 0, resize.dh, 0, resize.dw, cv::BORDER_CONSTANT, color);
-
-    this->rx = (float)img_.cols / (float)(resize.resized_image.cols - resize.dw);
-    this->ry = (float)img_.rows / (float)(resize.resized_image.rows - resize.dh);
-    if (with_segmentation)
-    {
-      cv::Mat blob = cv::dnn::blobFromImage(resize.resized_image, 1 / 255.0, cv::Size(640, 640), cv::Scalar(0, 0, 0), true);
-      auto input_port = compiled_model.input();
-      ov::Tensor input_tensor(input_port.get_element_type(), input_port.get_shape(), blob.ptr(0));
-      infer_request.set_input_tensor(input_tensor);
-    }
-    else
-    {
-      float *input_data = (float *)resize.resized_image.data;
-      input_tensor = ov::Tensor(compiled_model.input().get_element_type(), compiled_model.input().get_shape(), input_data);
-      infer_request.set_input_tensor(input_tensor);
-    }
-#endif
-  }
-
-  void CommonObjectDetectorIntelImpl::preprocess_img_seg(cv::Mat &img_, std::vector<float> &paddings)
-  {
-#ifdef WITH_INTEL
-    cv::Mat masked_img;
-    cv::Mat resized_img = letterbox(img_, paddings); // resize to (640,640) by letterbox
-    // BGR->RGB, u8(0-255)->f32(0.0-1.0), HWC->NCHW
-    cv::Mat blob = cv::dnn::blobFromImage(resized_img, 1 / 255.0, cv::Size(640, 640), cv::Scalar(0, 0, 0), true);
-
-    // Get input port for model with one input
-    auto input_port = compiled_model.input();
-    // Create tensor from external memory
-    ov::Tensor input_tensor(input_port.get_element_type(), input_port.get_shape(), blob.ptr(0));
-    // Set input tensor for model with one input
-    infer_request.set_input_tensor(input_tensor);
-#endif
-  }
-
-  void CommonObjectDetectorIntelImpl::postprocess_img_seg(cv::Mat &img_,
-                                                          std::vector<float> &paddings,
-                                                          std::vector<float> &boxes_x_,
-                                                          std::vector<float> &boxes_y_,
-                                                          std::vector<float> &boxes_w_,
-                                                          std::vector<float> &boxes_h_,
-                                                          std::vector<int> &boxes_label_,
-                                                          std::vector<float> &boxes_score_,
-                                                          std::vector<cv::Mat> &boxes_seg_,
-                                                          double &thrs_conf,
-                                                          double &thrs_nms)
-  {
-#ifdef WITH_INTEL
-    const ov::Tensor &detect = infer_request.get_output_tensor(0);
-    ov::Shape detect_shape = detect.get_shape();
-    const ov::Tensor &proto = infer_request.get_output_tensor(1);
-    ov::Shape proto_shape = proto.get_shape();
-
-    cv::Mat detect_buffer(detect_shape[1], detect_shape[2], CV_32F, detect.data());
-    cv::Mat proto_buffer(proto_shape[1], proto_shape[2] * proto_shape[3], CV_32F, proto.data());
-
-    cv::RNG rng;
-    float conf_threshold = thrs_conf;
-    float nms_threshold = thrs_nms;
-    std::vector<cv::Rect> boxes;
-    std::vector<int> class_ids;
-    std::vector<float> class_scores;
-    std::vector<float> confidences;
-    std::vector<cv::Mat> masks;
-
-    float scale = paddings[0];
-    for (int i = 0; i < detect_buffer.rows; i++)
-    {
-      float confidence = detect_buffer.at<float>(i, 4);
-      if (confidence < conf_threshold)
-      {
-        continue;
-      }
-      cv::Mat classes_scores = detect_buffer.row(i).colRange(5, 85);
-      cv::Point class_id;
-      double score;
-      cv::minMaxLoc(classes_scores, NULL, &score, NULL, &class_id);
-
-      // class score: 0~1
-      if (score > 0.25)
-      {
-        cv::Mat mask = detect_buffer.row(i).colRange(85, 117);
-        float cx = detect_buffer.at<float>(i, 0);
-        float cy = detect_buffer.at<float>(i, 1);
-        float w = detect_buffer.at<float>(i, 2);
-        float h = detect_buffer.at<float>(i, 3);
-        int left = static_cast<int>((cx - 0.5 * w - paddings[2]) / scale);
-        int top = static_cast<int>((cy - 0.5 * h - paddings[1]) / scale);
-        int width = static_cast<int>(w / scale);
-        int height = static_cast<int>(h / scale);
-        cv::Rect box;
-        box.x = left;
-        box.y = top;
-        box.width = width;
-        box.height = height;
-
-        boxes.push_back(box);
-        class_ids.push_back(class_id.x);
-        class_scores.push_back(score);
-        confidences.push_back(confidence);
-        masks.push_back(mask);
-      }
-    }
-
-    // NMS
-    std::vector<int> indices;
-    cv::dnn::NMSBoxes(boxes, confidences, thrs_conf, thrs_nms, indices);
-    // cv::Mat rgb_mask;
-    cv::Mat rgb_mask = cv::Mat::zeros(img_.size(), img_.type());
-
-    for (size_t i = 0; i < indices.size(); i++)
-    {
-      int index = indices[i];
-      int class_id = class_ids[index];
-      cv::Rect box = boxes[index];
-      int x1 = std::max(0, box.x);
-      int y1 = std::max(0, box.y);
-      int x2 = std::max(0, box.br().x);
-      int y2 = std::max(0, box.br().y);
-
-      cv::Mat m = masks[index] * proto_buffer;
-      for (int col = 0; col < m.cols; col++)
-      {
-        m.at<float>(0, col) = sigmoid_function(m.at<float>(0, col));
-      }
-      cv::Mat m1 = m.reshape(1, 160); // 1x25600 -> 160x160
-      int mx1 = std::max(0, int((x1 * scale + paddings[2]) * 0.25));
-      int mx2 = std::max(0, int((x2 * scale + paddings[2]) * 0.25));
-      int my1 = std::max(0, int((y1 * scale + paddings[1]) * 0.25));
-      int my2 = std::max(0, int((y2 * scale + paddings[1]) * 0.25));
-      cv::Mat mask_roi = m1(cv::Range(my1, my2), cv::Range(mx1, mx2));
-
-      cv::Mat rm, det_mask;
-      cv::resize(mask_roi, rm, cv::Size(x2 - x1, y2 - y1));
-      for (int r = 0; r < rm.rows; r++)
-      {
-        for (int c = 0; c < rm.cols; c++)
-        {
-          float pv = rm.at<float>(r, c);
-          if (pv > 0.5)
-          {
-            rm.at<float>(r, c) = 1.0;
-          }
-          else
-          {
-            rm.at<float>(r, c) = 0.0;
-          }
-        }
-      }
-
-      rm = rm * rng.uniform(0, 255);
-      rm.convertTo(det_mask, CV_8UC1);
-      if ((y1 + det_mask.rows) >= img_.rows)
-      {
-        y2 = img_.rows - 1;
-      }
-      if ((x1 + det_mask.cols) >= img_.cols)
-      {
-        x2 = img_.cols - 1;
-      }
-
-      cv::Mat mask = cv::Mat::zeros(cv::Size(img_.cols, img_.rows), CV_8UC1);
-      det_mask(cv::Range(0, y2 - y1), cv::Range(0, x2 - x1)).copyTo(mask(cv::Range(y1, y2), cv::Range(x1, x2)));
-      add(rgb_mask, cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), rgb_mask, mask);
-
-      boxes_x_.push_back(box.x);
-      boxes_y_.push_back(box.y);
-      boxes_w_.push_back(box.width);
-      boxes_h_.push_back(box.height);
-
-      boxes_label_.push_back((int)class_id);
-      boxes_score_.push_back(class_scores[index]);
-
-      cv::Mat mask_j = mask.clone();
-      boxes_seg_.push_back(mask_j);
-    }
-
-#endif
-  }
-
-  void CommonObjectDetectorIntelImpl::postprocess_img(std::vector<float> &boxes_x_,
-                                                      std::vector<float> &boxes_y_,
-                                                      std::vector<float> &boxes_w_,
-                                                      std::vector<float> &boxes_h_,
-                                                      std::vector<int> &boxes_label_,
-                                                      std::vector<float> &boxes_score_,
-                                                      double &thrs_conf,
-                                                      double &thrs_nms)
-  {
-#ifdef WITH_INTEL
-    const ov::Tensor &output_tensor = infer_request.get_output_tensor();
-    ov::Shape output_shape = output_tensor.get_shape();
-    float *detections = output_tensor.data<float>();
-
-    std::vector<cv::Rect> boxes;
-    vector<int> class_ids;
-    vector<float> confidences;
-    for (int i = 0; i < output_shape[1]; i++)
-    {
-      float *detection = &detections[i * output_shape[2]];
-
-      float confidence = detection[4];
-      if (confidence >= thrs_conf)
-      {
-        float *classes_scores = &detection[5];
-        cv::Mat scores(1, output_shape[2] - 5, CV_32FC1, classes_scores);
-        cv::Point class_id;
-        double max_class_score;
-        cv::minMaxLoc(scores, 0, &max_class_score, 0, &class_id);
-        if (max_class_score > thrs_conf)
-        {
-          confidences.push_back(confidence);
-          class_ids.push_back(class_id.x);
-          float x = detection[0];
-          float y = detection[1];
-          float w = detection[2];
-          float h = detection[3];
-          float xmin = x - (w / 2);
-          float ymin = y - (h / 2);
-
-          boxes.push_back(cv::Rect(xmin, ymin, w, h));
-        }
-      }
-    }
-
-    std::vector<int> nms_result;
-    cv::dnn::NMSBoxes(boxes, confidences, thrs_conf, thrs_nms, nms_result);
-
-    std::vector<Detection> output;
-    for (int i = 0; i < nms_result.size(); i++)
-    {
-      Detection result;
-      int idx = nms_result[i];
-      result.class_id = class_ids[idx];
-      result.confidence = confidences[idx];
-      result.box = boxes[idx];
-      output.push_back(result);
-    }
-
-    for (int i = 0; i < output.size(); i++)
-    {
-      auto detection = output[i];
-      auto box = detection.box;
-      auto classId = detection.class_id;
-      auto confidence = detection.confidence;
-
-      float xmax = box.x + box.width;
-      float ymax = box.y + box.height;
-
-      boxes_x_.push_back(this->rx * box.x);
-      boxes_y_.push_back(this->ry * box.y);
-      boxes_w_.push_back(this->rx * box.width);
-      boxes_h_.push_back(this->ry * box.height);
-
-      boxes_label_.push_back((int)detection.class_id);
-      boxes_score_.push_back(detection.confidence);
-    }
-#endif
-  }
-
-}

algorithm/common_det/intel/common_det_intel_impl.h

@@ -1,89 +0,0 @@
-#ifndef __SV_COMMON_DET_INTEL__
-#define __SV_COMMON_DET_INTEL__
-
-#include "sv_core.h"
-#include <string>
-#include <iostream>
-#include <opencv2/dnn.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/highgui.hpp>
-#include <string>
-#include <chrono>
-
-#ifdef WITH_INTEL
-#include <openvino/openvino.hpp>
-#endif
-
-struct Resize
-{
-  cv::Mat resized_image;
-  int dw;
-  int dh;
-};
-
-struct Detection
-{
-  int class_id;
-  float confidence;
-  cv::Rect box;
-};
-
-namespace sv
-{
-
-  class CommonObjectDetectorIntelImpl
-  {
-  public:
-    CommonObjectDetectorIntelImpl();
-    ~CommonObjectDetectorIntelImpl();
-
-    bool intelSetup(CommonObjectDetectorBase *base_, bool input_4k_);
-    void intelDetect(
-        CommonObjectDetectorBase *base_,
-        cv::Mat img_,
-        std::vector<float> &boxes_x_,
-        std::vector<float> &boxes_y_,
-        std::vector<float> &boxes_w_,
-        std::vector<float> &boxes_h_,
-        std::vector<int> &boxes_label_,
-        std::vector<float> &boxes_score_,
-        std::vector<cv::Mat> &boxes_seg_,
-        bool input_4k_);
-    void preprocess_img(cv::Mat &img_);
-    void preprocess_img_seg(cv::Mat &img_, std::vector<float> &paddings);
-    void postprocess_img_seg(cv::Mat &img_,
-                             std::vector<float> &paddings,
-                             std::vector<float> &boxes_x_,
-                             std::vector<float> &boxes_y_,
-                             std::vector<float> &boxes_w_,
-                             std::vector<float> &boxes_h_,
-                             std::vector<int> &boxes_label_,
-                             std::vector<float> &boxes_score_,
-                             std::vector<cv::Mat> &boxes_seg_,
-                             double &thrs_conf,
-                             double &thrs_nms);
-
-    void postprocess_img(std::vector<float> &boxes_x_,
-                         std::vector<float> &boxes_y_,
-                         std::vector<float> &boxes_w_,
-                         std::vector<float> &boxes_h_,
-                         std::vector<int> &boxes_label_,
-                         std::vector<float> &boxes_score_,
-                         double &thrs_conf,
-                         double &thrs_nms);
-
-#ifdef WITH_INTEL
-    int inpWidth;
-    int inpHeight;
-    bool with_segmentation;
-    float rx; // the width ratio of original image and resized image
-    float ry; // the height ratio of original image and resized image
-    Resize resize;
-    ov::Tensor input_tensor;
-    ov::InferRequest infer_request;
-    ov::CompiledModel compiled_model;
-#endif
-  };
-
-}
-#endif

algorithm/common_det/sv_common_det.cpp

@@ -8,24 +8,15 @@
 #include "common_det_cuda_impl.h"
 #endif
 
-#ifdef WITH_INTEL
-#include <openvino/openvino.hpp>
-#include "common_det_intel_impl.h"
-#endif
 
 namespace sv {
 
 
-CommonObjectDetector::CommonObjectDetector(bool input_4k)
+CommonObjectDetector::CommonObjectDetector()
 {
-  this->_input_4k = input_4k;
 #ifdef WITH_CUDA
   this->_cuda_impl = new CommonObjectDetectorCUDAImpl;
 #endif
-
-#ifdef WITH_INTEL
-  this->_intel_impl = new CommonObjectDetectorIntelImpl;
-#endif
 }
 CommonObjectDetector::~CommonObjectDetector()
 {
@@ -34,11 +25,7 @@ CommonObjectDetector::~CommonObjectDetector()
 bool CommonObjectDetector::setupImpl()
 {
 #ifdef WITH_CUDA
-  return this->_cuda_impl->cudaSetup(this, this->_input_4k);
+  return this->_cuda_impl->cudaSetup(this);
-#endif
-
-#ifdef WITH_INTEL
-  return this->_intel_impl->intelSetup(this, this->_input_4k);
 #endif
   return false;
 }
@@ -64,24 +51,14 @@ void CommonObjectDetector::detectImpl(
     boxes_h_,
     boxes_label_,
     boxes_score_,
-    boxes_seg_,
+    boxes_seg_
-    this->_input_4k);
+  );
-#endif
-
-#ifdef WITH_INTEL
-  this->_intel_impl->intelDetect(
-    this,
-    img_,
-    boxes_x_,
-    boxes_y_,
-    boxes_w_,
-    boxes_h_,
-    boxes_label_,
-    boxes_score_,
-    boxes_seg_,
-    this->_input_4k);
 #endif
 }
 
+
+
+
+
 }
 

algorithm/landing_det/cuda/landing_det_cuda_impl.cpp

@@ -1,7 +1,6 @@
 #include "landing_det_cuda_impl.h"
 #include <cmath>
 #include <fstream>
-#include "sv_util.h"
 
 #define SV_MODEL_DIR "/SpireCV/models/"
 #define SV_ROOT_DIR "/SpireCV/"
@@ -51,16 +50,6 @@ bool LandingMarkerDetectorCUDAImpl::cudaSetup()
 {
 #ifdef WITH_CUDA
   std::string trt_model_fn = get_home() + SV_MODEL_DIR + "LandingMarker.engine";
-  
-  std::vector<std::string> files;
-  _list_dir(get_home() + SV_MODEL_DIR, files, "-online.engine", "Nv-LandingMarker-resnet34");
-  if (files.size() > 0)
-  {
-    std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-    trt_model_fn = get_home() + SV_MODEL_DIR + files[0];
-  }
-  std::cout << "Load: " << trt_model_fn << std::endl;
-
   if (!is_file_exist(trt_model_fn))
   {
     throw std::runtime_error("SpireCV (104) Error loading the LandingMarker TensorRT model (File Not Exist)");

algorithm/landing_det/intel/landing_det_intel_impl.cpp

@@ -1,104 +0,0 @@
-#include "landing_det_intel_impl.h"
-#include <cmath>
-#include <fstream>
-#include "sv_util.h"
-
-#define SV_MODEL_DIR "/SpireCV/models/"
-#define SV_ROOT_DIR "/SpireCV/"
-
-namespace sv
-{
-
-#ifdef WITH_INTEL
-  using namespace cv;
-  using namespace std;
-  using namespace dnn;
-#endif
-
-  LandingMarkerDetectorIntelImpl::LandingMarkerDetectorIntelImpl()
-  {
-  }
-
-  LandingMarkerDetectorIntelImpl::~LandingMarkerDetectorIntelImpl()
-  {
-  }
-
-  bool LandingMarkerDetectorIntelImpl::intelSetup()
-  {
-#ifdef WITH_INTEL
-    std::string onnx_model_fn = get_home() + SV_MODEL_DIR + "LandingMarker.onnx";
-    std::vector<std::string> files;
-    list_dir(get_home() + SV_MODEL_DIR, files, "-online.onnx", "Int-LandingMarker-resnet34");
-    if (files.size() > 0)
-    {
-      std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-      onnx_model_fn = get_home() + SV_MODEL_DIR + files[0];
-    }
-
-    if (!is_file_exist(onnx_model_fn))
-    {
-      throw std::runtime_error("SpireCV (104) Error loading the LandingMarker ONNX model (File Not Exist)");
-    }
-
-    // OpenVINO
-    ov::Core core;
-    std::shared_ptr<ov::Model> model = core.read_model(onnx_model_fn);
-    ov::preprocess::PrePostProcessor ppp = ov::preprocess::PrePostProcessor(model);
-    ppp.input().tensor().set_element_type(ov::element::u8).set_layout("NHWC").set_color_format(ov::preprocess::ColorFormat::RGB);
-    ppp.input().preprocess().convert_element_type(ov::element::f32).convert_color(ov::preprocess::ColorFormat::RGB).scale({255, 255, 255}); // .scale({ 112, 112, 112 });
-    ppp.input().model().set_layout("NCHW");
-    ppp.output().tensor().set_element_type(ov::element::f32);
-    model = ppp.build();
-    this->compiled_model = core.compile_model(model, "GPU");
-    this->infer_request = compiled_model.create_infer_request();
-
-    return true;
-#endif
-    return false;
-  }
-
-  void LandingMarkerDetectorIntelImpl::intelRoiCNN(
-      std::vector<cv::Mat> &input_rois_,
-      std::vector<int> &output_labels_)
-  {
-#ifdef WITH_INTEL
-    output_labels_.clear();
-
-    for (int i = 0; i < input_rois_.size(); i++)
-    {
-      cv::Mat e_roi = input_rois_[i];
-
-      // Get input port for model with one input
-      auto input_port = compiled_model.input();
-      // Create tensor from external memory
-      ov::Tensor input_tensor(input_port.get_element_type(), input_port.get_shape(), e_roi.ptr(0));
-      // Set input tensor for model with one input
-      infer_request.set_input_tensor(input_tensor);
-      //preprocess_img(e_roi);
-
-      // infer_request.infer();
-      infer_request.start_async();
-      infer_request.wait();
-
-      const ov::Tensor &output_tensor = infer_request.get_output_tensor();
-      ov::Shape output_shape = output_tensor.get_shape();
-      this->_p_prob = output_tensor.data<float>();
-
-      // Find max index
-      double max = 0;
-      int label = 0;
-      for (int i = 0; i < 11; ++i)
-      {
-        if (max < this->_p_prob[i])
-        {
-          max = this->_p_prob[i];
-          label = i;
-        }
-      }
-      output_labels_.push_back(label);
-    }
-
-#endif
-  }
-
-}

algorithm/landing_det/intel/landing_det_intel_impl.h

@@ -1,37 +0,0 @@
-#ifndef __SV_LANDING_DET_INTEL__
-#define __SV_LANDING_DET_INTEL__
-
-#include "sv_core.h"
-#include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
-#include <opencv2/tracking.hpp>
-#include <string>
-#include <chrono>
-
-#ifdef WITH_INTEL
-#include <openvino/openvino.hpp>
-#endif
-
-namespace sv
-{
-  class LandingMarkerDetectorIntelImpl
-  {
-  public:
-    LandingMarkerDetectorIntelImpl();
-    ~LandingMarkerDetectorIntelImpl();
-
-    bool intelSetup();
-    void intelRoiCNN(
-        std::vector<cv::Mat> &input_rois_,
-        std::vector<int> &output_labels_);
-
-#ifdef WITH_INTEL
-    float *_p_prob;
-
-    ov::Tensor input_tensor;
-    ov::InferRequest infer_request;
-    ov::CompiledModel compiled_model;
-#endif
-  };
-}
-#endif

algorithm/landing_det/sv_landing_det.cpp

@@ -7,10 +7,6 @@
 #include "landing_det_cuda_impl.h"
 #endif
 
-#ifdef WITH_INTEL
-#include <openvino/openvino.hpp>
-#include "landing_det_intel_impl.h"
-#endif
 
 namespace sv {
 
@@ -20,10 +16,6 @@ LandingMarkerDetector::LandingMarkerDetector()
 #ifdef WITH_CUDA
   this->_cuda_impl = new LandingMarkerDetectorCUDAImpl;
 #endif
-
-#ifdef WITH_INTEL
-  this->_intel_impl = new LandingMarkerDetectorIntelImpl;
-#endif
 }
 LandingMarkerDetector::~LandingMarkerDetector()
 {
@@ -34,10 +26,6 @@ bool LandingMarkerDetector::setupImpl()
 #ifdef WITH_CUDA
   return this->_cuda_impl->cudaSetup();
 #endif
-
-#ifdef WITH_INTEL
-  return this->_intel_impl->intelSetup();
-#endif
   return false;
 }
 
@@ -52,13 +40,11 @@ void LandingMarkerDetector::roiCNN(
     output_labels_
   );
 #endif
-
+}
-#ifdef WITH_INTEL
+
-  this->_intel_impl->intelRoiCNN(
+
-    input_rois_,
+
-    output_labels_
+
-  );
+
-#endif
-}
 }
 

algorithm/mot/bytetrack/BYTETracker.cpp

@@ -1,262 +0,0 @@
-
-#include "BYTETracker.h"
-#include <fstream>
-
-namespace sv
-{
-
-	BYTETracker::BYTETracker(int frame_rate, int track_buffer)
-	{
-		track_thresh = 0.5;
-		high_thresh = 0.6;
-		match_thresh = 0.8;
-
-		frame_id = 0;
-		max_time_lost = int(frame_rate / 30.0 * track_buffer);
-	}
-
-	BYTETracker::~BYTETracker()
-	{
-	}
-
-	void BYTETracker::update(TargetsInFrame &tgts)
-	{
-		////////////////// Step 1: Get detections //////////////////
-		this->frame_id++;
-		std::vector<STrack> activated_stracks;
-		std::vector<STrack> refind_stracks;
-		std::vector<STrack> removed_stracks;
-		std::vector<STrack> lost_stracks;
-		std::vector<STrack> detections;
-		std::vector<STrack> detections_low;
-
-		std::vector<STrack> detections_cp;
-		std::vector<STrack> tracked_stracks_swap;
-		std::vector<STrack> resa, resb;
-		std::vector<STrack> output_stracks;
-
-		std::vector<STrack *> unconfirmed;
-		std::vector<STrack *> tracked_stracks;
-		std::vector<STrack *> strack_pool;
-		std::vector<STrack *> r_tracked_stracks;
-
-		if (tgts.targets.size() > 0)
-		{
-			for (int i = 0; i < tgts.targets.size(); i++)
-			{
-
-				tgts.targets[i].tracked_id = 0;
-				tgts.targets[i].has_tid = true;
-
-				std::vector<float> tlbr_;
-				tlbr_.resize(4);
-				tlbr_[0] = tgts.targets[i].cx * tgts.width - tgts.targets[i].w * tgts.width / 2;
-				tlbr_[1] = tgts.targets[i].cy * tgts.height - tgts.targets[i].h * tgts.height / 2;
-				tlbr_[2] = tgts.targets[i].cx * tgts.width + tgts.targets[i].w * tgts.width / 2;
-				tlbr_[3] = tgts.targets[i].cy * tgts.height + tgts.targets[i].h * tgts.height / 2;
-
-				float score = tgts.targets[i].score;
-
-				STrack strack(STrack::tlbr_to_tlwh(tlbr_), score);
-				if (score >= track_thresh)
-				{
-					detections.push_back(strack);
-				}
-				else
-				{
-					detections_low.push_back(strack);
-				}
-			}
-		}
-
-		// Add newly detected tracklets to tracked_stracks
-		for (int i = 0; i < this->tracked_stracks.size(); i++)
-		{
-			if (!this->tracked_stracks[i].is_activated)
-				unconfirmed.push_back(&this->tracked_stracks[i]);
-			else
-				tracked_stracks.push_back(&this->tracked_stracks[i]);
-		}
-
-		////////////////// Step 2: First association, with IoU //////////////////
-		strack_pool = joint_stracks(tracked_stracks, this->lost_stracks);
-		STrack::multi_predict(strack_pool, this->kalman_filter);
-
-		std::vector<std::vector<float>> dists;
-		int dist_size = 0, dist_size_size = 0;
-		dists = iou_distance(strack_pool, detections, dist_size, dist_size_size);
-
-		std::vector<std::vector<int>> matches;
-		std::vector<int> u_track, u_detection;
-		linear_assignment(dists, dist_size, dist_size_size, match_thresh, matches, u_track, u_detection);
-
-		for (int i = 0; i < matches.size(); i++)
-		{
-			STrack *track = strack_pool[matches[i][0]];
-			STrack *det = &detections[matches[i][1]];
-			if (track->state == TrackState::Tracked)
-			{
-				track->update(*det, this->frame_id);
-				activated_stracks.push_back(*track);
-			}
-			else
-			{
-				track->re_activate(*det, this->frame_id, false);
-				refind_stracks.push_back(*track);
-			}
-		}
-
-		////////////////// Step 3: Second association, using low score dets //////////////////
-		for (int i = 0; i < u_detection.size(); i++)
-		{
-			detections_cp.push_back(detections[u_detection[i]]);
-		}
-		detections.clear();
-		detections.assign(detections_low.begin(), detections_low.end());
-
-		for (int i = 0; i < u_track.size(); i++)
-		{
-			if (strack_pool[u_track[i]]->state == TrackState::Tracked)
-			{
-				r_tracked_stracks.push_back(strack_pool[u_track[i]]);
-			}
-		}
-
-		dists.clear();
-		dists = iou_distance(r_tracked_stracks, detections, dist_size, dist_size_size);
-
-		matches.clear();
-		u_track.clear();
-		u_detection.clear();
-		linear_assignment(dists, dist_size, dist_size_size, 0.5, matches, u_track, u_detection);
-
-		for (int i = 0; i < matches.size(); i++)
-		{
-			STrack *track = r_tracked_stracks[matches[i][0]];
-			STrack *det = &detections[matches[i][1]];
-			if (track->state == TrackState::Tracked)
-			{
-				track->update(*det, this->frame_id);
-				activated_stracks.push_back(*track);
-			}
-			else
-			{
-				track->re_activate(*det, this->frame_id, false);
-				refind_stracks.push_back(*track);
-			}
-		}
-
-		for (int i = 0; i < u_track.size(); i++)
-		{
-			STrack *track = r_tracked_stracks[u_track[i]];
-			if (track->state != TrackState::Lost)
-			{
-				track->mark_lost();
-				lost_stracks.push_back(*track);
-			}
-		}
-
-		// Deal with unconfirmed tracks, usually tracks with only one beginning frame
-		detections.clear();
-		detections.assign(detections_cp.begin(), detections_cp.end());
-
-		dists.clear();
-		dists = iou_distance(unconfirmed, detections, dist_size, dist_size_size);
-
-		matches.clear();
-		std::vector<int> u_unconfirmed;
-		u_detection.clear();
-		linear_assignment(dists, dist_size, dist_size_size, 0.7, matches, u_unconfirmed, u_detection);
-
-		for (int i = 0; i < matches.size(); i++)
-		{
-			unconfirmed[matches[i][0]]->update(detections[matches[i][1]], this->frame_id);
-			activated_stracks.push_back(*unconfirmed[matches[i][0]]);
-		}
-
-		for (int i = 0; i < u_unconfirmed.size(); i++)
-		{
-			STrack *track = unconfirmed[u_unconfirmed[i]];
-			track->mark_removed();
-			removed_stracks.push_back(*track);
-		}
-
-		////////////////// Step 4: Init new stracks //////////////////
-		for (int i = 0; i < u_detection.size(); i++)
-		{
-			STrack *track = &detections[u_detection[i]];
-			if (track->score < this->high_thresh)
-				continue;
-			track->activate(this->kalman_filter, this->frame_id);
-			activated_stracks.push_back(*track);
-		}
-
-		////////////////// Step 5: Update state //////////////////
-		for (int i = 0; i < this->lost_stracks.size(); i++)
-		{
-			if (this->frame_id - this->lost_stracks[i].end_frame() > this->max_time_lost)
-			{
-				this->lost_stracks[i].mark_removed();
-				removed_stracks.push_back(this->lost_stracks[i]);
-			}
-		}
-
-		for (int i = 0; i < this->tracked_stracks.size(); i++)
-		{
-			if (this->tracked_stracks[i].state == TrackState::Tracked)
-			{
-				tracked_stracks_swap.push_back(this->tracked_stracks[i]);
-			}
-		}
-		this->tracked_stracks.clear();
-		this->tracked_stracks.assign(tracked_stracks_swap.begin(), tracked_stracks_swap.end());
-
-		this->tracked_stracks = joint_stracks(this->tracked_stracks, activated_stracks);
-		this->tracked_stracks = joint_stracks(this->tracked_stracks, refind_stracks);
-
-		// std::cout << activated_stracks.size() << std::endl;
-
-		this->lost_stracks = sub_stracks(this->lost_stracks, this->tracked_stracks);
-		for (int i = 0; i < lost_stracks.size(); i++)
-		{
-			this->lost_stracks.push_back(lost_stracks[i]);
-		}
-
-		this->lost_stracks = sub_stracks(this->lost_stracks, this->removed_stracks);
-		for (int i = 0; i < removed_stracks.size(); i++)
-		{
-			this->removed_stracks.push_back(removed_stracks[i]);
-		}
-
-		remove_duplicate_stracks(resa, resb, this->tracked_stracks, this->lost_stracks);
-
-		this->tracked_stracks.clear();
-		this->tracked_stracks.assign(resa.begin(), resa.end());
-		this->lost_stracks.clear();
-		this->lost_stracks.assign(resb.begin(), resb.end());
-
-		for (int i = 0; i < this->tracked_stracks.size(); i++)
-		{
-			if (this->tracked_stracks[i].is_activated)
-			{
-				output_stracks.push_back(this->tracked_stracks[i]);
-			}
-		}
-
-		tgts.targets.clear();
-
-		for (unsigned long i = 0; i < output_stracks.size(); i++)
-		{
-			std::vector<float> tlwh = output_stracks[i].tlwh;
-			bool vertical = tlwh[2] / tlwh[3] > 1.6;
-
-			if (tlwh[2] * tlwh[3] > 20 && !vertical)
-			{
-				Target tgt;
-				tgt.setBox(tlwh[0], tlwh[1], tlwh[0] + tlwh[2], tlwh[1] + tlwh[3], tgts.width, tgts.height);
-				tgt.setTrackID(output_stracks[i].track_id);
-				tgts.targets.push_back(tgt);
-			}
-		}
-	}
-}

algorithm/mot/bytetrack/BYTETracker.h

@@ -1,56 +0,0 @@
-#ifndef __SV_BYTETrack__
-#define __SV_BYTETrack__
-
-#include "sv_core.h"
-#include "STrack.h"
-
-class detect_result
-{
-public:
-    int classId;
-    float confidence;
-    cv::Rect_<float> box;
-};
-
-namespace sv {
-
-class BYTETracker
-{
-public:
-	BYTETracker(int frame_rate = 30, int track_buffer = 30);
-	~BYTETracker();
-
-	void  update(TargetsInFrame &tgts);
-    cv::Scalar get_color(int idx);
-
-private:
-	 std::vector<STrack*> joint_stracks( std::vector<STrack*> &tlista,  std::vector<STrack> &tlistb);
-	 std::vector<STrack> joint_stracks( std::vector<STrack> &tlista,  std::vector<STrack> &tlistb);
-
-	 std::vector<STrack> sub_stracks( std::vector<STrack> &tlista,  std::vector<STrack> &tlistb);
-	void remove_duplicate_stracks( std::vector<STrack> &resa,  std::vector<STrack> &resb,  std::vector<STrack> &stracksa,  std::vector<STrack> &stracksb);
-
-	void linear_assignment( std::vector< std::vector<float> > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh,
-		 std::vector< std::vector<int> > &matches,  std::vector<int> &unmatched_a,  std::vector<int> &unmatched_b);
-	 std::vector< std::vector<float> > iou_distance( std::vector<STrack*> &atracks,  std::vector<STrack> &btracks, int &dist_size, int &dist_size_size);
-	 std::vector< std::vector<float> > iou_distance( std::vector<STrack> &atracks,  std::vector<STrack> &btracks);
-	 std::vector< std::vector<float> > ious( std::vector< std::vector<float> > &atlbrs,  std::vector< std::vector<float> > &btlbrs);
-
-	double lapjv(const  std::vector< std::vector<float> > &cost,  std::vector<int> &rowsol,  std::vector<int> &colsol, 
-		bool extend_cost = false, float cost_limit = LONG_MAX, bool return_cost = true);
-
-private:
-
-	float track_thresh;
-	float high_thresh;
-	float match_thresh;
-	int frame_id;
-	int max_time_lost;
-
-	std::vector<STrack> tracked_stracks;
-	std::vector<STrack> lost_stracks;
-	std::vector<STrack> removed_stracks;
-	byte_kalman::ByteKalmanFilter kalman_filter;
-};
-}
-#endif

algorithm/mot/bytetrack/BytekalmanFilter.cpp

@@ -1,152 +0,0 @@
-#include "BytekalmanFilter.h"
-#include <Eigen/Cholesky>
-
-namespace byte_kalman
-{
-	const double ByteKalmanFilter::chi2inv95[10] = {
-	0,
-	3.8415,
-	5.9915,
-	7.8147,
-	9.4877,
-	11.070,
-	12.592,
-	14.067,
-	15.507,
-	16.919
-	};
-	ByteKalmanFilter::ByteKalmanFilter()
-	{
-		int ndim = 4;
-		double dt = 1.;
-
-		_motion_mat = Eigen::MatrixXf::Identity(8, 8);
-		for (int i = 0; i < ndim; i++) {
-			_motion_mat(i, ndim + i) = dt;
-		}
-		_update_mat = Eigen::MatrixXf::Identity(4, 8);
-
-		this->_std_weight_position = 1. / 20;
-		this->_std_weight_velocity = 1. / 160;
-	}
-
-	KAL_DATA ByteKalmanFilter::initiate(const DETECTBOX &measurement)
-	{
-		DETECTBOX mean_pos = measurement;
-		DETECTBOX mean_vel;
-		for (int i = 0; i < 4; i++) mean_vel(i) = 0;
-
-		KAL_MEAN mean;
-		for (int i = 0; i < 8; i++) {
-			if (i < 4) mean(i) = mean_pos(i);
-			else mean(i) = mean_vel(i - 4);
-		}
-
-		KAL_MEAN std;
-		std(0) = 2 * _std_weight_position * measurement[3];
-		std(1) = 2 * _std_weight_position * measurement[3];
-		std(2) = 1e-2;
-		std(3) = 2 * _std_weight_position * measurement[3];
-		std(4) = 10 * _std_weight_velocity * measurement[3];
-		std(5) = 10 * _std_weight_velocity * measurement[3];
-		std(6) = 1e-5;
-		std(7) = 10 * _std_weight_velocity * measurement[3];
-
-		KAL_MEAN tmp = std.array().square();
-		KAL_COVA var = tmp.asDiagonal();
-		return std::make_pair(mean, var);
-	}
-
-	void ByteKalmanFilter::predict(KAL_MEAN &mean, KAL_COVA &covariance)
-	{
-		//revise the data;
-		DETECTBOX std_pos;
-		std_pos << _std_weight_position * mean(3),
-			_std_weight_position * mean(3),
-			1e-2,
-			_std_weight_position * mean(3);
-		DETECTBOX std_vel;
-		std_vel << _std_weight_velocity * mean(3),
-			_std_weight_velocity * mean(3),
-			1e-5,
-			_std_weight_velocity * mean(3);
-		KAL_MEAN tmp;
-		tmp.block<1, 4>(0, 0) = std_pos;
-		tmp.block<1, 4>(0, 4) = std_vel;
-		tmp = tmp.array().square();
-		KAL_COVA motion_cov = tmp.asDiagonal();
-		KAL_MEAN mean1 = this->_motion_mat * mean.transpose();
-		KAL_COVA covariance1 = this->_motion_mat * covariance *(_motion_mat.transpose());
-		covariance1 += motion_cov;
-
-		mean = mean1;
-		covariance = covariance1;
-	}
-
-	KAL_HDATA ByteKalmanFilter::project(const KAL_MEAN &mean, const KAL_COVA &covariance)
-	{
-		DETECTBOX std;
-		std << _std_weight_position * mean(3), _std_weight_position * mean(3),
-			1e-1, _std_weight_position * mean(3);
-		KAL_HMEAN mean1 = _update_mat * mean.transpose();
-		KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose());
-		Eigen::Matrix<float, 4, 4> diag = std.asDiagonal();
-		diag = diag.array().square().matrix();
-		covariance1 += diag;
-		//    covariance1.diagonal() << diag;
-		return std::make_pair(mean1, covariance1);
-	}
-
-	KAL_DATA
-		ByteKalmanFilter::update(
-			const KAL_MEAN &mean,
-			const KAL_COVA &covariance,
-			const DETECTBOX &measurement)
-	{
-		KAL_HDATA pa = project(mean, covariance);
-		KAL_HMEAN projected_mean = pa.first;
-		KAL_HCOVA projected_cov = pa.second;
-
-		//chol_factor, lower =
-		//scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)
-		//kalmain_gain =
-		//scipy.linalg.cho_solve((cho_factor, lower),
-		//np.dot(covariance, self._upadte_mat.T).T,
-		//check_finite=False).T
-		Eigen::Matrix<float, 4, 8> B = (covariance * (_update_mat.transpose())).transpose();
-		Eigen::Matrix<float, 8, 4> kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4
-		Eigen::Matrix<float, 1, 4> innovation = measurement - projected_mean; //eg.1x4
-		auto tmp = innovation * (kalman_gain.transpose());
-		KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix();
-		KAL_COVA new_covariance = covariance - kalman_gain * projected_cov*(kalman_gain.transpose());
-		return std::make_pair(new_mean, new_covariance);
-	}
-
-	Eigen::Matrix<float, 1, -1>
-		ByteKalmanFilter::gating_distance(
-			const KAL_MEAN &mean,
-			const KAL_COVA &covariance,
-			const std::vector<DETECTBOX> &measurements,
-			bool only_position)
-	{
-		KAL_HDATA pa = this->project(mean, covariance);
-		if (only_position) {
-			printf("not implement!");
-			exit(0);
-		}
-		KAL_HMEAN mean1 = pa.first;
-		KAL_HCOVA covariance1 = pa.second;
-
-		//    Eigen::Matrix<float, -1, 4, Eigen::RowMajor> d(size, 4);
-		DETECTBOXSS d(measurements.size(), 4);
-		int pos = 0;
-		for (DETECTBOX box : measurements) {
-			d.row(pos++) = box - mean1;
-		}
-		Eigen::Matrix<float, -1, -1, Eigen::RowMajor> factor = covariance1.llt().matrixL();
-		Eigen::Matrix<float, -1, -1> z = factor.triangularView<Eigen::Lower>().solve<Eigen::OnTheRight>(d).transpose();
-		auto zz = ((z.array())*(z.array())).matrix();
-		auto square_maha = zz.colwise().sum();
-		return square_maha;
-	}
-}

algorithm/mot/bytetrack/BytekalmanFilter.h

@@ -1,31 +0,0 @@
-#pragma once
-
-#include "dataType.h"
-
-namespace byte_kalman
-{
-	class ByteKalmanFilter
-	{
-	public:
-		static const double chi2inv95[10];
-		ByteKalmanFilter();
-		KAL_DATA initiate(const DETECTBOX& measurement);
-		void predict(KAL_MEAN& mean, KAL_COVA& covariance);
-		KAL_HDATA project(const KAL_MEAN& mean, const KAL_COVA& covariance);
-		KAL_DATA update(const KAL_MEAN& mean,
-			const KAL_COVA& covariance,
-			const DETECTBOX& measurement);
-
-		Eigen::Matrix<float, 1, -1> gating_distance(
-			const KAL_MEAN& mean,
-			const KAL_COVA& covariance,
-			const std::vector<DETECTBOX>& measurements,
-			bool only_position = false);
-
-	private:
-		Eigen::Matrix<float, 8, 8, Eigen::RowMajor> _motion_mat;
-		Eigen::Matrix<float, 4, 8, Eigen::RowMajor> _update_mat;
-		float _std_weight_position;
-		float _std_weight_velocity;
-	};
-}

algorithm/mot/bytetrack/STrack.cpp

@@ -1,192 +0,0 @@
-#include "STrack.h"
-
-STrack::STrack( std::vector<float> tlwh_, float score)
-{
-	_tlwh.resize(4);
-	_tlwh.assign(tlwh_.begin(), tlwh_.end());
-
-	is_activated = false;
-	track_id = 0;
-	state = TrackState::New;
-	
-	tlwh.resize(4);
-	tlbr.resize(4);
-
-	static_tlwh();
-	static_tlbr();
-	frame_id = 0;
-	tracklet_len = 0;
-	this->score = score;
-	start_frame = 0;
-}
-
-STrack::~STrack()
-{
-}
-
-void STrack::activate(byte_kalman::ByteKalmanFilter &kalman_filter, int frame_id)
-{
-	this->kalman_filter = kalman_filter;
-	this->track_id = this->next_id();
-
-	 std::vector<float> _tlwh_tmp(4);
-	_tlwh_tmp[0] = this->_tlwh[0];
-	_tlwh_tmp[1] = this->_tlwh[1];
-	_tlwh_tmp[2] = this->_tlwh[2];
-	_tlwh_tmp[3] = this->_tlwh[3];
-	 std::vector<float> xyah = tlwh_to_xyah(_tlwh_tmp);
-	DETECTBOX xyah_box;
-	xyah_box[0] = xyah[0];
-	xyah_box[1] = xyah[1];
-	xyah_box[2] = xyah[2];
-	xyah_box[3] = xyah[3];
-	auto mc = this->kalman_filter.initiate(xyah_box);
-	this->mean = mc.first;
-	this->covariance = mc.second;
-
-	static_tlwh();
-	static_tlbr();
-
-	this->tracklet_len = 0;
-	this->state = TrackState::Tracked;
-	if (frame_id == 1)
-	{
-		this->is_activated = true;
-	}
-	//this->is_activated = true;
-	this->frame_id = frame_id;
-	this->start_frame = frame_id;
-}
-
-void STrack::re_activate(STrack &new_track, int frame_id, bool new_id)
-{
-	 std::vector<float> xyah = tlwh_to_xyah(new_track.tlwh);
-	DETECTBOX xyah_box;
-	xyah_box[0] = xyah[0];
-	xyah_box[1] = xyah[1];
-	xyah_box[2] = xyah[2];
-	xyah_box[3] = xyah[3];
-	auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box);
-	this->mean = mc.first;
-	this->covariance = mc.second;
-
-	static_tlwh();
-	static_tlbr();
-
-	this->tracklet_len = 0;
-	this->state = TrackState::Tracked;
-	this->is_activated = true;
-	this->frame_id = frame_id;
-	this->score = new_track.score;
-	if (new_id)
-		this->track_id = next_id();
-}
-
-void STrack::update(STrack &new_track, int frame_id)
-{
-	this->frame_id = frame_id;
-	this->tracklet_len++;
-
-	 std::vector<float> xyah = tlwh_to_xyah(new_track.tlwh);
-	DETECTBOX xyah_box;
-	xyah_box[0] = xyah[0];
-	xyah_box[1] = xyah[1];
-	xyah_box[2] = xyah[2];
-	xyah_box[3] = xyah[3];
-
-	auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box);
-	this->mean = mc.first;
-	this->covariance = mc.second;
-
-	static_tlwh();
-	static_tlbr();
-
-	this->state = TrackState::Tracked;
-	this->is_activated = true;
-
-	this->score = new_track.score;
-}
-
-void STrack::static_tlwh()
-{
-	if (this->state == TrackState::New)
-	{
-		tlwh[0] = _tlwh[0];
-		tlwh[1] = _tlwh[1];
-		tlwh[2] = _tlwh[2];
-		tlwh[3] = _tlwh[3];
-		return;
-	}
-
-	tlwh[0] = mean[0];
-	tlwh[1] = mean[1];
-	tlwh[2] = mean[2];
-	tlwh[3] = mean[3];
-
-	tlwh[2] *= tlwh[3];
-	tlwh[0] -= tlwh[2] / 2;
-	tlwh[1] -= tlwh[3] / 2;
-}
-
-void STrack::static_tlbr()
-{
-	tlbr.clear();
-	tlbr.assign(tlwh.begin(), tlwh.end());
-	tlbr[2] += tlbr[0];
-	tlbr[3] += tlbr[1];
-}
-
- std::vector<float> STrack::tlwh_to_xyah( std::vector<float> tlwh_tmp)
-{
-	 std::vector<float> tlwh_output = tlwh_tmp;
-	tlwh_output[0] += tlwh_output[2] / 2;
-	tlwh_output[1] += tlwh_output[3] / 2;
-	tlwh_output[2] /= tlwh_output[3];
-	return tlwh_output;
-}
-
- std::vector<float> STrack::to_xyah()
-{
-	return tlwh_to_xyah(tlwh);
-}
-
- std::vector<float> STrack::tlbr_to_tlwh( std::vector<float> &tlbr)
-{
-	tlbr[2] -= tlbr[0];
-	tlbr[3] -= tlbr[1];
-	return tlbr;
-}
-
-void STrack::mark_lost()
-{
-	state = TrackState::Lost;
-}
-
-void STrack::mark_removed()
-{
-	state = TrackState::Removed;
-}
-
-int STrack::next_id()
-{
-	static int _count = 0;
-	_count++;
-	return _count;
-}
-
-int STrack::end_frame()
-{
-	return this->frame_id;
-}
-
-void STrack::multi_predict( std::vector<STrack*> &stracks, byte_kalman::ByteKalmanFilter &kalman_filter)
-{
-	for (int i = 0; i < stracks.size(); i++)
-	{
-		if (stracks[i]->state != TrackState::Tracked)
-		{
-			stracks[i]->mean[7] = 0;
-		}
-		kalman_filter.predict(stracks[i]->mean, stracks[i]->covariance);
-	}
-}

algorithm/mot/bytetrack/STrack.h

@@ -1,47 +0,0 @@
-#pragma once
-
-#include <opencv2/opencv.hpp>
-#include "BytekalmanFilter.h"
-
-enum TrackState { New = 0, Tracked, Lost, Removed };
-
-class STrack
-{
-public:
-	STrack( std::vector<float> tlwh_, float score);
-	~STrack();
-
-	 std::vector<float> static tlbr_to_tlwh( std::vector<float> &tlbr);
-	void static multi_predict( std::vector<STrack*> &stracks, byte_kalman::ByteKalmanFilter &kalman_filter);
-	void static_tlwh();
-	void static_tlbr();
-	 std::vector<float> tlwh_to_xyah( std::vector<float> tlwh_tmp);
-	 std::vector<float> to_xyah();
-	void mark_lost();
-	void mark_removed();
-	int next_id();
-	int end_frame();
-	
-	void activate(byte_kalman::ByteKalmanFilter &kalman_filter, int frame_id);
-	void re_activate(STrack &new_track, int frame_id, bool new_id = false);
-	void update(STrack &new_track, int frame_id);
-
-public:
-	bool is_activated;
-	int track_id;
-	int state;
-
-	 std::vector<float> _tlwh;
-	 std::vector<float> tlwh;
-	 std::vector<float> tlbr;
-	int frame_id;
-	int tracklet_len;
-	int start_frame;
-
-	KAL_MEAN mean;
-	KAL_COVA covariance;
-	float score;
-
-private:
-	byte_kalman::ByteKalmanFilter kalman_filter;
-};

algorithm/mot/bytetrack/dataType.h

@@ -1,27 +0,0 @@
-#pragma once
-
-#include <cstddef>
-#include <vector>
-
-#include <Eigen/Core>
-#include <Eigen/Dense>
-
-
-
-typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> DETECTBOX;
-typedef Eigen::Matrix<float, -1, 4, Eigen::RowMajor> DETECTBOXSS;
-
-
-//Kalmanfilter
-typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_FILTER;
-typedef Eigen::Matrix<float, 1, 8, Eigen::RowMajor> KAL_MEAN;
-typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_COVA;
-typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> KAL_HMEAN;
-typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> KAL_HCOVA;
-using KAL_DATA = std::pair<KAL_MEAN, KAL_COVA>;
-using KAL_HDATA = std::pair<KAL_HMEAN, KAL_HCOVA>;
-
-
-
-
-

algorithm/mot/bytetrack/lapjv.cpp

@@ -1,343 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include "lapjv.h"
-
-/** Column-reduction and reduction transfer for a dense cost matrix.
- */
-int_t _ccrrt_dense(const uint_t n, cost_t *cost[],
-	int_t *free_rows, int_t *x, int_t *y, cost_t *v)
-{
-	int_t n_free_rows;
-	boolean *unique;
-
-	for (uint_t i = 0; i < n; i++) {
-		x[i] = -1;
-		v[i] = LARGE;
-		y[i] = 0;
-	}
-	for (uint_t i = 0; i < n; i++) {
-		for (uint_t j = 0; j < n; j++) {
-			const cost_t c = cost[i][j];
-			if (c < v[j]) {
-				v[j] = c;
-				y[j] = i;
-			}
-			PRINTF("i=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, j, c, v[j], y[j]);
-		}
-	}
-	PRINT_COST_ARRAY(v, n);
-	PRINT_INDEX_ARRAY(y, n);
-	NEW(unique, boolean, n);
-	memset(unique, TRUE, n);
-	{
-		int_t j = n;
-		do {
-			j--;
-			const int_t i = y[j];
-			if (x[i] < 0) {
-				x[i] = j;
-			}
-			else {
-				unique[i] = FALSE;
-				y[j] = -1;
-			}
-		} while (j > 0);
-	}
-	n_free_rows = 0;
-	for (uint_t i = 0; i < n; i++) {
-		if (x[i] < 0) {
-			free_rows[n_free_rows++] = i;
-		}
-		else if (unique[i]) {
-			const int_t j = x[i];
-			cost_t min = LARGE;
-			for (uint_t j2 = 0; j2 < n; j2++) {
-				if (j2 == (uint_t)j) {
-					continue;
-				}
-				const cost_t c = cost[i][j2] - v[j2];
-				if (c < min) {
-					min = c;
-				}
-			}
-			PRINTF("v[%d] = %f - %f\n", j, v[j], min);
-			v[j] -= min;
-		}
-	}
-	FREE(unique);
-	return n_free_rows;
-}
-
-
-/** Augmenting row reduction for a dense cost matrix.
- */
-int_t _carr_dense(
-	const uint_t n, cost_t *cost[],
-	const uint_t n_free_rows,
-	int_t *free_rows, int_t *x, int_t *y, cost_t *v)
-{
-	uint_t current = 0;
-	int_t new_free_rows = 0;
-	uint_t rr_cnt = 0;
-	PRINT_INDEX_ARRAY(x, n);
-	PRINT_INDEX_ARRAY(y, n);
-	PRINT_COST_ARRAY(v, n);
-	PRINT_INDEX_ARRAY(free_rows, n_free_rows);
-	while (current < n_free_rows) {
-		int_t i0;
-		int_t j1, j2;
-		cost_t v1, v2, v1_new;
-		boolean v1_lowers;
-
-		rr_cnt++;
-		PRINTF("current = %d rr_cnt = %d\n", current, rr_cnt);
-		const int_t free_i = free_rows[current++];
-		j1 = 0;
-		v1 = cost[free_i][0] - v[0];
-		j2 = -1;
-		v2 = LARGE;
-		for (uint_t j = 1; j < n; j++) {
-			PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2);
-			const cost_t c = cost[free_i][j] - v[j];
-			if (c < v2) {
-				if (c >= v1) {
-					v2 = c;
-					j2 = j;
-				}
-				else {
-					v2 = v1;
-					v1 = c;
-					j2 = j1;
-					j1 = j;
-				}
-			}
-		}
-		i0 = y[j1];
-		v1_new = v[j1] - (v2 - v1);
-		v1_lowers = v1_new < v[j1];
-		PRINTF("%d %d 1=%d,%f 2=%d,%f v1'=%f(%d,%g) \n", free_i, i0, j1, v1, j2, v2, v1_new, v1_lowers, v[j1] - v1_new);
-		if (rr_cnt < current * n) {
-			if (v1_lowers) {
-				v[j1] = v1_new;
-			}
-			else if (i0 >= 0 && j2 >= 0) {
-				j1 = j2;
-				i0 = y[j2];
-			}
-			if (i0 >= 0) {
-				if (v1_lowers) {
-					free_rows[--current] = i0;
-				}
-				else {
-					free_rows[new_free_rows++] = i0;
-				}
-			}
-		}
-		else {
-			PRINTF("rr_cnt=%d >= %d (current=%d * n=%d)\n", rr_cnt, current * n, current, n);
-			if (i0 >= 0) {
-				free_rows[new_free_rows++] = i0;
-			}
-		}
-		x[free_i] = j1;
-		y[j1] = free_i;
-	}
-	return new_free_rows;
-}
-
-
-/** Find columns with minimum d[j] and put them on the SCAN list.
- */
-uint_t _find_dense(const uint_t n, uint_t lo, cost_t *d, int_t *cols, int_t *y)
-{
-	uint_t hi = lo + 1;
-	cost_t mind = d[cols[lo]];
-	for (uint_t k = hi; k < n; k++) {
-		int_t j = cols[k];
-		if (d[j] <= mind) {
-			if (d[j] < mind) {
-				hi = lo;
-				mind = d[j];
-			}
-			cols[k] = cols[hi];
-			cols[hi++] = j;
-		}
-	}
-	return hi;
-}
-
-
-// Scan all columns in TODO starting from arbitrary column in SCAN
-// and try to decrease d of the TODO columns using the SCAN column.
-int_t _scan_dense(const uint_t n, cost_t *cost[],
-	uint_t *plo, uint_t*phi,
-	cost_t *d, int_t *cols, int_t *pred,
-	int_t *y, cost_t *v)
-{
-	uint_t lo = *plo;
-	uint_t hi = *phi;
-	cost_t h, cred_ij;
-
-	while (lo != hi) {
-		int_t j = cols[lo++];
-		const int_t i = y[j];
-		const cost_t mind = d[j];
-		h = cost[i][j] - v[j] - mind;
-		PRINTF("i=%d j=%d h=%f\n", i, j, h);
-		// For all columns in TODO
-		for (uint_t k = hi; k < n; k++) {
-			j = cols[k];
-			cred_ij = cost[i][j] - v[j] - h;
-			if (cred_ij < d[j]) {
-				d[j] = cred_ij;
-				pred[j] = i;
-				if (cred_ij == mind) {
-					if (y[j] < 0) {
-						return j;
-					}
-					cols[k] = cols[hi];
-					cols[hi++] = j;
-				}
-			}
-		}
-	}
-	*plo = lo;
-	*phi = hi;
-	return -1;
-}
-
-
-/** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper.
- *
- * This is a dense matrix version.
- *
- * \return The closest free column index.
- */
-int_t find_path_dense(
-	const uint_t n, cost_t *cost[],
-	const int_t start_i,
-	int_t *y, cost_t *v,
-	int_t *pred)
-{
-	uint_t lo = 0, hi = 0;
-	int_t final_j = -1;
-	uint_t n_ready = 0;
-	int_t *cols;
-	cost_t *d;
-
-	NEW(cols, int_t, n);
-	NEW(d, cost_t, n);
-
-	for (uint_t i = 0; i < n; i++) {
-		cols[i] = i;
-		pred[i] = start_i;
-		d[i] = cost[start_i][i] - v[i];
-	}
-	PRINT_COST_ARRAY(d, n);
-	while (final_j == -1) {
-		// No columns left on the SCAN list.
-		if (lo == hi) {
-			PRINTF("%d..%d -> find\n", lo, hi);
-			n_ready = lo;
-			hi = _find_dense(n, lo, d, cols, y);
-			PRINTF("check %d..%d\n", lo, hi);
-			PRINT_INDEX_ARRAY(cols, n);
-			for (uint_t k = lo; k < hi; k++) {
-				const int_t j = cols[k];
-				if (y[j] < 0) {
-					final_j = j;
-				}
-			}
-		}
-		if (final_j == -1) {
-			PRINTF("%d..%d -> scan\n", lo, hi);
-			final_j = _scan_dense(
-				n, cost, &lo, &hi, d, cols, pred, y, v);
-			PRINT_COST_ARRAY(d, n);
-			PRINT_INDEX_ARRAY(cols, n);
-			PRINT_INDEX_ARRAY(pred, n);
-		}
-	}
-
-	PRINTF("found final_j=%d\n", final_j);
-	PRINT_INDEX_ARRAY(cols, n);
-	{
-		const cost_t mind = d[cols[lo]];
-		for (uint_t k = 0; k < n_ready; k++) {
-			const int_t j = cols[k];
-			v[j] += d[j] - mind;
-		}
-	}
-
-	FREE(cols);
-	FREE(d);
-
-	return final_j;
-}
-
-
-/** Augment for a dense cost matrix.
- */
-int_t _ca_dense(
-	const uint_t n, cost_t *cost[],
-	const uint_t n_free_rows,
-	int_t *free_rows, int_t *x, int_t *y, cost_t *v)
-{
-	int_t *pred;
-
-	NEW(pred, int_t, n);
-
-	for (int_t *pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) {
-		int_t i = -1, j;
-		uint_t k = 0;
-
-		PRINTF("looking at free_i=%d\n", *pfree_i);
-		j = find_path_dense(n, cost, *pfree_i, y, v, pred);
-		ASSERT(j >= 0);
-		ASSERT(j < n);
-		while (i != *pfree_i) {
-			PRINTF("augment %d\n", j);
-			PRINT_INDEX_ARRAY(pred, n);
-			i = pred[j];
-			PRINTF("y[%d]=%d -> %d\n", j, y[j], i);
-			y[j] = i;
-			PRINT_INDEX_ARRAY(x, n);
-			SWAP_INDICES(j, x[i]);
-			k++;
-			if (k >= n) {
-				ASSERT(FALSE);
-			}
-		}
-	}
-	FREE(pred);
-	return 0;
-}
-
-
-/** Solve dense sparse LAP.
- */
-int lapjv_internal(
-	const uint_t n, cost_t *cost[],
-	int_t *x, int_t *y)
-{
-	int ret;
-	int_t *free_rows;
-	cost_t *v;
-
-	NEW(free_rows, int_t, n);
-	NEW(v, cost_t, n);
-	ret = _ccrrt_dense(n, cost, free_rows, x, y, v);
-	int i = 0;
-	while (ret > 0 && i < 2) {
-		ret = _carr_dense(n, cost, ret, free_rows, x, y, v);
-		i++;
-	}
-	if (ret > 0) {
-		ret = _ca_dense(n, cost, ret, free_rows, x, y, v);
-	}
-	FREE(v);
-	FREE(free_rows);
-	return ret;
-}

algorithm/mot/bytetrack/lapjv.h

@@ -1,63 +0,0 @@
-#ifndef LAPJV_H
-#define LAPJV_H
-
-#define LARGE 1000000
-
-#if !defined TRUE
-#define TRUE 1
-#endif
-#if !defined FALSE
-#define FALSE 0
-#endif
-
-#define NEW(x, t, n) if ((x = (t *)malloc(sizeof(t) * (n))) == 0) { return -1; }
-#define FREE(x) if (x != 0) { free(x); x = 0; }
-#define SWAP_INDICES(a, b) { int_t _temp_index = a; a = b; b = _temp_index; }
-
-#if 0
-#include <assert.h>
-#define ASSERT(cond) assert(cond)
-#define PRINTF(fmt, ...) printf(fmt, ##__VA_ARGS__)
-#define PRINT_COST_ARRAY(a, n) \
-    while (1) { \
-        printf(#a" = ["); \
-        if ((n) > 0) { \
-            printf("%f", (a)[0]); \
-            for (uint_t j = 1; j < n; j++) { \
-                printf(", %f", (a)[j]); \
-            } \
-        } \
-        printf("]\n"); \
-        break; \
-    }
-#define PRINT_INDEX_ARRAY(a, n) \
-    while (1) { \
-        printf(#a" = ["); \
-        if ((n) > 0) { \
-            printf("%d", (a)[0]); \
-            for (uint_t j = 1; j < n; j++) { \
-                printf(", %d", (a)[j]); \
-            } \
-        } \
-        printf("]\n"); \
-        break; \
-    }
-#else
-#define ASSERT(cond)
-#define PRINTF(fmt, ...)
-#define PRINT_COST_ARRAY(a, n)
-#define PRINT_INDEX_ARRAY(a, n)
-#endif
-
-
-typedef signed int int_t;
-typedef unsigned int uint_t;
-typedef double cost_t;
-typedef char boolean;
-typedef enum fp_t { FP_1 = 1, FP_2 = 2, FP_DYNAMIC = 3 } fp_t;
-
-extern int_t lapjv_internal(
-	const uint_t n, cost_t *cost[],
-	int_t *x, int_t *y);
-
-#endif // LAPJV_H

algorithm/mot/bytetrack/utils.cpp

@@ -1,432 +0,0 @@
-#include "BYTETracker.h"
-#include "lapjv.h"
-
-namespace sv {
-
-std::vector<STrack*> BYTETracker::joint_stracks( std::vector<STrack*> &tlista,  std::vector<STrack> &tlistb)
-{
-    std::map<int, int> exists;
-    std::vector<STrack*> res;
-    for (int i = 0; i < tlista.size(); i++)
-    {
-        exists.insert(std::pair<int, int>(tlista[i]->track_id, 1));
-        res.push_back(tlista[i]);
-    }
-    for (int i = 0; i < tlistb.size(); i++)
-    {
-        int tid = tlistb[i].track_id;
-        if (!exists[tid] || exists.count(tid) == 0)
-        {
-            exists[tid] = 1;
-            res.push_back(&tlistb[i]);
-        }
-    }
-    return res;
-}
-
-std::vector<STrack> BYTETracker::joint_stracks( std::vector<STrack> &tlista,  std::vector<STrack> &tlistb)
-{
-    std::map<int, int> exists;
-    std::vector<STrack> res;
-    for (int i = 0; i < tlista.size(); i++)
-    {
-        exists.insert(std::pair<int, int>(tlista[i].track_id, 1));
-        res.push_back(tlista[i]);
-    }
-    for (int i = 0; i < tlistb.size(); i++)
-    {
-        int tid = tlistb[i].track_id;
-        if (!exists[tid] || exists.count(tid) == 0)
-        {
-            exists[tid] = 1;
-            res.push_back(tlistb[i]);
-        }
-    }
-    return res;
-}
-
-std::vector<STrack> BYTETracker::sub_stracks( std::vector<STrack> &tlista,  std::vector<STrack> &tlistb)
-{
-    std::map<int, STrack> stracks;
-    for (int i = 0; i < tlista.size(); i++)
-    {
-        stracks.insert(std::pair<int, STrack>(tlista[i].track_id, tlista[i]));
-    }
-    for (int i = 0; i < tlistb.size(); i++)
-    {
-        int tid = tlistb[i].track_id;
-        if (stracks.count(tid) != 0)
-        {
-            stracks.erase(tid);
-        }
-    }
-
-    std::vector<STrack> res;
-    std::map<int, STrack>::iterator  it;
-    for (it = stracks.begin(); it != stracks.end(); ++it)
-    {
-        res.push_back(it->second);
-    }
-
-    return res;
-}
-
-void BYTETracker::remove_duplicate_stracks( std::vector<STrack> &resa,  std::vector<STrack> &resb,  std::vector<STrack> &stracksa,  std::vector<STrack> &stracksb)
-{
-    std::vector< std::vector<float> > pdist = iou_distance(stracksa, stracksb);
-    std::vector<std::pair<int, int> > pairs;
-    for (int i = 0; i < pdist.size(); i++)
-    {
-        for (int j = 0; j < pdist[i].size(); j++)
-        {
-            if (pdist[i][j] < 0.15)
-            {
-                pairs.push_back(std::pair<int, int>(i, j));
-            }
-        }
-    }
-
-    std::vector<int> dupa, dupb;
-    for (int i = 0; i < pairs.size(); i++)
-    {
-        int timep = stracksa[pairs[i].first].frame_id - stracksa[pairs[i].first].start_frame;
-        int timeq = stracksb[pairs[i].second].frame_id - stracksb[pairs[i].second].start_frame;
-        if (timep > timeq)
-            dupb.push_back(pairs[i].second);
-        else
-            dupa.push_back(pairs[i].first);
-    }
-
-    for (int i = 0; i < stracksa.size(); i++)
-    {
-        std::vector<int>::iterator iter = find(dupa.begin(), dupa.end(), i);
-        if (iter == dupa.end())
-        {
-            resa.push_back(stracksa[i]);
-        }
-    }
-
-    for (int i = 0; i < stracksb.size(); i++)
-    {
-        std::vector<int>::iterator iter = find(dupb.begin(), dupb.end(), i);
-        if (iter == dupb.end())
-        {
-            resb.push_back(stracksb[i]);
-        }
-    }
-}
-
-void BYTETracker::linear_assignment( std::vector< std::vector<float> > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh,
-                                     std::vector< std::vector<int> > &matches,  std::vector<int> &unmatched_a,  std::vector<int> &unmatched_b)
-{
-    if (cost_matrix.size() == 0)
-    {
-        for (int i = 0; i < cost_matrix_size; i++)
-        {
-            unmatched_a.push_back(i);
-        }
-        for (int i = 0; i < cost_matrix_size_size; i++)
-        {
-            unmatched_b.push_back(i);
-        }
-        return;
-    }
-
-    std::vector<int> rowsol;  std::vector<int> colsol;
-    float c = lapjv(cost_matrix, rowsol, colsol, true, thresh);
-    for (int i = 0; i < rowsol.size(); i++)
-    {
-        if (rowsol[i] >= 0)
-        {
-            std::vector<int> match;
-            match.push_back(i);
-            match.push_back(rowsol[i]);
-            matches.push_back(match);
-        }
-        else
-        {
-            unmatched_a.push_back(i);
-        }
-    }
-
-    for (int i = 0; i < colsol.size(); i++)
-    {
-        if (colsol[i] < 0)
-        {
-            unmatched_b.push_back(i);
-        }
-    }
-}
-
-std::vector< std::vector<float> > BYTETracker::ious( std::vector< std::vector<float> > &atlbrs,  std::vector< std::vector<float> > &btlbrs)
-{
-    std::vector< std::vector<float> > ious;
-    if (atlbrs.size()*btlbrs.size() == 0)
-        return ious;
-
-    ious.resize(atlbrs.size());
-    for (int i = 0; i < ious.size(); i++)
-    {
-        ious[i].resize(btlbrs.size());
-    }
-
-    //bbox_ious
-    for (int k = 0; k < btlbrs.size(); k++)
-    {
-        std::vector<float> ious_tmp;
-        float box_area = (btlbrs[k][2] - btlbrs[k][0] + 1)*(btlbrs[k][3] - btlbrs[k][1] + 1);
-        for (int n = 0; n < atlbrs.size(); n++)
-        {
-            float iw = cv::min(atlbrs[n][2], btlbrs[k][2]) - cv::max(atlbrs[n][0], btlbrs[k][0]) + 1;
-            if (iw > 0)
-            {
-                float ih = cv::min(atlbrs[n][3], btlbrs[k][3]) - cv::max(atlbrs[n][1], btlbrs[k][1]) + 1;
-                if(ih > 0)
-                {
-                    float ua = (atlbrs[n][2] - atlbrs[n][0] + 1)*(atlbrs[n][3] - atlbrs[n][1] + 1) + box_area - iw * ih;
-                    ious[n][k] = iw * ih / ua;
-                }
-                else
-                {
-                    ious[n][k] = 0.0;
-                }
-            }
-            else
-            {
-                ious[n][k] = 0.0;
-            }
-        }
-    }
-
-    return ious;
-}
-
-std::vector< std::vector<float> > BYTETracker::iou_distance( std::vector<STrack*> &atracks,  std::vector<STrack> &btracks, int &dist_size, int &dist_size_size)
-{
-    std::vector< std::vector<float> > cost_matrix;
-    if (atracks.size() * btracks.size() == 0)
-    {
-        dist_size = atracks.size();
-        dist_size_size = btracks.size();
-        return cost_matrix;
-    }
-    std::vector< std::vector<float> > atlbrs, btlbrs;
-    for (int i = 0; i < atracks.size(); i++)
-    {
-        atlbrs.push_back(atracks[i]->tlbr);
-    }
-    for (int i = 0; i < btracks.size(); i++)
-    {
-        btlbrs.push_back(btracks[i].tlbr);
-    }
-
-    dist_size = atracks.size();
-    dist_size_size = btracks.size();
-
-    std::vector< std::vector<float> > _ious = ious(atlbrs, btlbrs);
-
-    for (int i = 0; i < _ious.size();i++)
-    {
-        std::vector<float> _iou;
-        for (int j = 0; j < _ious[i].size(); j++)
-        {
-            _iou.push_back(1 - _ious[i][j]);
-        }
-        cost_matrix.push_back(_iou);
-    }
-
-    return cost_matrix;
-}
-
-std::vector< std::vector<float> > BYTETracker::iou_distance( std::vector<STrack> &atracks,  std::vector<STrack> &btracks)
-{
-    std::vector< std::vector<float> > atlbrs, btlbrs;
-    for (int i = 0; i < atracks.size(); i++)
-    {
-        atlbrs.push_back(atracks[i].tlbr);
-    }
-    for (int i = 0; i < btracks.size(); i++)
-    {
-        btlbrs.push_back(btracks[i].tlbr);
-    }
-
-    std::vector< std::vector<float> > _ious = ious(atlbrs, btlbrs);
-    std::vector< std::vector<float> > cost_matrix;
-    for (int i = 0; i < _ious.size(); i++)
-    {
-        std::vector<float> _iou;
-        for (int j = 0; j < _ious[i].size(); j++)
-        {
-            _iou.push_back(1 - _ious[i][j]);
-        }
-        cost_matrix.push_back(_iou);
-    }
-
-    return cost_matrix;
-}
-
-double BYTETracker::lapjv(const  std::vector< std::vector<float> > &cost,  std::vector<int> &rowsol,  std::vector<int> &colsol,
-                          bool extend_cost, float cost_limit, bool return_cost)
-{
-    std::vector< std::vector<float> > cost_c;
-    cost_c.assign(cost.begin(), cost.end());
-
-    std::vector< std::vector<float> > cost_c_extended;
-
-    int n_rows = cost.size();
-    int n_cols = cost[0].size();
-    rowsol.resize(n_rows);
-    colsol.resize(n_cols);
-
-    int n = 0;
-    if (n_rows == n_cols)
-    {
-        n = n_rows;
-    }
-    else
-    {
-        if (!extend_cost)
-        {
-            std::cout << "set extend_cost=True" << std::endl;
-            system("pause");
-            exit(0);
-        }
-    }
-
-    if (extend_cost || cost_limit < LONG_MAX)
-    {
-        n = n_rows + n_cols;
-        cost_c_extended.resize(n);
-        for (int i = 0; i < cost_c_extended.size(); i++)
-            cost_c_extended[i].resize(n);
-
-        if (cost_limit < LONG_MAX)
-        {
-            for (int i = 0; i < cost_c_extended.size(); i++)
-            {
-                for (int j = 0; j < cost_c_extended[i].size(); j++)
-                {
-                    cost_c_extended[i][j] = cost_limit / 2.0;
-                }
-            }
-        }
-        else
-        {
-            float cost_max = -1;
-            for (int i = 0; i < cost_c.size(); i++)
-            {
-                for (int j = 0; j < cost_c[i].size(); j++)
-                {
-                    if (cost_c[i][j] > cost_max)
-                        cost_max = cost_c[i][j];
-                }
-            }
-            for (int i = 0; i < cost_c_extended.size(); i++)
-            {
-                for (int j = 0; j < cost_c_extended[i].size(); j++)
-                {
-                    cost_c_extended[i][j] = cost_max + 1;
-                }
-            }
-        }
-
-        for (int i = n_rows; i < cost_c_extended.size(); i++)
-        {
-            for (int j = n_cols; j < cost_c_extended[i].size(); j++)
-            {
-                cost_c_extended[i][j] = 0;
-            }
-        }
-        for (int i = 0; i < n_rows; i++)
-        {
-            for (int j = 0; j < n_cols; j++)
-            {
-                cost_c_extended[i][j] = cost_c[i][j];
-            }
-        }
-
-        cost_c.clear();
-        cost_c.assign(cost_c_extended.begin(), cost_c_extended.end());
-    }
-
-    double **cost_ptr;
-    cost_ptr = new double *[sizeof(double *) * n];
-    for (int i = 0; i < n; i++)
-        cost_ptr[i] = new double[sizeof(double) * n];
-
-    for (int i = 0; i < n; i++)
-    {
-        for (int j = 0; j < n; j++)
-        {
-            cost_ptr[i][j] = cost_c[i][j];
-        }
-    }
-
-    int* x_c = new int[sizeof(int) * n];
-    int *y_c = new int[sizeof(int) * n];
-
-    int ret = lapjv_internal(n, cost_ptr, x_c, y_c);
-    if (ret != 0)
-    {
-        std::cout << "Calculate Wrong!" << std::endl;
-        system("pause");
-        exit(0);
-    }
-
-    double opt = 0.0;
-
-    if (n != n_rows)
-    {
-        for (int i = 0; i < n; i++)
-        {
-            if (x_c[i] >= n_cols)
-                x_c[i] = -1;
-            if (y_c[i] >= n_rows)
-                y_c[i] = -1;
-        }
-        for (int i = 0; i < n_rows; i++)
-        {
-            rowsol[i] = x_c[i];
-        }
-        for (int i = 0; i < n_cols; i++)
-        {
-            colsol[i] = y_c[i];
-        }
-
-        if (return_cost)
-        {
-            for (int i = 0; i < rowsol.size(); i++)
-            {
-                if (rowsol[i] != -1)
-                {
-                    //cout << i << "\t" << rowsol[i] << "\t" << cost_ptr[i][rowsol[i]] << endl;
-                    opt += cost_ptr[i][rowsol[i]];
-                }
-            }
-        }
-    }
-    else if (return_cost)
-    {
-        for (int i = 0; i < rowsol.size(); i++)
-        {
-            opt += cost_ptr[i][rowsol[i]];
-        }
-    }
-
-    for (int i = 0; i < n; i++)
-    {
-        delete[]cost_ptr[i];
-    }
-    delete[]cost_ptr;
-    delete[]x_c;
-    delete[]y_c;
-
-    return opt;
-}
-
-cv::Scalar BYTETracker::get_color(int idx)
-{
-    idx += 3;
-    return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255);
-}
-}

algorithm/mot/sort/sort.cpp

@@ -1,377 +0,0 @@
-#include "sort.h"
-#include <cmath>
-#include <fstream>
-#include <limits>
-#include <vector>
-#include "gason.h"
-#include "sv_util.h"
-
-using namespace std;
-using namespace Eigen;
-
-namespace sv {
-
-
-KalmanFilter::KalmanFilter()
-{
-    this->_chi2inv95 << 3.8415, 5.9915, 7.8147, 9.4877, 11.070, 12.592, 14.067, 15.507, 16.919;
-    this->_F = MatrixXd::Identity(8, 8);
-    for (int i=0; i<4; i++)
-    {
-        this->_F(i,i+4) = 1.; //1
-    }
-    this->_H = MatrixXd::Identity(4, 8);
-    this->_std_weight_position = 1. / 20; //1./20
-    this->_std_weight_vel = 1. / 160; //1./160
-}
-
-KalmanFilter::~KalmanFilter()
-{
-}
-
-pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
-{
-    Matrix<double,8,1> mean;
-    Matrix<double,4,1> zero_vector;
-    zero_vector.setZero();
-    mean << bbox(0), bbox(1), (double)bbox(2) / (double)bbox(3), bbox(3), zero_vector;
-    VectorXd stds(8);
-    stds << 2 * this->_std_weight_position * mean(3), 2 * this->_std_weight_position * mean(3), 0.01, 2 * this->_std_weight_position * mean(3), \
-        10 * this->_std_weight_vel * mean(3), 10 * this->_std_weight_vel * mean(3), 1e-5, 10 * this->_std_weight_vel * mean(3);
-    MatrixXd squared = stds.array().square();
-    Matrix<double, 8, 8> covariances;
-    covariances = squared.asDiagonal();
-    return make_pair(mean, covariances);
-}
-
-pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances, sv::Box &box)
-{
-    VectorXd measurement(4);
-    double a = (double)(box.x2-box.x1) / (double)(box.y2-box.y1);
-    measurement << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, a, box.y2-box.y1;
-    pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > projected = project(mean, covariances);
-    Matrix<double, 4, 1> projected_mean = projected.first;
-    Matrix<double, 4, 4> projected_cov = projected.second;
-
-    Eigen::LLT<Eigen::MatrixXd> chol_factor(projected_cov);
-    MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose();
-    
-    VectorXd innovation = measurement - projected_mean;
-    Matrix<double, 8, 1> new_mean = mean + Kalman_gain *innovation;
-    Matrix<double, 8, 8> new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose();
-
-    return make_pair(new_mean, new_covariances);
-}
-
-pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > KalmanFilter::project(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances)
-{
-    VectorXd stds(4);
-    stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.1, this->_std_weight_position * mean(3); 
-    MatrixXd squared = stds.array().square();
-    MatrixXd R = squared.asDiagonal();
-
-    Matrix<double, 4, 1>  pro_mean = this->_H * mean;
-    Matrix<double, 4, 4>  pro_covariances = this->_H * covariances * this->_H.transpose() + R;
-    return make_pair(pro_mean, pro_covariances);
-}
-pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::predict(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances)
-{
-    VectorXd stds(8);
-    stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 1e-2, this->_std_weight_position * mean(3), \
-        this->_std_weight_vel * mean(3), this->_std_weight_vel * mean(3), 1e-5, this->_std_weight_vel * mean(3); // a = 0.01
-    MatrixXd squared = stds.array().square();
-    MatrixXd Q = squared.asDiagonal();
-    Matrix<double, 8, 1> pre_mean = this->_F * mean;
-    Matrix<double, 8, 8> pre_cov = this->_F * covariances * this->_F.transpose()+Q;//+Q
-    return make_pair(pre_mean, pre_cov);
-}
-
-
-SORT::~SORT()
-{
-}
-
-void SORT::update(TargetsInFrame& tgts)
-{
-    KalmanFilter kf;
-    if (! this->_tracklets.size() || tgts.targets.size() == 0)
-    {
-        Vector4d bbox;
-        for (int i=0; i<tgts.targets.size(); i++)
-        {
-            sv::Box box;
-            tgts.targets[i].getBox(box);		
-            Tracklet tracklet;
-            tracklet.id = ++ this->_next_tracklet_id;
-            tgts.targets[i].tracked_id = this->_next_tracklet_id;
-            tgts.targets[i].has_tid = true;
-
-            tracklet.bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1;  // x,y,w,h; center(x,y)
-            
-            tracklet.age = 0;
-            tracklet.hits = 1;
-            //tracklet.misses = 0;
-            tracklet.frame_id = tgts.frame_id;
-            tracklet.category_id = tgts.targets[i].category_id;
-            if (tgts.frame_id == 0)
-            {
-                tracklet.tentative = false;
-            }
-            else
-            {
-                tracklet.tentative = true;
-            }
-            // initate the motion
-            pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.initiate(tracklet.bbox);
-            tracklet.mean = motion.first;
-            tracklet.covariance = motion.second;
-            
-            this->_tracklets.push_back(tracklet);
-    }
-    }
-    else
-    {
-        for (int i=0; i<tgts.targets.size(); i++)
-        {
-            tgts.targets[i].tracked_id = 0;
-            tgts.targets[i].has_tid = true;
-        }
-
-        vector<int> match_det(tgts.targets.size(), -1);
-        // predict the next state of each tracklet
-        for (auto& tracklet : this->_tracklets)
-        {
-            tracklet.age++;
-            pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.predict(tracklet.mean, tracklet.covariance);
-            tracklet.bbox << motion.first(0), motion.first(1), motion.first(2) * motion.first(3), motion.first(3);
-            tracklet.mean = motion.first;
-            tracklet.covariance = motion.second;    
-        }
-
-        // Match the detections to the existing tracklets 
-        vector<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
-        for (int i=0; i<this->_tracklets.size(); i++)
-        {
-            for (int j=0; j<tgts.targets.size(); j++)
-            {
-                sv::Box box;
-                tgts.targets[j].getBox(box);
-                iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
-            }
-        }
-        
-        vector<pair<int, int> > matches = this->_hungarian(iouMatrix);
-        for (auto& match : matches)
-        {
-            int trackletIndex = match.first;
-            int detectionIndex = match.second;
-            if (trackletIndex >= 0 && detectionIndex >= 0)
-            {
-                if (iouMatrix[match.first][match.second] <= 1-_iou_threshold)  // iou_thrshold
-                {
-                    sv::Box box;
-                    tgts.targets[detectionIndex].getBox(box);
-                    this->_tracklets[trackletIndex].age = 0;
-                    this->_tracklets[trackletIndex].hits++;
-                    this->_tracklets[trackletIndex].frame_id = tgts.frame_id;
-                    this->_tracklets[trackletIndex].bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1;
-                    tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id;
-                    match_det[detectionIndex] = trackletIndex;
-                }
-            }
-        }
-        std::vector <vector<double>> ().swap(iouMatrix);
-        for (int i=0; i<tgts.targets.size(); i++)
-        { 
-            if (match_det[i] == -1)
-            {
-                sv::Box box;
-                tgts.targets[i].getBox(box);
-                Tracklet tracklet;
-                tracklet.id = ++ this->_next_tracklet_id;
-                tracklet.bbox << box.x1+(box.x2-box.x1)/2, (double)(box.y1+(box.y2-box.y1)/2), box.x2-box.x1, box.y2-box.y1; // c_x, c_y, w, h
-                tracklet.age = 0;
-                tracklet.hits = 1;
-                tracklet.frame_id = tgts.frame_id;
-                tracklet.category_id = tgts.targets[i].category_id;
-                tracklet.tentative = true;
-                
-                pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > new_motion = kf.initiate(tracklet.bbox);
-                tracklet.mean = new_motion.first;
-                tracklet.covariance = new_motion.second;
-
-                tgts.targets[i].tracked_id = this->_next_tracklet_id;
-                tgts.targets[i].has_tid = true;
-                this->_tracklets.push_back(tracklet);
-            }
-            else
-            {
-                sv::Box box;
-                int track_id = match_det[i];
-                tgts.targets[i].getBox(box);
-                pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > updated = kf.update(this->_tracklets[track_id].mean, this->_tracklets[track_id].covariance, box);
-                this->_tracklets[track_id].mean = updated.first;
-                this->_tracklets[track_id].covariance = updated.second;
-            }
-        }
-        
-        //sift tracklets
-        for (auto& tracklet : this->_tracklets)
-        {
-            if (tracklet.hits >= _min_hits)
-            {
-                tracklet.tentative = false;
-            }
-            if ((tgts.frame_id-tracklet.frame_id <= _max_age) && !(tracklet.tentative && tracklet.frame_id != tgts.frame_id))
-            {
-                _new_tracklets.push_back(tracklet);
-            } 
-        }
-        _tracklets = _new_tracklets;
-        std::vector <Tracklet> ().swap(_new_tracklets);
-    }
-}
-
-vector<Tracklet> SORT::getTracklets() const
-{
-    return this->_tracklets;
-}
-
-double SORT::_iou(Tracklet& tracklet, sv::Box& box)
-{
-    double trackletX1 = tracklet.bbox(0)-tracklet.bbox(2)/2;
-    double trackletY1 = tracklet.bbox(1)-tracklet.bbox(3)/2;
-    double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2)/2;
-    double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3)/2;
-    
-    double detectionX1 = box.x1;
-    double detectionY1 = box.y1;
-    double detectionX2 = box.x2;
-    double detectionY2 = box.y2;
-
-    double intersectionX1 = max(trackletX1, detectionX1);
-    double intersectionY1 = max(trackletY1, detectionY1);
-    double intersectionX2 = min(trackletX2, detectionX2);
-    double intersectionY2 = min(trackletY2, detectionY2);
-
-    double w = (intersectionX2-intersectionX1 > 0.0) ? (intersectionX2-intersectionX1) : 0.0;
-    double h = (intersectionY2-intersectionY1 > 0.0) ? (intersectionY2-intersectionY1) : 0.0;
-    double intersectionArea = w * h;
-
-    double trackletArea = tracklet.bbox(2) * tracklet.bbox(3);
-
-    double detectionArea = (box.x2-box.x1) * (box.y2-box.y1);
-    double unionArea = trackletArea + detectionArea - intersectionArea;
-    double iou = (1 - intersectionArea / unionArea * 1.0);
-
-    return iou;
-}
-
-// Function to find the minimum element in a vector
-double SORT::_findMin(const std::vector<double>& vec) {
-    double minVal = std::numeric_limits<double>::max();
-    for (double val : vec) {
-        if (val < minVal) {
-            minVal = val;
-        }
-    }
-    return minVal;
-}
-
-// Function to subtract the minimum value from each row of the cost matrix
-void SORT::_subtractMinFromRows(std::vector<std::vector<double>>& costMatrix) {
-    for (auto& row : costMatrix) {
-        double minVal = _findMin(row);
-        for (double& val : row) {
-            val -= minVal;
-        }
-    }
-}
-
-// Function to subtract the minimum value from each column of the cost matrix
-void SORT::_subtractMinFromCols(std::vector<std::vector<double>>& costMatrix) {
-    for (size_t col = 0; col < costMatrix[0].size(); ++col) {
-        double minVal = std::numeric_limits<double>::max();
-        for (size_t row = 0; row < costMatrix.size(); ++row) {
-            if (costMatrix[row][col] < minVal) {
-                minVal = costMatrix[row][col];
-            }
-        }
-        for (size_t row = 0; row < costMatrix.size(); ++row) {
-            costMatrix[row][col] -= minVal;
-        }
-    }
-}
-
-// Function to find a matching using the Hungarian algorithm
-vector<pair<int, int> > SORT::_hungarian(vector<vector<double> > costMatrix)
-{
-    size_t numRows = costMatrix.size();
-    size_t numCols = costMatrix[0].size();
-
-    //transpose the matrix if necessary
-    const bool transposed = numCols > numRows;
-    if (transposed) {
-        vector<vector<double>> transposedMatrix(numCols, vector<double>(numRows));
-        for (int i = 0; i < numRows; i++)
-        {
-            for (int j = 0; j < numCols; j++)
-            {
-                transposedMatrix[j][i] = costMatrix[i][j];
-            }
-        }
-        costMatrix = transposedMatrix;
-        swap(numRows, numCols);
-    }
-    // Determine the larger dimension for matching
-    size_t maxDim = std::max(numRows, numCols);
-
-    // Create a square cost matrix by padding with zeros if necessary
-    std::vector<std::vector<double>> squareMatrix(maxDim, std::vector<double>(maxDim, 0.0));
-    for (size_t row = 0; row < numRows; ++row) {
-        for (size_t col = 0; col < numCols; ++col) {
-            squareMatrix[row][col] = costMatrix[row][col];
-        }
-    }
-
-    // Subtract the minimum value from each row and column
-    _subtractMinFromRows(squareMatrix);
-    _subtractMinFromCols(squareMatrix);
-
-    // Initialize the assignment vectors with -1 values
-    std::vector<int> rowAssignment(maxDim, -1);
-    std::vector<int> colAssignment(maxDim, -1);
-
-    // Perform the matching
-    for (size_t row = 0; row < maxDim; ++row) {
-        std::vector<bool> visitedCols(maxDim, false);
-        for (size_t col = 0; col < maxDim; ++col) {
-            if (squareMatrix[row][col] == 0 && colAssignment[col] == -1) {
-                rowAssignment[row] = col;
-                colAssignment[col] = row;
-                break;
-            }
-        }
-    }
-
-    // Convert the assignment vectors to pair<int, int> format
-    std::vector<std::pair<int, int>> assignmentPairs;
-    for (size_t row = 0; row < numRows; ++row) {
-        int col = rowAssignment[row];
-        if (col != -1) {
-            if (col >= numCols) {
-                col = -1;
-            }
-            assignmentPairs.emplace_back(row, col);
-        }
-    }
-    if (transposed) {
-        for (auto& assignment : assignmentPairs)
-        {
-            swap(assignment.first, assignment.second);
-        }
-    }
-    return assignmentPairs;
-    }
-}
-

algorithm/mot/sort/sort.h

@@ -1,77 +0,0 @@
-#ifndef __SV_SORT__
-#define __SV_SORT__
-
-#include "sv_core.h"
-#include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
-#include <opencv2/tracking.hpp>
-#include <string>
-#include <chrono>
-#include <Eigen/Dense>
-
-
-namespace sv {
-
-
-
-// define the tracklet struct to store the tracked objects.
-struct Tracklet
-{
-/* data */
-public:
-    Eigen::Vector4d bbox;  // double x, y, w, h;
-    int id = 0;
-    int age;
-    int hits;
-    int misses;
-    int frame_id = 0;
-    int category_id;
-    bool tentative;
-    std::vector<double> features;
-    Eigen::Matrix<double, 8, 1> mean;
-    Eigen::Matrix<double, 8, 8> covariance;
-};
-
-
-class KalmanFilter {
-public:
-    KalmanFilter();
-    ~KalmanFilter();
-    std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > initiate(Eigen::Vector4d &bbox);
-    std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > update(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances, Box &box);
-    std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > predict(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
-    std::pair<Eigen::Matrix<double, 4, 1>, Eigen::Matrix<double, 4, 4> > project(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
-private:
-    Eigen::Matrix<double, 8, 8> _F;
-    Eigen::Matrix<double, 4, 8> _H;
-    Eigen::Matrix<double, 9, 1> _chi2inv95;
-    double _std_weight_position;
-    double _std_weight_vel;
-};
-
-
-class SORT {
-public:
-    SORT(double iou_threshold, int max_age, int min_hits): _iou_threshold(iou_threshold), _max_age(max_age), _min_hits(min_hits), _next_tracklet_id(0) {};
-    ~SORT();
-    void update(TargetsInFrame &tgts);
-    std::vector<Tracklet> getTracklets() const;
-private:
-    double _iou(Tracklet &tracklet, Box &box);
-    std::vector<std::pair<int,int>> _hungarian(std::vector<std::vector<double>> costMatrix);
-    double _findMin(const std::vector<double>& vec);
-    void _subtractMinFromRows(std::vector<std::vector<double>>& costMatrix);
-    void _subtractMinFromCols(std::vector<std::vector<double>>& costMatrix);
-    //bool _augment(const std::vector<std::vector<double>>& costMatrix, int row, std::vector<int>& rowMatch, std::vector<int>& colMatch, std::vector<bool>& visited);
-
-    double _iou_threshold;
-    int _max_age;
-    int _min_hits;
-    int _next_tracklet_id;
-    std::vector <Tracklet> _tracklets;
-    std::vector <Tracklet> _new_tracklets;
-};
-
-
-}
-#endif

algorithm/mot/sv_mot.cpp

@@ -1,12 +1,8 @@
 #include "sv_mot.h"
 #include <cmath>
 #include <fstream>
-#include <limits>
-#include <vector>
 #include "gason.h"
 #include "sv_util.h"
-#include "sort.h"
-#include "BYTETracker.h"
 
 using namespace std;
 using namespace Eigen;
@@ -18,22 +14,15 @@ MultipleObjectTracker::MultipleObjectTracker()
 {
     this->_params_loaded = false;
     this->_sort_impl = NULL;
-    this->_bytetrack_impl = NULL;
 }
 MultipleObjectTracker::~MultipleObjectTracker()
 {
     if (this->_sort_impl)
         delete this->_sort_impl;
-    else if (this->_bytetrack_impl)
-        delete this->_bytetrack_impl;
 }
 
-sv::TargetsInFrame MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tgts_)
+void MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tgts_)
 {
-    sv::TargetsInFrame person_tgts(tgts_.frame_id);
-    person_tgts.width = img_.size().width;
-    person_tgts.height = img_.size().height;
-
     if (!this->_params_loaded)
     {
         this->_load();
@@ -42,28 +31,8 @@ sv::TargetsInFrame MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tg
     if ("sort" == this->_algorithm && this->_sort_impl)
     {
         this->_detector->detect(img_, tgts_);
-        for (auto target : tgts_.targets)
+        this->_sort_impl->update(tgts_);
-        {
-            if (target.category_id == 0)
-            {
-                person_tgts.targets.push_back(target);
-            }
-        }
-        this->_sort_impl->update(person_tgts);
     }
-    else if ("bytetrack" == this->_algorithm && this->_bytetrack_impl)
-    {
-        this->_detector->detect(img_, tgts_);
-        for (auto target : tgts_.targets)
-        {
-            if (target.category_id == 0)
-            {
-                person_tgts.targets.push_back(target);
-            }
-        }
-        this->_bytetrack_impl->update(person_tgts);
-    }
-    return person_tgts;
 }
 
 void MultipleObjectTracker::init(CommonObjectDetector* detector_)
@@ -77,10 +46,6 @@ void MultipleObjectTracker::init(CommonObjectDetector* detector_)
     {
         this->_sort_impl = new SORT(this->_iou_thres, this->_max_age, this->_min_hits);
     }
-    else if("bytetrack" == this->_algorithm)
-    {
-        this->_bytetrack_impl = new BYTETracker(this->_frame_rate, this->_track_buffer);
-    }
     this->_detector = detector_;
 }
 
@@ -104,7 +69,7 @@ void MultipleObjectTracker::_load()
             else
                 this->_with_reid = false;
             
-            //std::cout << "with_reid: " << this->_with_reid << std::endl;
+            std::cout << "with_reid: " << this->_with_reid << std::endl;
         }
         else if ("reid_input_size" == std::string(i->key) && i->value.getTag() == JSON_ARRAY) {
             int jcnt = 0;
@@ -117,39 +82,353 @@ void MultipleObjectTracker::_load()
                 }
                 jcnt += 1;
             }
-            //std::cout << "reid_input_w: " << this->_reid_input_w << std::endl;
+            std::cout << "reid_input_w: " << this->_reid_input_w << std::endl;
-            //std::cout << "reid_input_h: " << this->_reid_input_h << std::endl;
+            std::cout << "reid_input_h: " << this->_reid_input_h << std::endl;
         }
         else if ("reid_num_samples" == std::string(i->key)) {
             this->_reid_num_samples = i->value.toNumber();
-            //std::cout << "reid_num_samples: " << this->_reid_num_samples << std::endl;
+            std::cout << "reid_num_samples: " << this->_reid_num_samples << std::endl;
         }
         else if ("reid_match_thres" == std::string(i->key)) {
             this->_reid_match_thres = i->value.toNumber();
-            //std::cout << "reid_match_thres: " << this->_reid_match_thres << std::endl;
+            std::cout << "reid_match_thres: " << this->_reid_match_thres << std::endl;
         }
         else if ("iou_thres" == std::string(i->key)) {
             this->_iou_thres = i->value.toNumber();
-            //std::cout << "iou_thres: " << this->_iou_thres << std::endl;
+            std::cout << "iou_thres: " << this->_iou_thres << std::endl;
         }
         else if ("max_age" == std::string(i->key)) {
             this->_max_age = i->value.toNumber();
-            //std::cout << "max_age: " << this->_max_age << std::endl;
+            std::cout << "max_age: " << this->_max_age << std::endl;
         }
         else if ("min_hits" == std::string(i->key)) {
             this->_min_hits = i->value.toNumber();
-            //std::cout << "min_hits: " << this->_min_hits << std::endl;
+            std::cout << "min_hits: " << this->_min_hits << std::endl;
-        }
-        else if ("frame_rate" == std::string(i->key)) {
-            this->_frame_rate = i->value.toNumber();
-            //std::cout << "max_age: " << this->_max_age << std::endl;
-        }
-        else if ("track_buffer" == std::string(i->key)) {
-            this->_track_buffer = i->value.toNumber();
-            //std::cout << "min_hits: " << this->_min_hits << std::endl;
         }
     }
 }
 
+
+
+KalmanFilter::KalmanFilter()
+{
+    this->_chi2inv95 << 3.8415, 5.9915, 7.8147, 9.4877, 11.070, 12.592, 14.067, 15.507, 16.919;
+    this->_F = MatrixXd::Identity(8, 8);
+    for (int i=0; i<4; i++)
+    {
+        this->_F(i,i+4) = 1;
+    }
+    this->_H = MatrixXd::Identity(4, 8);
+    this->_std_weight_position = 1. / 20;
+    this->_std_weight_vel = 1. / 160;
+}
+
+KalmanFilter::~KalmanFilter()
+{
+}
+
+pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
+{
+    Matrix<double,8,1> mean;
+    mean << bbox(0), bbox(1), bbox(2) / bbox(3), bbox(3), 0, 0, 0, 0;
+    VectorXd stds(8);
+    stds << 2 * this->_std_weight_position * mean(3), 2 * this->_std_weight_position * mean(3), 0.01, 2 * this->_std_weight_position * mean(3), \
+        10 * this->_std_weight_vel * mean(3), 10 * this->_std_weight_vel * mean(3), 1e-5, 10 * this->_std_weight_vel * mean(3);
+    MatrixXd squared = stds.array().square();
+    Matrix<double, 8, 8> covariances;
+    covariances = squared.asDiagonal();
+    return make_pair(mean, covariances);
+}
+
+pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances, sv::Box &box)
+{
+    MatrixXd R;
+    Vector4d stds;
+
+    stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.1, this->_std_weight_position * mean(3);
+    MatrixXd squared = stds.array().square();
+    R = squared.asDiagonal();
+    MatrixXd S = this->_H * covariances * this->_H.transpose() + R;    
+
+    MatrixXd Kalman_gain = covariances * this->_H.transpose() * S.inverse();
+    VectorXd measurement(4);
+    measurement << box.x1, box.y1, (box.x2-box.x1) / (box.y2-box.y1), box.y2-box.y1;
+    Matrix<double, 8, 1> new_mean = mean + Kalman_gain * (measurement - this->_H * mean);
+    Matrix<double, 8, 8> new_covariances = (MatrixXd::Identity(8, 8) - Kalman_gain * this->_H) * covariances;
+    return make_pair(new_mean, new_covariances);    
+}
+
+pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::predict(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances)
+{
+    VectorXd stds(8);
+    stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.01, this->_std_weight_position * mean(3), \
+        this->_std_weight_vel * mean(3), this->_std_weight_vel * mean(3), 1e-5, this->_std_weight_vel * mean(3);
+    MatrixXd squared = stds.array().square();
+    MatrixXd Q = squared.asDiagonal();
+    Matrix<double, 8, 1> pre_mean = this->_F * mean;
+    Matrix<double, 8, 8> pre_cov = this->_F * covariances * this->_F.transpose() + Q;
+    return make_pair(pre_mean, pre_cov);
+}
+
+
+SORT::~SORT()
+{
+}
+
+void SORT::update(TargetsInFrame& tgts)
+{
+    sv::KalmanFilter kf;
+    if (! this->_tracklets.size())
+    {
+        Vector4d bbox;
+        for (int i=0; i<tgts.targets.size(); i++)
+        {
+            sv::Box box;
+            tgts.targets[i].getBox(box);		
+            Tracklet tracklet;
+            tracklet.id = ++ this->_next_tracklet_id;
+            // cout << tracklet.id << endl;
+            tgts.targets[i].tracked_id = this->_next_tracklet_id;
+            tgts.targets[i].has_tid = true;
+
+            tracklet.bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1;  // x,y,w,h
+            tracklet.age = 0;
+            tracklet.hits = 1;
+            tracklet.misses = 0;
+            tracklet.frame_id = tgts.frame_id;
+            tracklet.tentative = true;
+            // initate the motion
+            pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.initiate(tracklet.bbox);
+            tracklet.mean = motion.first;
+            tracklet.covariance = motion.second;
+
+            this->_tracklets.push_back(tracklet);
+        }
+    }
+    else
+    {
+        for (int i=0; i<tgts.targets.size(); i++)
+        {
+            tgts.targets[i].tracked_id = 0;
+            tgts.targets[i].has_tid = true;
+        }
+
+        array<int, 100> match_det;
+        match_det.fill(-1);
+        // predict the next state of each tracklet
+        for (auto& tracklet : this->_tracklets)
+        {
+            tracklet.age++;
+            pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.predict(tracklet.mean, tracklet.covariance);
+            tracklet.bbox << motion.first(0), motion.first(1), motion.first(2) * motion.first(3), motion.first(3);
+            tracklet.mean = motion.first;
+            tracklet.covariance = motion.second;    
+        }
+        
+        // Match the detections to the existing tracklets 
+        // cout << "the num of targets: " << tgts.targets.size() << endl;
+        // cout << "the num of tracklets: " << this->_tracklets.size() << endl;
+        vector<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
+        for (int i=0; i<this->_tracklets.size(); i++)
+        {
+            for (int j=0; j<tgts.targets.size(); j++)
+            {
+                sv::Box box;
+                tgts.targets[j].getBox(box);
+                iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
+            }
+        }
+        vector<pair<int, int> > matches = this->_hungarian(iouMatrix);
+        for (auto& match : matches)
+        {
+            int trackletIndex = match.first;
+            int detectionIndex = match.second;
+            if (trackletIndex >= 0 && detectionIndex >= 0)
+            {
+                if (iouMatrix[match.first][match.second] >= _iou_threshold)  // iou_thrshold
+                {
+                    sv::Box box;
+                    tgts.targets[detectionIndex].getBox(box);
+                    this->_tracklets[trackletIndex].age = 0;
+                    this->_tracklets[trackletIndex].hits++;
+                    this->_tracklets[trackletIndex].frame_id = tgts.frame_id;
+                    this->_tracklets[trackletIndex].bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1;
+
+                    auto[mean, covariance] = kf.update(this->_tracklets[trackletIndex].mean, this->_tracklets[trackletIndex].covariance, box);
+                    this->_tracklets[trackletIndex].mean = mean;
+                    this->_tracklets[trackletIndex].covariance = covariance;
+
+                    tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id;
+                    match_det[detectionIndex] = detectionIndex;
+                }
+            }
+        }
+        // create new tracklets for unmatched detections
+        for (int i=0; i<tgts.targets.size(); i++)
+        { 
+            if (match_det[i] == -1)
+            {
+                sv::Box box;
+                tgts.targets[i].getBox(box);
+                Tracklet tracklet;
+                tracklet.id = ++ this->_next_tracklet_id;
+                tracklet.bbox << box.x1, box.y1, box.x2-box.x1, box.y2-box.y1;
+
+                tracklet.age = 0;
+                tracklet.hits = 1;
+                tracklet.misses = 0;
+                tracklet.frame_id = tgts.frame_id;
+                tracklet.tentative = true;
+                
+                auto[new_mean, new_covariance] = kf.initiate(tracklet.bbox);
+                tracklet.mean = new_mean;
+                tracklet.covariance = new_covariance;
+                
+                tgts.targets[i].tracked_id = this->_next_tracklet_id;
+                tgts.targets[i].has_tid = true;
+                this->_tracklets.push_back(tracklet);
+            }
+        }
+        for (auto& tracklet : this->_tracklets)
+        {
+            if (tracklet.hits >= _min_hits)
+            {
+                tracklet.tentative = false;
+            }
+            if ((tgts.frame_id-tracklet.frame_id <= _max_age) || (!tracklet.tentative && tracklet.frame_id == tgts.frame_id))
+            {
+                _new_tracklets.push_back(tracklet);
+            } 
+        }
+        _tracklets = _new_tracklets;
+        std::vector <Tracklet> ().swap(_new_tracklets);
+    }
+}
+
+vector<Tracklet> SORT::getTracklets() const
+{
+    return this->_tracklets;
+}
+
+double SORT::_iou(Tracklet& tracklet, sv::Box& box)
+{
+    double trackletX1 = tracklet.bbox(0);
+    double trackletY1 = tracklet.bbox(1);
+    double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2);
+    double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3);
+
+    double detectionX1 = box.x1;
+    double detectionY1 = box.y1;
+    double detectionX2 = box.x2;
+    double detectionY2 = box.y2;
+    double intersectionX1 = max(trackletX1, detectionX1);
+    double intersectionY1 = max(trackletY1, detectionY1);
+    double intersectionX2 = min(trackletX2, detectionX2);
+    double intersectionY2 = min(trackletY2, detectionY2);
+
+    double w = (intersectionX2-intersectionX1 > 0.0) ? (intersectionX2-intersectionX1) : 0.0;
+    double h = (intersectionY2-intersectionY1 > 0.0) ? (intersectionY2-intersectionY1) : 0.0;
+    double intersectionArea = w * h;
+
+    double trackletArea = tracklet.bbox(2) * tracklet.bbox(3);
+
+    double detectionArea = (box.x2-box.x1) * (box.y2-box.y1);
+    double unionArea = trackletArea + detectionArea - intersectionArea;
+    double iou = (1 - intersectionArea / unionArea * 1.0);
+
+    return iou;
+}
+
+vector<pair<int, int> > SORT::_hungarian(vector<vector<double> > costMatrix)
+{
+    int numRows = costMatrix.size();
+    int numCols = costMatrix[0].size();
+
+    const bool transposed = numCols > numRows;
+    // transpose the matrix if necessary
+    if (transposed)
+    {
+        vector<vector<double> > transposedMatrix(numCols, vector<double>(numRows));
+        for (int i=0; i<numRows; i++)
+        {
+            for (int j=0; j<numCols; j++)
+            {
+                transposedMatrix[j][i] = costMatrix[i][j];
+            }
+        }
+        costMatrix = transposedMatrix;
+        swap(numRows, numCols);
+    }
+    vector<double>rowMin(numRows, numeric_limits<double>::infinity());
+    vector<double>colMin(numCols, numeric_limits<double>::infinity());
+    vector<int>rowMatch(numRows, -1);
+    vector<int>colMatch(numCols, -1);
+    vector<pair<int, int> > matches;
+    // step1: Subtract the row minimums from each row
+    for (int i=0; i<numRows; i++)
+    {
+        for (int j=0; j<numCols; j++)
+        {
+            rowMin[i] = min(rowMin[i], costMatrix[i][j]);
+        }
+        for (int j=0; j<numCols; j++)
+        {
+            costMatrix[i][j] -= rowMin[i];
+        }
+    }
+    // step2: substract the colcum minimums from each column
+    for (int j=0; j<numCols; j++)
+    {
+        for (int i=0; i<numRows; i++)
+        {
+            colMin[j] = min(colMin[j], costMatrix[i][j]);
+        }
+        for (int i=0; i<numRows; i++)
+        {
+            costMatrix[i][j] -= colMin[j];
+        }
+    }
+    // step3: find a maximal matching
+    for (int i=0; i<numRows; i++) 
+    {
+        vector<bool> visited(numCols, false);
+        this->_augment(costMatrix, i, rowMatch, colMatch, visited);
+    }
+    // step4: calculate the matches
+    matches.clear();
+    for (int j=0; j<numCols; j++)
+    {
+        matches.push_back(make_pair(colMatch[j], j));
+    }
+    if (transposed)
+    {
+        for (auto& match : matches)
+        {
+            swap(match.first, match.second);
+        }
+    }
+    return matches;
+}
+
+bool SORT::_augment(const vector<vector<double> >& costMatrix, int row, vector<int>& rowMatch, vector<int>& colMatch, vector<bool>& visited) 
+{
+    int numCols = costMatrix[0].size();
+    for (int j=0; j<numCols; j++)
+    {
+        if (costMatrix[row][j] == 0 && !visited[j])
+        {
+            visited[j] = true;
+            if (colMatch[j] == -1 || this->_augment(costMatrix, colMatch[j], rowMatch, colMatch, visited))
+            {
+                rowMatch[row] = j;
+                colMatch[j] = row;
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+
 }
 

algorithm/sot/ocv470/sot_ocv470_impl.cpp

@@ -1,7 +1,6 @@
 #include "sot_ocv470_impl.h"
 #include <cmath>
 #include <fstream>
-#include "sv_util.h"
 
 #define SV_MODEL_DIR "/SpireCV/models/"
 #define SV_ROOT_DIR "/SpireCV/"
@@ -31,39 +30,9 @@ bool SingleObjectTrackerOCV470Impl::ocv470Setup(SingleObjectTrackerBase* base_)
   std::string net = get_home() + SV_MODEL_DIR + "dasiamrpn_model.onnx";
   std::string kernel_cls1 = get_home() + SV_MODEL_DIR + "dasiamrpn_kernel_cls1.onnx";
   std::string kernel_r1 = get_home() + SV_MODEL_DIR + "dasiamrpn_kernel_r1.onnx";
-  
-  std::vector<std::string> files1, files2, files3;
-  _list_dir(get_home() + SV_MODEL_DIR, files1, ".onnx", "Ocv-DaSiamRPN-Model-");
-  _list_dir(get_home() + SV_MODEL_DIR, files2, ".onnx", "Ocv-DaSiamRPN-Kernel-CLS1-");
-  _list_dir(get_home() + SV_MODEL_DIR, files3, ".onnx", "Ocv-DaSiamRPN-Kernel-R1-");
-  if (files1.size() > 0 && files2.size() > 0 && files3.size() > 0)
-  {
-    std::sort(files1.rbegin(), files1.rend(), _comp_str_lesser);
-    std::sort(files2.rbegin(), files2.rend(), _comp_str_lesser);
-    std::sort(files3.rbegin(), files3.rend(), _comp_str_lesser);
-    net = get_home() + SV_MODEL_DIR + files1[0];
-    kernel_cls1 = get_home() + SV_MODEL_DIR + files2[0];
-    kernel_r1 = get_home() + SV_MODEL_DIR + files3[0];
-  }
-  std::cout << "Load: " << net << std::endl;
-  std::cout << "Load: " << kernel_cls1 << std::endl;
-  std::cout << "Load: " << kernel_r1 << std::endl;
 
   std::string backbone = get_home() + SV_MODEL_DIR + "nanotrack_backbone_sim.onnx";
   std::string neckhead = get_home() + SV_MODEL_DIR + "nanotrack_head_sim.onnx";
-  
-  std::vector<std::string> files4, files5;
-  _list_dir(get_home() + SV_MODEL_DIR, files4, ".onnx", "Ocv-NanoTrack-Backbone-SIM-");
-  _list_dir(get_home() + SV_MODEL_DIR, files5, ".onnx", "Ocv-NanoTrack-Head-SIM-");
-  if (files4.size() > 0 && files5.size() > 0)
-  {
-    std::sort(files4.rbegin(), files4.rend(), _comp_str_lesser);
-    std::sort(files5.rbegin(), files5.rend(), _comp_str_lesser);
-    backbone = get_home() + SV_MODEL_DIR + files4[0];
-    neckhead = get_home() + SV_MODEL_DIR + files5[0];
-  }
-  std::cout << "Load: " << backbone << std::endl;
-  std::cout << "Load: " << neckhead << std::endl;
 
   try
   {

algorithm/veri/cuda/veri_det_cuda_impl.cpp

@@ -1,200 +0,0 @@
-#include "veri_det_cuda_impl.h"
-#include <cmath>
-#include <fstream>
-#include "sv_util.h"
-
-#define SV_MODEL_DIR "/SpireCV/models/"
-#define SV_ROOT_DIR "/SpireCV/"
-
-#ifdef WITH_CUDA
-#include "yolov7/logging.h"
-#define TRTCHECK(status)                                 \
-  do                                                     \
-  {                                                      \
-    auto ret = (status);                                 \
-    if (ret != 0)                                        \
-    {                                                    \
-      std::cerr << "Cuda failure: " << ret << std::endl; \
-      abort();                                           \
-    }                                                    \
-  } while (0)
-
-#define DEVICE 0 // GPU id
-#define BATCH_SIZE 1
-
-#define MAX_IMAGE_INPUT_SIZE_THRESH 3000 * 3000 // ensure it exceed the maximum size in the input images !
-#endif
-
-#include <iostream>
-#include <cmath>
-int BAT = 1;
-float cosineSimilarity(float *vec1, float *vec2, int size)
-{
-  // 计算向量的点积
-  float dotProduct = 0.0f;
-  for (int i = 0; i < size; ++i)
-  {
-    dotProduct += vec1[i] * vec2[i];
-  }
-
-  // 计算向量的模长
-  float magnitudeVec1 = 0.0f;
-  float magnitudeVec2 = 0.0f;
-  for (int i = 0; i < size; ++i)
-  {
-    magnitudeVec1 += vec1[i] * vec1[i];
-    magnitudeVec2 += vec2[i] * vec2[i];
-  }
-  magnitudeVec1 = std::sqrt(magnitudeVec1);
-  magnitudeVec2 = std::sqrt(magnitudeVec2);
-
-  // 计算余弦相似性
-  float similarity = dotProduct / (magnitudeVec1 * magnitudeVec2);
-
-  return similarity;
-}
-
-namespace sv
-{
-
-  using namespace cv;
-
-#ifdef WITH_CUDA
-  using namespace nvinfer1;
-  static Logger g_nvlogger;
-#endif
-
-  VeriDetectorCUDAImpl::VeriDetectorCUDAImpl()
-  {
-  }
-
-  VeriDetectorCUDAImpl::~VeriDetectorCUDAImpl()
-  {
-  }
-
-  bool VeriDetectorCUDAImpl::cudaSetup()
-  {
-#ifdef WITH_CUDA
-    std::string trt_model_fn = get_home() + SV_MODEL_DIR + "veri.engine";
-    
-    std::vector<std::string> files;
-    _list_dir(get_home() + SV_MODEL_DIR, files, "-online.engine", "Nv-VERI-mobilenet_v3");
-    if (files.size() > 0)
-    {
-      std::sort(files.rbegin(), files.rend(), _comp_str_lesser);
-      trt_model_fn = get_home() + SV_MODEL_DIR + files[0];
-    }
-    std::cout << "Load: " << trt_model_fn << std::endl;
-    
-    if (!is_file_exist(trt_model_fn))
-    {
-      throw std::runtime_error("SpireCV (104) Error loading the VeriDetector TensorRT model (File Not Exist)");
-    }
-    char *trt_model_stream{nullptr};
-    size_t trt_model_size{0};
-    try
-    {
-      std::ifstream file(trt_model_fn, std::ios::binary);
-      file.seekg(0, file.end);
-      trt_model_size = file.tellg();
-      file.seekg(0, file.beg);
-      trt_model_stream = new char[trt_model_size];
-      assert(trt_model_stream);
-      file.read(trt_model_stream, trt_model_size);
-      file.close();
-    }
-    catch (const std::runtime_error &e)
-    {
-      throw std::runtime_error("SpireCV (104) Error loading the TensorRT model!");
-    }
-
-    // TensorRT
-    IRuntime *runtime = nvinfer1::createInferRuntime(g_nvlogger);
-    assert(runtime != nullptr);
-    ICudaEngine *p_cu_engine = runtime->deserializeCudaEngine(trt_model_stream, trt_model_size);
-    assert(p_cu_engine != nullptr);
-    this->_trt_context = p_cu_engine->createExecutionContext();
-    assert(this->_trt_context != nullptr);
-
-    delete[] trt_model_stream;
-    const ICudaEngine &cu_engine = this->_trt_context->getEngine();
-    assert(cu_engine.getNbBindings() == 3);
-
-    this->_input_index = cu_engine.getBindingIndex("input");
-    this->_output_index1 = cu_engine.getBindingIndex("output");
-    this->_output_index2 = cu_engine.getBindingIndex("/head/layers.0/act/Mul_output_0");
-    TRTCHECK(cudaMalloc(&_p_buffers[this->_input_index], 2 * 3 * 224 * 224 * sizeof(float)));
-    TRTCHECK(cudaMalloc(&_p_buffers[this->_output_index1], 2 * 576 * sizeof(float)));
-    TRTCHECK(cudaMalloc(&_p_buffers[this->_output_index2], 2 * 1280 * sizeof(float)));
-    TRTCHECK(cudaStreamCreate(&_cu_stream));
-
-    auto input_dims = nvinfer1::Dims4{2, 3, 224, 224};
-    this->_trt_context->setBindingDimensions(this->_input_index, input_dims);
-
-    this->_p_data = new float[2 * 3 * 224 * 224];
-    this->_p_prob1 = new float[2 * 576];
-    this->_p_prob2 = new float[2 * 1280];
-    // Input
-    TRTCHECK(cudaMemcpyAsync(_p_buffers[this->_input_index], this->_p_data, 2 * 3 * 224 * 224 * sizeof(float), cudaMemcpyHostToDevice, this->_cu_stream));
-    // this->_trt_context->enqueue(1, _p_buffers, this->_cu_stream, nullptr);
-    this->_trt_context->enqueueV2(_p_buffers, this->_cu_stream, nullptr);
-    // Output
-    TRTCHECK(cudaMemcpyAsync(this->_p_prob1, _p_buffers[this->_output_index1], 2 * 576 * sizeof(float), cudaMemcpyDeviceToHost, this->_cu_stream));
-    TRTCHECK(cudaMemcpyAsync(this->_p_prob2, _p_buffers[this->_output_index2], 2 * 1280 * sizeof(float), cudaMemcpyDeviceToHost, this->_cu_stream));
-    cudaStreamSynchronize(this->_cu_stream);
-    return true;
-#endif
-    return false;
-  }
-
-  void VeriDetectorCUDAImpl::cudaRoiCNN(
-      std::vector<cv::Mat> &input_rois_,
-      std::vector<float> &output_labels_)
-  {
-#ifdef WITH_CUDA
-
-
-    for (int i = 0; i < 2; i++)
-    {
-      for (int row = 0; row < 224; ++row)
-      {
-        uchar *uc_pixel = input_rois_[i].data + row * input_rois_[i].step; // compute row id
-        for (int col = 0; col < 224; ++col)
-        {
-          // mean=[136.20, 141.50, 145.41], std=[44.77, 44.20, 44.30]
-          this->_p_data[col + row * 224 + 224 * 224 * 3 * i] = ((float)uc_pixel[0] - 136.20f) / 44.77f;
-          this->_p_data[col + row * 224 + 224 * 224  + 224 * 224 * 3 * i] = ((float)uc_pixel[1] - 141.50f) / 44.20f;
-          this->_p_data[col + row * 224 + 224 * 224 * 2  + 224 * 224 * 3 * i] = ((float)uc_pixel[2] - 145.41f) / 44.30f;
-          uc_pixel += 3;
-        }
-      }
-    }
-
-
-    // Input
-    TRTCHECK(cudaMemcpyAsync(_p_buffers[this->_input_index], this->_p_data, 2 * 3 * 224 * 224 * sizeof(float), cudaMemcpyHostToDevice, this->_cu_stream));
-    // this->_trt_context->enqueue(1, _p_buffers, this->_cu_stream, nullptr);
-    this->_trt_context->enqueueV2(_p_buffers, this->_cu_stream, nullptr);
-    // Output
-    TRTCHECK(cudaMemcpyAsync(this->_p_prob1, _p_buffers[this->_output_index1], 2 * 576 * sizeof(float), cudaMemcpyDeviceToHost, this->_cu_stream));
-    TRTCHECK(cudaMemcpyAsync(this->_p_prob2, _p_buffers[this->_output_index2], 2 * 1280 * sizeof(float), cudaMemcpyDeviceToHost, this->_cu_stream));
-    cudaStreamSynchronize(this->_cu_stream);
-
-    // Find max index
-    double max = 0;
-    int label = 0;
-    for (int i = 0; i < 576; ++i)
-    {
-      if (max < this->_p_prob1[i])
-      {
-        max = this->_p_prob1[i];
-        label = i;
-      }
-    }
-
-    float similarity = cosineSimilarity(this->_p_prob2, this->_p_prob2 + 1280, 1280);
-    output_labels_.push_back(label);
-    output_labels_.push_back(similarity);
-  }
-#endif
-}

algorithm/veri/cuda/veri_det_cuda_impl.h

@@ -1,50 +0,0 @@
-#ifndef __SV_VERI_DET_CUDA__
-#define __SV_VERI_DET_CUDA__
-
-#include "sv_core.h"
-#include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
-#include <opencv2/tracking.hpp>
-#include <string>
-#include <chrono>
-
-
-
-#ifdef WITH_CUDA
-#include <NvInfer.h>
-#include <cuda_runtime_api.h>
-#endif
-
-
-
-namespace sv {
-
-
-class VeriDetectorCUDAImpl
-{
-public:
-  VeriDetectorCUDAImpl();
-  ~VeriDetectorCUDAImpl();
-
-  bool cudaSetup();
-  void cudaRoiCNN(
-    std::vector<cv::Mat>& input_rois_,
-    std::vector<float>& output_labels_
-  );
-
-#ifdef WITH_CUDA
-  float *_p_data;
-  float *_p_prob1;
-  float *_p_prob2;
-  nvinfer1::IExecutionContext *_trt_context;
-  int _input_index;
-  int _output_index1;
-  int _output_index2;
-  void *_p_buffers[3];
-  cudaStream_t _cu_stream;
-#endif
-};
-
-
-}
-#endif

algorithm/veri/intel/veri_det_intel_impl.cpp

@@ -1,112 +0,0 @@
-#include "veri_det_intel_impl.h"
-#include <cmath>
-#include <fstream>
-
-#define SV_MODEL_DIR "/SpireCV/models/"
-#define SV_ROOT_DIR "/SpireCV/"
-
-#include <iostream>
-#include <cmath>
-int BAT = 1;
-float cosineSimilarity(float *vec1, float *vec2, int size)
-{
-  // 计算向量的点积
-  float dotProduct = 0.0f;
-  for (int i = 0; i < size; ++i)
-  {
-    dotProduct += vec1[i] * vec2[i];
-  }
-
-  // 计算向量的模长
-  float magnitudeVec1 = 0.0f;
-  float magnitudeVec2 = 0.0f;
-  for (int i = 0; i < size; ++i)
-  {
-    magnitudeVec1 += vec1[i] * vec1[i];
-    magnitudeVec2 += vec2[i] * vec2[i];
-  }
-  magnitudeVec1 = std::sqrt(magnitudeVec1);
-  magnitudeVec2 = std::sqrt(magnitudeVec2);
-
-  // 计算余弦相似性
-  float similarity = dotProduct / (magnitudeVec1 * magnitudeVec2);
-
-  return similarity;
-}
-
-namespace sv
-{
-#ifdef WITH_INTEL
-  using namespace cv;
-  using namespace std;
-  using namespace dnn;
-#endif
-
-  VeriDetectorIntelImpl::VeriDetectorIntelImpl()
-  {
-  }
-
-  VeriDetectorIntelImpl::~VeriDetectorIntelImpl()
-  {
-  }
-
-  bool VeriDetectorIntelImpl::intelSetup()
-  {
-#ifdef WITH_INTEL
-    std::string onnx_model_fn = get_home() + SV_MODEL_DIR + "veri.onnx";
-    if (!is_file_exist(onnx_model_fn))
-    {
-      throw std::runtime_error("SpireCV (104) Error loading the VeriDetector openVINO model (File Not Exist)");
-    }
-
-    // OpenVINO
-    ov::Core core;
-    this->compiled_model = core.compile_model(onnx_model_fn, "GPU");
-    this->infer_request = compiled_model.create_infer_request();
-
-    return true;
-#endif
-    return false;
-  }
-
-  void VeriDetectorIntelImpl::intelRoiCNN(
-      std::vector<cv::Mat> &input_rois_,
-      std::vector<float> &output_labels_)
-  {
-#ifdef WITH_INTEL
-
-    Mat blobs;
-    blobFromImages(input_rois_, blobs, 1 / 255.0, Size(224, 224), Scalar(0, 0, 0), true, true);
-
-    auto input_port = compiled_model.input();
-    ov::Tensor input_tensor(input_port.get_element_type(), input_port.get_shape(), blobs.ptr(0));
-
-    infer_request.infer();
-
-    const ov::Tensor &label_pre = infer_request.get_output_tensor(0);
-    this->_p_prob1 = label_pre.data<float>();
-
-    const ov::Tensor &proto_pre = infer_request.get_output_tensor(1);
-    this->_p_prob2 = proto_pre.data<float>();
-
-    // Find max index
-    double max = 0;
-    int label = 0;
-    for (int i = 0; i < 576; ++i)
-    {
-      if (max < this->_p_prob1[i])
-      {
-        max = this->_p_prob1[i];
-        label = i;
-      }
-    }
-
-    float similarity = cosineSimilarity(this->_p_prob2, this->_p_prob2 + 1280, 1280);
-    
-    output_labels_.push_back(label);
-    output_labels_.push_back(similarity);
-#endif
-  }
-
-
-}

algorithm/veri/intel/veri_det_intel_impl.h

@@ -1,41 +0,0 @@
-#ifndef __SV_VERI_DET_INTEL__
-#define __SV_VERI_DET_INTEL__
-
-#include "sv_core.h"
-#include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
-#include <opencv2/tracking.hpp>
-#include <string>
-#include <chrono>
-
-#ifdef WITH_INTEL
-#include <openvino/openvino.hpp>
-#endif
-
-namespace sv
-{
-  class VeriDetectorIntelImpl
-  {
-  public:
-    VeriDetectorIntelImpl();
-    ~VeriDetectorIntelImpl();
-
-    bool intelSetup();
-    void intelRoiCNN(
-        std::vector<cv::Mat> &input_rois_,
-        std::vector<float> &output_labels_);
-
-#ifdef WITH_INTEL
-
-    float *_p_data;
-    float *_p_prob1;
-    float *_p_prob2;
-
-    ov::Tensor input_tensor;
-    ov::InferRequest infer_request;
-    ov::CompiledModel compiled_model;
-#endif
-  };
-
-}
-#endif

algorithm/veri/sv_veri_det.cpp

@@ -1,168 +0,0 @@
-#include "sv_veri_det.h"
-#include <cmath>
-#include <fstream>
-#include "gason.h"
-#include "sv_util.h"
-
-#ifdef WITH_CUDA
-#include <NvInfer.h>
-#include <cuda_runtime_api.h>
-#include "veri_det_cuda_impl.h"
-#endif
-
-#ifdef WITH_INTEL
-#include <openvino/openvino.hpp>
-#include "veri_det_intel_impl.h"
-#endif
-
-#define SV_ROOT_DIR "/SpireCV/"
-
-namespace sv
-{
-
-  VeriDetector::VeriDetector()
-  {
-#ifdef WITH_CUDA
-    this->_cuda_impl = new VeriDetectorCUDAImpl;
-#endif
-
-#ifdef WITH_INTEL
-    this->_intel_impl = new VeriDetectorIntelImpl;
-#endif
-  }
-  VeriDetector::~VeriDetector()
-  {
-  }
-
-  void VeriDetector::_load()
-  {
-    JsonValue all_value;
-    JsonAllocator allocator;
-    _load_all_json(this->alg_params_fn, all_value, allocator);
-
-    JsonValue veriliner_params_value;
-    _parser_algorithm_params("VeriDetector", all_value, veriliner_params_value);
-
-    for (auto i : veriliner_params_value)
-    {
-      if ("vehicle_ID" == std::string(i->key))
-      {
-        this->vehicle_id = i->value.toString();
-        std::cout << "vehicle_ID Load Sucess!" << std::endl;
-      }
-    }
-  }
-
-  bool VeriDetector::setupImpl()
-  {
-#ifdef WITH_CUDA
-    return this->_cuda_impl->cudaSetup();
-#endif
-  
-#ifdef WITH_INTEL
-    return this->_intel_impl->intelSetup();
-#endif
-    return false;
-  }
-
-
-  void VeriDetector::roiCNN(
-      std::vector<cv::Mat> &input_rois_,
-      std::vector<float> &output_labels_)
-  {
-#ifdef WITH_CUDA
-    this->_cuda_impl->cudaRoiCNN(
-        input_rois_,
-        output_labels_);
-#endif
-
-#ifdef WITH_INTEL
-    this->_intel_impl->intelRoiCNN(
-        input_rois_,
-        output_labels_);
-#endif
-  }
-
-  void VeriDetector::detect(cv::Mat img_, const cv::Rect &bounding_box_, sv::Target &tgt)
-  {
-    if (!_params_loaded)
-    {
-      this->_load();
-      this->_loadLabels();
-      _params_loaded = true;
-    }
-
-    // convert Rect2d from left-up to center.
-    targetPos[0] = float(bounding_box_.x) + float(bounding_box_.width) * 0.5f;
-    targetPos[1] = float(bounding_box_.y) + float(bounding_box_.height) * 0.5f;
-
-    targetSz[0] = float(bounding_box_.width);
-    targetSz[1] = float(bounding_box_.height);
-
-    // Extent the bounding box.
-    float sumSz = targetSz[0] + targetSz[1];
-    float wExtent = targetSz[0] + 0.5 * (sumSz);
-    float hExtent = targetSz[1] + 0.5 * (sumSz);
-    int sz = int(cv::sqrt(wExtent * hExtent));
-
-    cv::Mat crop;
-    getSubwindow(crop, img_, sz, 224);
-
-    std::string img_ground_dir = get_home()  + SV_ROOT_DIR + this->vehicle_id;
-    cv::Mat img_ground = cv::imread(img_ground_dir);
-    cv::resize(img_ground, img_ground, cv::Size(224, 224));
-    std::vector<cv::Mat> input_rois_ = {crop, img_ground};
-    
-    std::vector<float> output_labels;
-#ifdef WITH_CUDA
-    roiCNN(input_rois_, output_labels);
-#endif
-
-#ifdef WITH_INTEL
-    roiCNN(input_rois_, output_labels);
-#endif
-   
-    if (output_labels.size() > 0)
-    {
-      tgt.sim_score = output_labels[1];
-    }
-  }
-
-  void VeriDetector::getSubwindow(cv::Mat &dstCrop, cv::Mat &srcImg, int originalSz, int resizeSz)
-  {
-    cv::Scalar avgChans = mean(srcImg);
-    cv::Size imgSz = srcImg.size();
-    int c = (originalSz + 1) / 2;
-
-    int context_xmin = (int)(targetPos[0]) - c;
-    int context_xmax = context_xmin + originalSz - 1;
-    int context_ymin = (int)(targetPos[1]) - c;
-    int context_ymax = context_ymin + originalSz - 1;
-
-    int left_pad = std::max(0, -context_xmin);
-    int top_pad = std::max(0, -context_ymin);
-    int right_pad = std::max(0, context_xmax - imgSz.width + 1);
-    int bottom_pad = std::max(0, context_ymax - imgSz.height + 1);
-
-    context_xmin += left_pad;
-    context_xmax += left_pad;
-    context_ymin += top_pad;
-    context_ymax += top_pad;
-
-    cv::Mat cropImg;
-    if (left_pad == 0 && top_pad == 0 && right_pad == 0 && bottom_pad == 0)
-    {
-      // Crop image without padding.
-      cropImg = srcImg(cv::Rect(context_xmin, context_ymin,
-                                context_xmax - context_xmin + 1, context_ymax - context_ymin + 1));
-    }
-    else // Crop image with padding, and the padding value is avgChans
-    {
-      cv::Mat tmpMat;
-      cv::copyMakeBorder(srcImg, tmpMat, top_pad, bottom_pad, left_pad, right_pad, cv::BORDER_CONSTANT, avgChans);
-      cropImg = tmpMat(cv::Rect(context_xmin, context_ymin, context_xmax - context_xmin + 1, context_ymax - context_ymin + 1));
-    }
-    resize(cropImg, dstCrop, cv::Size(resizeSz, resizeSz));
-  }
-
-}

calib_webcam_1280x720.yaml

@@ -0,0 +1,20 @@
+%YAML:1.0
+---
+calibration_time: "2021年01月12日 星期二 18时08分01秒"
+image_width: 1280
+image_height: 720
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 7.9379415710551370e+02, 0., 2.9783879354295328e+02, 0.,
+       7.9491985564466654e+02, 3.0942416136837386e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 2.0950200339181715e-01, -1.1587468096518483e+00,
+       5.5342063671841328e-03, 2.2214393775334758e-04,
+       1.7127431916651392e+00 ]
+avg_reprojection_error: 2.8342964851391211e-01

calib_webcam_640x480.yaml

@@ -0,0 +1,20 @@
+%YAML:1.0
+---
+calibration_time: "2023年07月14日 星期五 16时39分17秒"
+image_width: 640
+image_height: 480
+flags: 0
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 4.5099311307542973e+02, 0., 3.2898947972890943e+02, 0.,
+       6.0215873600107579e+02, 2.4195307609106428e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 1.0737258446369682e-01, -1.2782122264046064e-01,
+       1.6844258609297487e-03, -6.6256775118868144e-04,
+       -3.5333889479158398e-01 ]
+avg_reprojection_error: 3.3968000452388564e-01

gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h

@@ -1,38 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2022-10-27 18:10:06
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:30:27
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_crc32.h
- */
-#ifndef AT10_GIMBAL_CRC32_H
-#define AT10_GIMBAL_CRC32_H
-
-namespace AT10
-{
-    static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght)
-    {
-        unsigned short temp = 0;
-        unsigned short i = 0;
-        for (i = 0; i < Lenght; i++)
-        {
-            temp += pData[i];
-        }
-        return temp & 0XFF;
-    }
-
-    static inline unsigned char checkXOR(unsigned char *pData, unsigned char Lenght)
-    {
-        unsigned char temp = Lenght;
-        unsigned char i;
-        for (i = 1; i < Lenght - 1; i++)
-        {
-            temp ^= pData[i];
-        }
-        return temp;
-    }
-
-} // namespace name
-
-#endif

gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp

@@ -1,404 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2022-10-27 18:10:06
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-06 10:27:59
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.cpp
- */
-#include "AT10_gimbal_driver.h"
-#include "AT10_gimbal_crc32.h"
-#include "string.h"
-
-/**
- * The function creates a new instance of the g1GimbalDriver class, which is a subclass of the
- * IamovGimbalBase class
- *
- * @param _IO The IOStreamBase object that will be used to communicate with the gimbal.
- */
-AT10GimbalDriver::AT10GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO)
-{
-    rxQueue = new fifoRing(sizeof(AT10::GIMBAL_EXTEND_FRAME_T), MAX_QUEUE_SIZE);
-    txQueue = new fifoRing(sizeof(AT10::GIMBAL_EXTEND_FRAME_T), MAX_QUEUE_SIZE);
-
-    stdRxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE);
-    stdTxQueue = new fifoRing(sizeof(AT10::GIMBAL_STD_FRAME_T), MAX_QUEUE_SIZE);
-    parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-}
-
-/**
- * The function takes a command, a pointer to a payload, and the size of the payload. It then copies
- * the payload into the tx buffer, calculates the checksum, and then calculates the CRC32 of the
- * payload. It then copies the CRC32 into the tx buffer, and then copies the tx buffer into the txQueue
- *
- * @param uint32_t 4 bytes
- * @param pPayload pointer to the data to be sent
- * @param payloadSize the size of the payload
- *
- * @return The size of the data to be sent.
- */
-uint32_t AT10GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize)
-{
-    uint32_t ret = 0;
-
-    if (cmd > 0XFF)
-    {
-        AT10::GIMBAL_EXTEND_FRAME_T txTemp;
-        txTemp.head = cmd;
-        memcpy(txTemp.data, pPayload, payloadSize);
-        payloadSize--;
-        txTemp.len = payloadSize;
-        if (txQueue->inCell(&txTemp))
-        {
-            ret = payloadSize + sizeof(uint32_t) + sizeof(uint8_t);
-        }
-    }
-    else
-    {
-        AT10::GIMBAL_STD_FRAME_T txTemp;
-        txTemp.head = AT10::GIMBAL_CMD_STD;
-        txTemp.len = payloadSize + 3;
-        txTemp.cmd = cmd;
-        memcpy(txTemp.data, pPayload, payloadSize);
-        txTemp.data[payloadSize] = AT10::checkXOR((uint8_t *)&txTemp.len, txTemp.len);
-        if (stdTxQueue->inCell(&txTemp))
-        {
-            ret = payloadSize + 6;
-        }
-    }
-
-    return ret;
-}
-
-void AT10GimbalDriver::convert(void *buf)
-{
-    AT10::GIMBAL_EXTEND_FRAME_T *temp;
-    temp = reinterpret_cast<AT10::GIMBAL_EXTEND_FRAME_T *>(buf);
-    switch (temp->head)
-    {
-    case AT10::GIMBAL_CMD_RCV_STATE:
-        std::cout << "Undefined old frame from AT10\r\n";
-        break;
-
-    case AT10::GIMBAL_CMD_STD:
-        AT10::GIMBAL_STD_FRAME_T *stdTemp;
-        stdTemp = reinterpret_cast<AT10::GIMBAL_STD_FRAME_T *>(buf);
-        switch (stdTemp->cmd)
-        {
-        case AT10::GIMBAL_CMD_STD_RCV_STATE:
-            AT10::GIMBAL_RCV_STD_STATE_MSG_T *tempRcv;
-            tempRcv = reinterpret_cast<AT10::GIMBAL_RCV_STD_STATE_MSG_T *>(((uint8_t *)buf) + AT10_STD_PAYLOAD_OFFSET);
-            mState.lock();
-
-            state.abs.roll = (amovGimbalTools::conversionBigLittle((uint16_t)(tempRcv->B1.roll & 0XFF0F)) * 0.043956043956044f) - 90.0f;
-            state.abs.yaw = (int16_t)amovGimbalTools::conversionBigLittle((uint16_t)tempRcv->B1.yaw) * 0.0054931640625f;
-            state.abs.pitch = (int16_t)amovGimbalTools::conversionBigLittle((uint16_t)tempRcv->B1.pitch) * 0.0054931640625f;
-
-            state.rel.yaw = state.abs.yaw;
-            state.rel.roll = state.abs.roll;
-            state.rel.pitch = state.abs.pitch;
-
-            state.fov.x = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovX) * 0.1;
-            state.fov.y = amovGimbalTools::conversionBigLittle(tempRcv->D1.fovY) * 0.1;
-            if ((amovGimbalTools::conversionBigLittle(tempRcv->D1.camera) & 0X0003) == 0X01)
-            {
-                state.video = AMOV_GIMBAL_VIDEO_TAKE;
-            }
-            else
-            {
-                state.video = AMOV_GIMBAL_VIDEO_OFF;
-            }
-            updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
-                                      state.abs.roll, state.abs.pitch, state.abs.yaw,
-                                      state.fov.x, state.fov.y, updataCaller);
-            mState.unlock();
-            break;
-
-        case AT10::GIMBAL_CMD_STD_NOP:
-            break;
-
-        default:
-            std::cout << "Undefined std frame from AT10";
-            std::cout << std::endl;
-            break;
-        }
-        break;
-    default:
-        printf("\r\nUndefined frame from AT10,head:%08X", temp->head);
-        break;
-    }
-}
-
-/**
- * It's a state machine that parses a serial stream of bytes into a struct
- *
- * @param uint8_t unsigned char
- *
- * @return A boolean value.
- */
-bool AT10GimbalDriver::parser(IN uint8_t byte)
-{
-    bool state = false;
-    static uint8_t payloadLenghte = 0;
-    static uint8_t *pRx = nullptr;
-    uint8_t suncheck;
-
-    switch (parserState)
-    {
-    case AT10::GIMBAL_SERIAL_STATE_IDLE:
-        if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X000000FF) >> 0))
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD1;
-        }
-        else if (byte == ((AT10::GIMBAL_CMD_STD & 0X0000FF00) >> 8))
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_STD_HAED1;
-        }
-        break;
-
-    // STD msg
-    case AT10::GIMBAL_SERIAL_STATE_STD_HAED1:
-        if (byte == ((AT10::GIMBAL_CMD_STD & 0X00FF0000) >> 16))
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_STD_HAED2;
-        }
-        else
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-        }
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_STD_HAED2:
-        if (byte == ((AT10::GIMBAL_CMD_STD & 0XFF000000) >> 24))
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_STD_LEN;
-        }
-        else
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-        }
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_STD_LEN:
-        stdRx.len = byte;
-        payloadLenghte = (byte & 0X3F) - 3;
-        pRx = stdRx.data;
-        parserState = AT10::GIMBAL_SERIAL_STATE_STD_CMD;
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_STD_CMD:
-        stdRx.cmd = byte;
-        parserState = AT10::GIMBAL_SERIAL_STATE_STD_DATE;
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_STD_DATE:
-        *pRx = byte;
-        pRx++;
-        payloadLenghte--;
-        if (payloadLenghte == 0)
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_STD_CHECK;
-        }
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_STD_CHECK:
-        stdRx.checkXOR = byte;
-        if (AT10::checkXOR((uint8_t *)&stdRx.len, (stdRx.len & 0X3F)) == byte)
-        {
-            state = true;
-            stdRxQueue->inCell(&stdRx);
-        }
-        else
-        {
-            memset(&stdRx, 0, sizeof(AT10::GIMBAL_STD_FRAME_T));
-        }
-        parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-        payloadLenghte = 0;
-        pRx = nullptr;
-
-        break;
-
-    // EXT msg
-    case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD1:
-        if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X0000FF00) >> 8))
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD2;
-        }
-        else
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-        }
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD2:
-        if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0X00FF0000) >> 16))
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_EXT_HEAD3;
-        }
-        else
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-        }
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_EXT_HEAD3:
-        if (byte == ((AT10::GIMBAL_CMD_RCV_STATE & 0XFF000000) >> 24))
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_EXT_DATE;
-            payloadLenghte = sizeof(AT10::GIMBAL_RCV_POS_MSG_T);
-            pRx = extendRx.data;
-            extendRx.head = AT10::GIMBAL_CMD_RCV_STATE;
-        }
-        else
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-        }
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_EXT_DATE:
-        *pRx = byte;
-        payloadLenghte--;
-        pRx++;
-        if (payloadLenghte == 0)
-        {
-            parserState = AT10::GIMBAL_SERIAL_STATE_EXT_CHECK;
-        }
-        break;
-
-    case AT10::GIMBAL_SERIAL_STATE_EXT_CHECK:
-        suncheck = AT10::CheckSum(extendRx.data, sizeof(AT10::GIMBAL_RCV_POS_MSG_T));
-        if (byte == suncheck)
-        {
-            state = true;
-            rxQueue->inCell(&extendRx);
-        }
-        else
-        {
-            memset(&extendRx, 0, sizeof(AT10::GIMBAL_EXTEND_FRAME_T));
-        }
-        parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-        payloadLenghte = 0;
-        pRx = nullptr;
-        break;
-
-    default:
-        parserState = AT10::GIMBAL_SERIAL_STATE_IDLE;
-        memset(&extendRx, 0, sizeof(AT10::GIMBAL_EXTEND_FRAME_T));
-        memset(&stdRx, 0, sizeof(AT10::GIMBAL_STD_FRAME_T));
-        payloadLenghte = 0;
-        pRx = nullptr;
-        break;
-    }
-
-    return state;
-}
-
-void AT10GimbalDriver::sendHeart(void)
-{
-    uint8_t temp = 0X00;
-    while (1)
-    {
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-        pack(AT10::GIMBAL_CMD_STD_HEART, &temp, sizeof(temp));
-    }
-}
-
-void AT10GimbalDriver::sendStd(void)
-{
-    uint8_t tempBuffer[72];
-    while (1)
-    {
-        if (!IO->isBusy() && IO->isOpen())
-        {
-            bool state = false;
-
-            state = stdTxQueue->outCell(&tempBuffer);
-            if (state)
-            {
-                IO->outPutBytes((uint8_t *)&tempBuffer + 1,
-                                reinterpret_cast<AT10::GIMBAL_STD_FRAME_T *>(tempBuffer)->len + 3);
-            }
-        }
-    }
-}
-
-void AT10GimbalDriver::stackStart(void)
-{
-    if (!this->IO->isOpen())
-    {
-        this->IO->open();
-    }
-    std::thread sendHeartLoop(&AT10GimbalDriver::sendHeart, this);
-    std::thread sendStdLoop(&AT10GimbalDriver::sendStd, this);
-
-    this->sendThreadHanle = sendStdLoop.native_handle();
-    this->sendHreatThreadHandle = sendHeartLoop.native_handle();
-
-    sendHeartLoop.detach();
-    sendStdLoop.detach();
-}
-
-void AT10GimbalDriver::parserLoop(void)
-{
-    uint8_t temp[65536];
-    uint32_t i = 0, getCount = 0;
-
-    while (1)
-    {
-        getCount = IO->inPutBytes(temp);
-
-        for (i = 0; i < getCount; i++)
-        {
-            parser(temp[i]);
-        }
-    }
-}
-
-void AT10GimbalDriver::getStdRxPack(void)
-{
-    uint8_t tempBuffer[280];
-    while (1)
-    {
-        if (stdRxQueue->outCell(tempBuffer))
-        {
-            msgCustomCallback(tempBuffer, msgCaller);
-            convert(tempBuffer);
-        }
-    }
-}
-
-void AT10GimbalDriver::getExtRxPack(void)
-{
-    uint8_t tempBuffer[280];
-    while (1)
-    {
-        if (rxQueue->outCell(tempBuffer))
-        {
-            msgCustomCallback(tempBuffer, msgCaller);
-            convert(tempBuffer);
-        }
-    }
-}
-
-void AT10GimbalDriver::parserStart(pAmovGimbalStateInvoke callback, void *caller)
-{
-    this->updateGimbalStateCallback = callback;
-    this->updataCaller = caller;
-
-    std::thread parser(&AT10GimbalDriver::parserLoop, this);
-    std::thread getStdRxPackLoop(&AT10GimbalDriver::getStdRxPack, this);
-    std::thread getExtRxPackLooP(&AT10GimbalDriver::getExtRxPack, this);
-
-    this->parserThreadHanle = parser.native_handle();
-    this->stackThreadHanle = getStdRxPackLoop.native_handle();
-    this->extStackThreadHandle = getExtRxPackLooP.native_handle();
-
-    parser.detach();
-    getStdRxPackLoop.detach();
-    getExtRxPackLooP.detach();
-}
-
-uint32_t AT10GimbalDriver::calPackLen(void *pack)
-{
-    return 0;
-}

gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h

@@ -1,88 +0,0 @@
-/*
- * @Description:    Q10f吊舱的驱动文件
- * @Author: L LC @amov
- * @Date: 2022-10-28 12:24:21
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-06 10:27:48
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/AT10/AT10_gimbal_driver.h
- */
-#ifndef __AT10_DRIVER_H
-#define __AT10_DRIVER_H
-#include "../amov_gimbal_private.h"
-#include "AT10_gimbal_struct.h"
-#include <mutex>
-#include <malloc.h>
-#include <iostream>
-
-class AT10GimbalDriver : protected amovGimbal::amovGimbalBase
-{
-private:
-    AT10::GIMBAL_SERIAL_STATE_T parserState;
-    AT10::GIMBAL_EXTEND_FRAME_T extendRx;
-
-    AT10::GIMBAL_STD_FRAME_T stdRx;
-    fifoRing *stdRxQueue;
-    fifoRing *stdTxQueue;
-
-    // void send(void);
-
-    void stackStart(void);
-    void sendHeart(void);
-    void sendStd(void);
-
-    void parserStart(pAmovGimbalStateInvoke callback, void *caller);
-    void parserLoop(void);
-    void getExtRxPack(void);
-    void getStdRxPack(void);
-
-    std::thread::native_handle_type sendHreatThreadHandle;
-    std::thread::native_handle_type extStackThreadHandle;
-
-    bool parser(IN uint8_t byte);
-    void convert(void *buf);
-    uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
-    uint32_t calPackLen(void *pack);
-
-public:
-    // funtions
-    uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
-    uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
-    uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
-    uint32_t setGimabalHome(void);
-
-    uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-    uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-
-    uint32_t takePic(void);
-    uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
-
-    uint32_t extensionFuntions(void *cmd);
-
-    // builds
-    static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
-    {
-        return new AT10GimbalDriver(_IO);
-    }
-
-    AT10GimbalDriver(amovGimbal::IOStreamBase *_IO);
-    ~AT10GimbalDriver()
-    {
-        if (stdRxQueue != nullptr)
-        {
-            delete stdRxQueue;
-        }
-        if (stdTxQueue != nullptr)
-        {
-            delete stdTxQueue;
-        }
-        // set thread kill anytime
-        pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
-        parserThreadHanle = parserThreadHanle == 0 ? 0 : pthread_cancel(parserThreadHanle);
-        sendThreadHanle = sendThreadHanle == 0 ? 0 : pthread_cancel(sendThreadHanle);
-        stackThreadHanle = stackThreadHanle == 0 ? 0 : pthread_cancel(stackThreadHanle);
-        sendHreatThreadHandle = sendHreatThreadHandle == 0 ? 0 : pthread_cancel(sendHreatThreadHandle);
-        extStackThreadHandle = extStackThreadHandle == 0 ? 0 : pthread_cancel(extStackThreadHandle);
-    }
-};
-
-#endif

gimbal_ctrl/driver/src/AT10/AT10_gimbal_funtion.cpp

@@ -1,208 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-03-02 10:00:52
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-11-27 16:27:18
- * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_funtion.cpp
- */
-#include "AT10_gimbal_driver.h"
-#include "AT10_gimbal_crc32.h"
-#include "string.h"
-#include "math.h"
-/**
- * It sets the gimbal position.
- *
- * @param pos the position of the gimbal
- *
- * @return The return value is the number of bytes written to the buffer.
- */
-uint32_t AT10GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
-{
-    int16_t yaw, pitch;
-    AT10::GIMBAL_CMD_A1_MSG_T temp;
-    yaw = (int16_t)(pos.yaw / (360.0f / 65536.0f));
-    printf("\r\n %04X\r\n", yaw);
-    yaw = amovGimbalTools::conversionBigLittle((uint16_t)yaw);
-    pitch = (int16_t)(pos.pitch / (360.0f / 65536.0f));
-    pitch = amovGimbalTools::conversionBigLittle((uint16_t)pitch);
-    temp.cmd = 0x0B;
-    temp.param[0] = yaw;
-    temp.param[1] = pitch;
-    temp.param[2] = 0;
-    temp.param[3] = 0;
-    return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T));
-}
-
-/**
- * It takes a struct of type AMOV_GIMBAL_POS_T and converts it to a struct of type
- * G1::GIMBAL_SET_POS_MSG_T
- *
- * @param speed the speed of the gimbal
- *
- * @return The return value is the number of bytes written to the buffer.
- */
-uint32_t AT10GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
-{
-    int16_t speedYaw, speedPitch;
-    AT10::GIMBAL_CMD_A1_MSG_T temp;
-    speedYaw = (int16_t)(speed.yaw * 100);
-    printf("\r\n %04X\r\n", speedYaw);
-    speedYaw = amovGimbalTools::conversionBigLittle((uint16_t)speedYaw);
-    speedPitch = (int16_t)(speed.pitch * 100);
-    speedPitch = amovGimbalTools::conversionBigLittle((uint16_t)speedPitch);
-    temp.cmd = 0x01;
-    temp.param[0] = speedYaw;
-    temp.param[1] = speedPitch;
-    temp.param[2] = 0;
-    temp.param[3] = 0;
-    return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T));
-}
-
-/**
- * This function sets the gimbal's follow speed
- *
- * @param followSpeed the speed of the gimbal
- *
- * @return The return value is the number of bytes written to the buffer.
- */
-uint32_t AT10GimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
-{
-    state.maxFollow.pitch = fabs(followSpeed.pitch * 100);
-    state.maxFollow.yaw = fabs(followSpeed.yaw * 100);
-    state.maxFollow.roll = fabs(followSpeed.roll * 100);
-    return 0;
-}
-
-/**
- * This function sets the gimbal to its home position
- *
- * @return The return value is the number of bytes written to the buffer.
- */
-uint32_t AT10GimbalDriver::setGimabalHome(void)
-{
-    AT10::GIMBAL_CMD_A1_MSG_T temp;
-    temp.cmd = 0x04;
-    temp.param[0] = 0;
-    temp.param[1] = 0;
-    temp.param[2] = 0;
-    temp.param[3] = 0;
-    return pack(AT10::GIMBAL_CMD_STD_MOTOR, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_A1_MSG_T));
-}
-
-/**
- * It takes a picture.
- *
- * @return The return value is the number of bytes written to the serial port.
- */
-uint32_t AT10GimbalDriver::takePic(void)
-{
-    uint16_t temp = 0x13 << 3;
-    uint16_t data = amovGimbalTools::conversionBigLittle(temp);
-
-    return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t));
-}
-
-/**
- * The function sets the video state of the gimbal
- *
- * @param newState The new state of the video.
- *
- * @return The return value is the number of bytes written to the serial port.
- */
-uint32_t AT10GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
-{
-    uint16_t temp;
-    if (newState == AMOV_GIMBAL_VIDEO_T::AMOV_GIMBAL_VIDEO_TAKE)
-    {
-        temp = 0x14 << 3;
-    }
-    else
-    {
-        temp = 0x15 << 3;
-    }
-
-    uint16_t data = amovGimbalTools::conversionBigLittle(temp);
-
-    return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t));
-}
-
-uint32_t AT10GimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
-{
-    if (targetRate == 0.0f)
-    {
-        uint16_t temp = 0;
-        switch (zoom)
-        {
-        case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
-            temp = 0X08 << 3;
-            break;
-        case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
-            temp = 0X09 << 3;
-            break;
-        case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
-            temp = 0X01 << 3;
-            break;
-        default:
-            break;
-        }
-        uint16_t data = amovGimbalTools::conversionBigLittle(temp);
-
-        return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t));
-    }
-    else
-    {
-        AT10::GIMBAL_CMD_C2_MSG_T temp;
-        temp.cmd = 0x53;
-        temp.param = targetRate * 10;
-        temp.param = amovGimbalTools::conversionBigLittle(temp.param);
-        return pack(AT10::GIMBAL_CMD_STD_CAMERA2, (uint8_t *)&temp, sizeof(AT10::GIMBAL_CMD_C2_MSG_T));
-    }
-}
-
-uint32_t AT10GimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
-{
-    uint16_t temp = 0;
-    switch (zoom)
-    {
-    case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_IN:
-        temp = 0X0B << 3;
-        break;
-    case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_OUT:
-        temp = 0X0A << 3;
-        break;
-    case AMOV_GIMBAL_ZOOM_T::AMOV_GIMBAL_ZOOM_STOP:
-        temp = 0X01 << 3;
-        break;
-    default:
-        break;
-    }
-    uint16_t data = amovGimbalTools::conversionBigLittle(temp);
-
-    return pack(AT10::GIMBAL_CMD_STD_CAMERA, (uint8_t *)&data, sizeof(uint16_t));
-}
-
-//
-/**
- * The function `extensionFuntions` in the `AT10GimbalDriver` class takes a command as input, casts it
- * to a specific type, and then packs the command and its parameters into a byte array.
- *
- * @param cmd The "cmd" parameter is a void pointer, which means it can point to any type of data. In
- * this case, it is being cast to a pointer of type AT10::AT10_EXT_CMD_T using the reinterpret_cast
- * operator.
- *
- * @return the result of the `pack` function, which is of type `uint32_t`.
- */
-uint32_t AT10GimbalDriver::extensionFuntions(void *cmd)
-{
-    AT10::AT10_EXT_CMD_T *tempCMD;
-    tempCMD = reinterpret_cast<AT10::AT10_EXT_CMD_T *>(cmd);
-
-    return pack(tempCMD->cmd, (uint8_t *)tempCMD->param, tempCMD->paramLen);
-}
-
-// AT10_EXT_CMD_T infraredOpen ;
-// infraredOpen.cmd = AT10::GIMBAL_CMD_STD_CAMERA;
-// infraredOpen.param[0] = 0X02;
-// infraredOpen.param[1] = 0;
-// infraredOpen.paramLen = 2;

gimbal_ctrl/driver/src/AT10/AT10_gimbal_struct.h

@@ -1,174 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2022-10-27 18:10:07
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-08-25 19:32:59
- * @FilePath: /gimbal-sdk-multi-platform/src/AT10/AT10_gimbal_struct.h
- */
-#ifndef AT10_GIMBAL_STRUCT_H
-#define AT10_GIMBAL_STRUCT_H
-
-#include <stdint.h>
-namespace AT10
-{
-#define AT10_MAX_GIMBAL_PAYLOAD 64
-#define AT10_EXT_PAYLOAD_OFFSET 4
-#define AT10_STD_PAYLOAD_OFFSET 6
-#define AT10_EXT_SCALE_FACTOR_ANGLE 0.02197f
-#define AT10_EXT_SCALE_FACTOR_SPEED 0.06103f
-
-    typedef enum
-    {
-        GIMBAL_CMD_STD_NOP = 0X00,
-        GIMBAL_CMD_STD_HEART = 0X10,
-        GIMBAL_CMD_STD_RCV_STATE = 0X40,
-        GIMBAL_CMD_STD_MOTOR = 0X1A,
-        GIMBAL_CMD_STD_CAMERA = 0X1C,
-        GIMBAL_CMD_STD_CAMERA2 = 0X2C,
-        GIMBAL_CMD_STD_MOTOR2 = 0X32,
-        GIMBAL_CMD_STD = 0XDCAA5500,
-        GIMBAL_CMD_RCV_STATE = 0X721A583E,
-        GIMBAL_CMD_SET_FEEDBACK_L = 0X143055AA,
-        GIMBAL_CMD_SET_FEEDBACK_H = 0X003155AA,
-        GIMBAL_CMD_OPEN_FEEDBACK = 0X3E003E3E,
-        GIMBAL_CMD_CLOSE_FEEDBACK = 0X3D003D3E,
-    } GIMBAL_CMD_T;
-
-    typedef enum
-    {
-        GIMBAL_SERIAL_STATE_IDLE,
-        GIMBAL_SERIAL_STATE_EXT_HEAD1,
-        GIMBAL_SERIAL_STATE_EXT_HEAD2,
-        GIMBAL_SERIAL_STATE_EXT_HEAD3,
-        GIMBAL_SERIAL_STATE_EXT_DATE,
-        GIMBAL_SERIAL_STATE_EXT_CHECK,
-        GIMBAL_SERIAL_STATE_STD_HAED1,
-        GIMBAL_SERIAL_STATE_STD_HAED2,
-        GIMBAL_SERIAL_STATE_STD_LEN,
-        GIMBAL_SERIAL_STATE_STD_CMD,
-        GIMBAL_SERIAL_STATE_STD_DATE,
-        GIMBAL_SERIAL_STATE_STD_CHECK,
-    } GIMBAL_SERIAL_STATE_T;
-
-#pragma pack(1)
-    typedef struct
-    {
-        uint8_t cmd;
-        uint8_t param[AT10_MAX_GIMBAL_PAYLOAD];
-        uint8_t paramLen;
-    } AT10_EXT_CMD_T;
-
-    typedef struct
-    {
-        uint32_t head;
-        uint8_t len;
-        uint8_t cmd;
-        uint8_t data[AT10_MAX_GIMBAL_PAYLOAD];
-        uint8_t checkXOR;
-    } GIMBAL_STD_FRAME_T;
-
-    typedef struct
-    {
-        uint32_t head;
-        uint8_t data[AT10_MAX_GIMBAL_PAYLOAD];
-        uint8_t checkSum;
-        uint8_t len;
-    } GIMBAL_EXTEND_FRAME_T;
-
-    typedef struct
-    {
-        uint16_t timeStamp;
-        int16_t rollIMUAngle;
-        int16_t pitchIMUAngle;
-        int16_t yawIMUAngle;
-        int16_t rollTAGAngle;
-        int16_t pitchTAGAngle;
-        int16_t yawTAGAngle;
-        int16_t rollTAGSpeed;
-        int16_t pitchTAGSpeed;
-        int16_t yawTAGSpeed;
-        int16_t rollStatorRotorAngle;
-        int16_t pitchStatorRotorAngle;
-        int16_t yawStatorRotorAngle;
-    } GIMBAL_RCV_POS_MSG_T;
-
-    typedef struct
-    {
-        uint8_t hight;
-        uint8_t reserve;
-        uint32_t lat;
-        uint32_t log;
-        int16_t alt;
-        uint32_t latTar;
-        uint32_t logTar;
-        int16_t altTar;
-    } GIMBAL_RCV_GPS_STATE_MSG_T;
-
-    typedef struct
-    {
-        int16_t roll;
-        int16_t yaw;
-        int16_t pitch;
-    } GIMBAL_RCV_MOTOR_STATE_MSG_T;
-
-    typedef struct
-    {
-        uint8_t mode;
-        uint8_t reserve;
-        uint16_t camera;
-        uint16_t distance;
-        uint16_t fovY;
-        uint16_t fovX;
-        uint16_t rate;
-    } GIMBAL_RCV_CAMERA_STATE_MSG_T;
-
-    typedef struct
-    {
-        GIMBAL_RCV_GPS_STATE_MSG_T T1;
-        uint8_t F1;
-        GIMBAL_RCV_MOTOR_STATE_MSG_T B1;
-        GIMBAL_RCV_CAMERA_STATE_MSG_T D1;
-    } GIMBAL_RCV_STD_STATE_MSG_T;
-
-    typedef struct
-    {
-        uint16_t param;
-    } GIMBAL_CMD_C1_MSG_T;
-
-    typedef struct
-    {
-        uint8_t cmd;
-        uint16_t param;
-    } GIMBAL_CMD_C2_MSG_T;
-
-    typedef struct
-    {
-        uint8_t cmd;
-        uint16_t param[4];
-    } GIMBAL_CMD_A1_MSG_T;
-
-    typedef struct
-    {
-        uint8_t cmd;
-        uint8_t reserve;
-        uint32_t param[3];
-    } GIMBAL_CMD_S1_MSG_T;
-
-    typedef struct
-    {
-        uint8_t param[3];
-    } GIMBAL_CMD_E1_MSG_T;
-
-    typedef struct
-    {
-        GIMBAL_CMD_A1_MSG_T a1;
-        GIMBAL_CMD_C1_MSG_T c1;
-        GIMBAL_CMD_E1_MSG_T e1;
-        GIMBAL_CMD_S1_MSG_T s1;
-    } GIMBAL_CMD_A1C1E1S1_MSG_T;
-
-#pragma pack()
-
-}
-#endif

gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cc

@@ -0,0 +1,212 @@
+/*
+ * @Description    :
+ * @Author         : Aiyangsky
+ * @Date           : 2022-08-26 21:42:10
+ * @LastEditors    : Aiyangsky
+ * @LastEditTime   : 2022-08-27 03:43:49
+ * @FilePath       : \mavlink\src\route\Ring_Fifo.c
+ */
+
+#include <string.h>
+
+#include "Ring_Fifo.h"
+
+/**
+ * @description:
+ * @param       {RING_FIFO_CB_T} *fifo                  fifo struct pointer
+ * @param       {unsigned short} cell_size              sizeof(cell)
+ * @param       {unsigned char} *buffer                 fifo buffer address
+ * @param       {unsigned int} buffer_lenght            sizeof(buffer)
+ * @return      {*}
+ * @note       :
+ */
+void Ring_Fifo_init(RING_FIFO_CB_T *fifo, unsigned short cell_size,
+                    unsigned char *buffer, unsigned int buffer_lenght)
+{
+    fifo->cell_size = cell_size;
+
+    fifo->start = buffer;
+    // Remainder is taken to avoid splicing in the output so as to improve the efficiency
+    fifo->end = buffer + buffer_lenght - (buffer_lenght % cell_size);
+    fifo->in = buffer;
+    fifo->out = buffer;
+    fifo->curr_number = 0;
+    fifo->max_number = buffer_lenght / cell_size;
+}
+
+/**
+ * @description:                                        add a cell to fifo
+ * @param       {RING_FIFO_CB_T} *fifo                  fifo struct pointer
+ * @param       {void} *data                            cell data [in]
+ * @return      {*}                                     Success or fail
+ * @note       :                                        failed if without space
+ */
+bool Ring_Fifo_in_cell(RING_FIFO_CB_T *fifo, void *data)
+{
+    unsigned char *next;
+    unsigned char *ptemp = fifo->in;
+    bool ret = false;
+
+    LOCK();
+
+    if (fifo->curr_number < fifo->max_number)
+    {
+        next = fifo->in + fifo->cell_size;
+        if (next >= fifo->end)
+        {
+            next = fifo->start;
+        }
+        fifo->in = next;
+        fifo->curr_number++;
+        memcpy(ptemp, data, fifo->cell_size);
+        ret = true;
+    }
+
+    UNLOCK();
+
+    return ret;
+}
+
+/**
+ * @description:                                        add a series of cells to fifo
+ * @param       {RING_FIFO_CB_T} *fifo
+ * @param       {void} *data                            cells data [in]
+ * @param       {unsigned short} number                 expect add number of cells
+ * @return      {*}                                     number of successful add
+ * @note       :
+ */
+unsigned short Ring_Fifo_in_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number)
+{
+    // Number of remaining storable cells is described to simplify the calculation in the copying process.
+    unsigned short diff = fifo->max_number - fifo->curr_number;
+    unsigned short count_temp, count_temp_r;
+    unsigned char *next;
+    unsigned char *ptemp = fifo->in;
+    unsigned short ret;
+
+    LOCK();
+
+    if (diff > number)
+    {
+        ret = number;
+    }
+    else if (diff > 0 && diff < number)
+    {
+        ret = diff;
+    }
+    else
+    {
+        ret = 0;
+    }
+
+    count_temp = fifo->cell_size * ret;
+    next = fifo->in + count_temp;
+    // Moving the write pointer and the number of stored cells before
+    // copying data reduces the likelihood of multithreaded write conflicts.
+    fifo->curr_number += ret;
+
+    if (next < fifo->end)
+    {
+        fifo->in = next;
+        memcpy(ptemp, data, count_temp);
+    }
+    else
+    {
+        count_temp_r = fifo->end - fifo->in;
+        next = fifo->start + count_temp - count_temp_r;
+        fifo->in = next;
+        memcpy(ptemp, data, count_temp_r);
+        memcpy(fifo->start, ((unsigned char *)data) + count_temp_r, count_temp - count_temp_r);
+    }
+
+    UNLOCK();
+
+    return ret;
+}
+
+/**
+ * @description:                                        output a cell
+ * @param       {RING_FIFO_CB_T} *fifo
+ * @param       {void} *data                            cell data [out]
+ * @return      {*}                                     Success or fail
+ * @note       :                                        fail if without cell
+ */
+bool Ring_Fifo_out_cell(RING_FIFO_CB_T *fifo, void *data)
+{
+    unsigned char *next;
+    unsigned char *ptemp = fifo->out;
+    bool ret = false;
+
+    LOCK();
+
+    if (fifo->curr_number > 0)
+    {
+        next = fifo->out + fifo->cell_size;
+        if (next >= fifo->end)
+        {
+            next = fifo->start;
+        }
+        fifo->out = next;
+        fifo->curr_number--;
+        memcpy(data, ptemp, fifo->cell_size);
+        ret = true;
+    }
+
+    UNLOCK();
+
+    return ret;
+}
+
+/**
+ * @description:                                        output a series of cells in fifo
+ * @param       {RING_FIFO_CB_T} *fifo
+ * @param       {void} *data                            cells data [out]
+ * @param       {unsigned short} number                 expect out number of cells
+ * @return      {*}                                     number of successful output
+ * @note       :
+ */
+unsigned short Ring_Fifo_out_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number)
+{
+    unsigned char *next;
+    unsigned char *ptemp = fifo->out;
+    unsigned short count_temp, count_temp_r;
+    unsigned short ret;
+
+    LOCK();
+
+    if (fifo->curr_number > number)
+    {
+        ret = number;
+    }
+    else if (fifo->curr_number < number && fifo->curr_number > 0)
+    {
+        ret = fifo->curr_number;
+    }
+    else
+    {
+        ret = 0;
+    }
+
+    count_temp = fifo->cell_size * ret;
+    next = fifo->out + count_temp;
+
+    fifo->curr_number -= ret;
+
+    if (next < fifo->end)
+    {
+        fifo->out = next;
+        memcpy(data, ptemp, count_temp);
+    }
+    else
+    {
+        count_temp_r = fifo->end - fifo->in;
+        next = fifo->start + count_temp - count_temp_r;
+        fifo->out = next;
+        memcpy(data, ptemp, count_temp_r);
+        memcpy(((unsigned char *)data) + count_temp_r, fifo->start, count_temp - count_temp_r);
+    }
+
+    UNLOCK();
+
+    return ret;
+}

gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp

@@ -1,228 +0,0 @@
-/*
- * @Description    :
- * @Author         : Aiyangsky
- * @Date           : 2022-08-26 21:42:10
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-11-28 11:47:34
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.cpp
- */
-
-#include <string.h>
-
-#include "Ring_Fifo.h"
-
-/**
- * The `fifoRing` constructor initializes the `cellSize`, `maxNumber`, `currNumber`, `start`, `in`,
- * `out`, and `end` variables, and allocates memory for the `start` pointer.
- *
- * @param _cellSize The `_cellSize` parameter represents the size of each cell in the FIFO ring buffer.
- * It determines the amount of memory allocated for each element in the buffer.
- * @param _cellNum The parameter `_cellNum` represents the number of cells in the FIFO ring.
- */
-fifoRing::fifoRing(unsigned short _cellSize, unsigned int _cellNum)
-{
-    cellSize = _cellSize;
-    maxNumber = _cellNum;
-    currNumber = 0;
-
-    start = nullptr;
-    start = (uint8_t *)malloc(_cellNum * _cellSize);
-    if (start == nullptr)
-    {
-        std::cout << "fifo malloc failed! size :" << (_cellNum * _cellSize) << std::endl;
-        exit(1);
-    }
-
-    memset(start, 0, _cellNum * _cellSize);
-
-    in = start;
-    out = start;
-    end = start + _cellNum * _cellSize;
-}
-
-/**
- * The `inCell` function adds data to a FIFO ring buffer and returns true if successful.
- *
- * @param data A pointer to the data that needs to be stored in the FIFO ring.
- *
- * @return a boolean value.
- */
-bool fifoRing::inCell(void *data)
-{
-    std::lock_guard<std::mutex> locker(fifoMutex);
-    unsigned char *next;
-    unsigned char *ptemp = in;
-    bool ret = false;
-
-    if (currNumber < maxNumber)
-    {
-        next = in + cellSize;
-        if (next >= end)
-        {
-            next = start;
-        }
-        in = next;
-        currNumber++;
-        memcpy(ptemp, data, cellSize);
-        ret = true;
-        notEmpty.notify_all();
-    }
-
-    return ret;
-}
-
-/**
- * The `inCells` function is used to store data in a FIFO ring buffer, returning the number of cells
- * successfully stored.
- *
- * @param data A pointer to the data that needs to be stored in the FIFO ring.
- * @param number The parameter "number" represents the number of cells that the function should attempt
- * to store in the FIFO ring.
- *
- * @return the number of cells that were successfully stored in the FIFO ring buffer.
- */
-unsigned short fifoRing::inCells(void *data, unsigned short number)
-{
-    std::lock_guard<std::mutex> locker(fifoMutex);
-    // Number of remaining storable cells is described to simplify the calculation in the copying process.
-    unsigned short diff = maxNumber - currNumber;
-    unsigned short count_temp, count_temp_r;
-    unsigned char *next;
-    unsigned char *ptemp = in;
-    unsigned short ret;
-
-    if (diff > number)
-    {
-        ret = number;
-    }
-    else if (diff > 0 && diff < number)
-    {
-        ret = diff;
-    }
-    else
-    {
-        ret = 0;
-    }
-
-    count_temp = cellSize * ret;
-    next = in + count_temp;
-    // Moving the write pointer and the number of stored cells before
-    // copying data reduces the likelihood of multithreaded write conflicts.
-    currNumber += ret;
-
-    if (next < end)
-    {
-        in = next;
-        memcpy(ptemp, data, count_temp);
-    }
-    else
-    {
-        count_temp_r = end - in;
-        next = start + count_temp - count_temp_r;
-        in = next;
-        memcpy(ptemp, data, count_temp_r);
-        memcpy(start, ((unsigned char *)data) + count_temp_r, count_temp - count_temp_r);
-    }
-    if (ret > 0)
-    {
-        notEmpty.notify_all();
-    }
-
-    return ret;
-}
-
-/**
- * The `outCell` function removes a cell from the FIFO ring buffer and copies its data to the provided
- * memory location.
- *
- * @param data A pointer to the memory location where the data from the cell will be copied to.
- *
- * @return a boolean value. If a cell is successfully taken from the FIFO ring and the data is copied
- * into the provided pointer, the function returns true. Otherwise, if the FIFO ring is empty and no
- * cell can be taken, the function waits until a cell becomes available and then returns false.
- */
-bool fifoRing::outCell(void *data)
-{
-    std::lock_guard<std::mutex> locker(fifoMutex);
-    unsigned char *next;
-    unsigned char *ptemp = out;
-    bool ret = false;
-
-    if (currNumber > 0)
-    {
-        next = out + cellSize;
-        if (next >= end)
-        {
-            next = start;
-        }
-        out = next;
-        currNumber--;
-        memcpy(data, ptemp, cellSize);
-        ret = true;
-    }
-    else
-    {
-        notEmpty.wait(fifoMutex);
-    }
-
-    return ret;
-}
-
-/**
- * The `outCells` function retrieves a specified number of cells from a FIFO ring buffer and copies the
- * data into a provided buffer.
- *
- * @param data A pointer to the memory location where the extracted data will be stored.
- * @param number The parameter "number" represents the number of cells that should be read from the
- * FIFO ring.
- *
- * @return the number of cells that were successfully read from the FIFO ring buffer.
- */
-unsigned short fifoRing::outCells(void *data, unsigned short number)
-{
-    std::lock_guard<std::mutex> locker(fifoMutex);
-
-    unsigned char *next;
-    unsigned char *ptemp = out;
-    unsigned short count_temp, count_temp_r;
-    unsigned short ret;
-
-    if (currNumber > number)
-    {
-        ret = number;
-    }
-    else if (currNumber < number && currNumber > 0)
-    {
-        ret = currNumber;
-    }
-    else
-    {
-        ret = 0;
-    }
-
-    count_temp = cellSize * ret;
-    next = out + count_temp;
-
-    currNumber -= ret;
-
-    if (next < end)
-    {
-        out = next;
-        memcpy(data, ptemp, count_temp);
-    }
-    else
-    {
-        count_temp_r = end - in;
-        next = start + count_temp - count_temp_r;
-        out = next;
-        memcpy(data, ptemp, count_temp_r);
-        memcpy(((unsigned char *)data) + count_temp_r, start, count_temp - count_temp_r);
-    }
-
-    if (ret == 0)
-    {
-        notEmpty.wait(fifoMutex);
-    }
-
-    return ret;
-}

gimbal_ctrl/driver/src/FIFO/Ring_Fifo.h

@@ -3,47 +3,45 @@
  * @Author         : Aiyangsky
  * @Date           : 2022-08-26 21:42:02
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-11-28 11:47:39
+ * @LastEditTime: 2023-03-03 16:12:37
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/FIFO/Ring_Fifo.h
+ * @FilePath: \host\gimbal-sdk-multi-platform\src\FIFO\Ring_Fifo.h
  */
 
 #ifndef RING_FIFO_H
 #define RING_FIFO_H
 
-#include <thread>
+#include "stdbool.h"
-#include <mutex>
-#include <condition_variable>
-#include <iostream>
 
-class fifoRing
+#ifdef __cplusplus
+extern "C"
 {
-private:
+#endif
-    unsigned char *start;
-    unsigned char *in;
-    unsigned char *out;
-    unsigned char *end;
 
-    unsigned short currNumber;
+#define LOCK()
-    unsigned short maxNumber;
+#define UNLOCK()
-    unsigned short cellSize;
 
-    std::mutex fifoMutex;
+    typedef struct
-    std::condition_variable_any notEmpty;
-
-public:
-    fifoRing(unsigned short _cellSize, unsigned int _cellNum);
-    ~fifoRing()
     {
-        if (start != nullptr)
+        unsigned char *start;
-        {
+        unsigned char *in;
-            free(start);
+        unsigned char *out;
-        }
+        unsigned char *end;
-    }
 
-    bool inCell(void *data);
+        unsigned short curr_number;
-    unsigned short inCells(void *data, unsigned short number);
+        unsigned short max_number;
-    bool outCell(void *data);
+        unsigned short cell_size;
-    unsigned short outCells(void *data, unsigned short number);
+    } RING_FIFO_CB_T;
-};
+
+    void Ring_Fifo_init(RING_FIFO_CB_T *fifo, unsigned short cell_size,
+                        unsigned char *buffer, unsigned int buffer_lenght);
+    bool Ring_Fifo_in_cell(RING_FIFO_CB_T *fifo, void *data);
+    unsigned short Ring_Fifo_in_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number);
+    bool Ring_Fifo_out_cell(RING_FIFO_CB_T *fifo, void *data);
+    unsigned short Ring_Fifo_out_cells(RING_FIFO_CB_T *fifo, void *data, unsigned short number);
+
+
+#ifdef __cplusplus
+}
+#endif
 
 #endif

gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h

@@ -3,8 +3,8 @@
  * @Author: L LC @amov
  * @Date: 2022-10-27 18:10:06
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:30:13
+ * @LastEditTime: 2022-10-28 14:10:02
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_crc32.h
+ * @FilePath: \amov-gimbal-sdk\src\G1\g1_gimbal_crc32.h
  */
 #ifndef G1_GIMBAL_CRC32_H
 #define G1_GIMBAL_CRC32_H

gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp

@@ -3,8 +3,8 @@
  * @Author: L LC @amov
  * @Date: 2022-10-27 18:10:06
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:22:57
+ * @LastEditTime: 2023-04-18 10:12:46
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.cpp
+ * @FilePath: /gimbal-sdk-multi-platform/src/G1/g1_gimbal_driver.cpp
  */
 #include "g1_gimbal_driver.h"
 #include "g1_gimbal_crc32.h"
@@ -16,10 +16,27 @@
  *
  * @param _IO The IOStreamBase object that will be used to communicate with the gimbal.
  */
-g1GimbalDriver::g1GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO)
+g1GimbalDriver::g1GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO)
 {
-    rxQueue = new fifoRing(sizeof(G1::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
+    memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T));
-    txQueue = new fifoRing(sizeof(G1::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
+    memset(&txQueue, 0, sizeof(RING_FIFO_CB_T));
+
+    rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T));
+    if (rxBuffer == NULL)
+    {
+        std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
+        exit(1);
+    }
+    txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T));
+    if (txBuffer == NULL)
+    {
+        free(rxBuffer);
+        std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
+        exit(1);
+    }
+
+    Ring_Fifo_init(&rxQueue, sizeof(G1::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T));
+    Ring_Fifo_init(&txQueue, sizeof(G1::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(G1::GIMBAL_FRAME_T));
 
     parserState = G1::GIMBAL_SERIAL_STATE_IDLE;
 }
@@ -49,14 +66,32 @@ uint32_t g1GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloa
     txTemp.crc.u32 = G1::CRC32Software(txTemp.payload, payloadSize);
     memcpy(txTemp.payload + payloadSize, txTemp.crc.u8, sizeof(uint32_t));
 
-    if (txQueue->inCell(&txTemp))
+    txMutex.lock();
+    if (Ring_Fifo_in_cell(&txQueue, &txTemp))
     {
         ret = txTemp.lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t);
     }
+    txMutex.unlock();
 
     return ret;
 }
 
+/**
+ * > This function is used to get a packet from the receive queue
+ *
+ * @param void This is the type of data that will be stored in the queue.
+ *
+ * @return A boolean value.
+ */
+bool g1GimbalDriver::getRxPack(OUT void *pack)
+{
+    bool state = false;
+    rxMutex.lock();
+    state = Ring_Fifo_out_cell(&rxQueue, pack);
+    rxMutex.unlock();
+    return state;
+}
+
 void g1GimbalDriver::convert(void *buf)
 {
     G1::GIMBAL_FRAME_T *temp;
@@ -75,7 +110,7 @@ void g1GimbalDriver::convert(void *buf)
         state.rel.pitch = tempPos->HALL_pitch * G1_SCALE_FACTOR;
         updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
                                   state.abs.roll, state.abs.pitch, state.abs.yaw,
-                                  state.fov.x, state.fov.y, updataCaller);
+                                  state.fov.x, state.fov.y);
         mState.unlock();
         break;
 
@@ -90,9 +125,32 @@ void g1GimbalDriver::convert(void *buf)
     }
 }
 
-uint32_t g1GimbalDriver::calPackLen(void *pack)
+/**
+ * The function is called by the main thread to send a command to the gimbal.
+ *
+ * The function first checks to see if the serial port is busy and if it is open. If it is not busy and
+ * it is open, the function locks the txMutex and then checks to see if there is a command in the
+ * txQueue. If there is a command in the txQueue, the function copies the command to the tx buffer and
+ * then unlocks the txMutex. The function then sends the command to the gimbal.
+ *
+ * The txQueue is a ring buffer that holds commands that are waiting to be sent to the gimbal. The
+ * txQueue is a ring buffer because the gimbal can only process one command at a time. If the gimbal is
+ * busy processing a command, the command will be placed in the txQueue and sent to the gimbal when the
+ * gimbal is ready to receive the command.
+ */
+void g1GimbalDriver::send(void)
 {
-    return ((G1::GIMBAL_FRAME_T *)pack)->lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t);
+    if (!IO->isBusy() && IO->isOpen())
+    {
+        bool state = false;
+        txMutex.lock();
+        state = Ring_Fifo_out_cell(&txQueue, &tx);
+        txMutex.unlock();
+        if (state)
+        {
+            IO->outPutBytes((uint8_t *)&tx, tx.lenght + G1_PAYLOAD_OFFSET + sizeof(uint32_t));
+        }
+    }
 }
 
 /**
@@ -167,7 +225,9 @@ bool g1GimbalDriver::parser(IN uint8_t byte)
             if (*((uint32_t *)(pRx - sizeof(uint32_t))) == G1::CRC32Software(rx.payload, rx.lenght))
             {
                 state = true;
-                rxQueue->inCell(&rx);
+                rxMutex.lock();
+                Ring_Fifo_in_cell(&rxQueue, &rx);
+                rxMutex.unlock();
             }
             else
             {

gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h

@@ -3,57 +3,66 @@
  * @Author: L LC @amov
  * @Date: 2022-10-28 12:24:21
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:29:58
+ * @LastEditTime: 2023-03-13 12:29:17
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_driver.h
+ * @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_driver.h
  */
-#ifndef __G1_DRIVER_H
+#include "../amov_gimbal.h"
-#define __G1_DRIVER_H
-
-#include "../amov_gimbal_private.h"
 #include "g1_gimbal_struct.h"
 #include <mutex>
 #include <malloc.h>
 #include <iostream>
 
-class g1GimbalDriver : protected amovGimbal::amovGimbalBase
+#ifndef __G1_DRIVER_H
+#define __G1_DRIVER_H
+
+extern "C"
+{
+#include "Ring_Fifo.h"
+}
+
+class g1GimbalDriver : protected amovGimbal::IamovGimbalBase
 {
 private:
     G1::GIMBAL_CMD_PARSER_STATE_T parserState;
     G1::GIMBAL_FRAME_T rx;
+    G1::GIMBAL_FRAME_T tx;
+
+    std::mutex rxMutex;
+    uint8_t *rxBuffer;
+    RING_FIFO_CB_T rxQueue;
+    std::mutex txMutex;
+    uint8_t *txBuffer;
+    RING_FIFO_CB_T txQueue;
 
-    uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
     bool parser(IN uint8_t byte);
+    void send(void);
+
     void convert(void *buf);
-    uint32_t calPackLen(void *pack);
+    uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
+    bool getRxPack(OUT void *pack);
 
 public:
     // funtions
-    uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
+    uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos);
-    uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
+    uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed);
-    uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
+    uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed);
     uint32_t setGimabalHome(void);
 
     uint32_t takePic(void);
-    uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
+    uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
-
-    uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
-                                const AMOV_GIMBAL_VELOCITY_T &speed,
-                                const AMOV_GIMBAL_VELOCITY_T &acc,
-                                void *extenData);
-
-    uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
-                                const AMOV_GIMBAL_VELOCITY_T &seppd,
-                                const AMOV_GIMBAL_VELOCITY_T &acc,
-                                void *extenData);
-    uint32_t extensionFuntions(void *cmd);
 
     // builds
-    static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
+    static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
     {
         return new g1GimbalDriver(_IO);
     }
 
     g1GimbalDriver(amovGimbal::IOStreamBase *_IO);
+    ~g1GimbalDriver()
+    {
+        free(rxBuffer);
+        free(txBuffer);
+    }
 };
 
 #endif

gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp

@@ -3,13 +3,12 @@
  * @Author: L LC @amov
  * @Date: 2023-03-02 10:00:52
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:29:51
+ * @LastEditTime: 2023-03-17 18:29:33
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_funtion.cpp
+ * @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_funtion.cpp
  */
 #include "g1_gimbal_driver.h"
 #include "g1_gimbal_crc32.h"
 #include "string.h"
-#include <math.h>
 
 /**
  * It sets the gimbal position.
@@ -18,7 +17,7 @@
  *
  * @return The return value is the number of bytes written to the buffer.
  */
-uint32_t g1GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
+uint32_t g1GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
 {
     G1::GIMBAL_SET_POS_MSG_T temp;
     temp.mode = G1::GIMBAL_CMD_POS_MODE_ANGLE;
@@ -32,14 +31,14 @@ uint32_t g1GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
 }
 
 /**
- * It takes a struct of type AMOV_GIMBAL_POS_T and converts it to a struct of type
+ * It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type
  * G1::GIMBAL_SET_POS_MSG_T
  *
  * @param speed the speed of the gimbal
  *
  * @return The return value is the number of bytes written to the buffer.
  */
-uint32_t g1GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
+uint32_t g1GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
 {
     G1::GIMBAL_SET_POS_MSG_T temp;
     temp.mode = G1::GIMBAL_CMD_POS_MODE_SPEED;
@@ -59,7 +58,7 @@ uint32_t g1GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
  *
  * @return The return value is the number of bytes written to the buffer.
  */
-uint32_t g1GimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
+uint32_t g1GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
 {
     state.maxFollow.pitch = followSpeed.pitch / G1_SCALE_FACTOR;
     state.maxFollow.roll = followSpeed.roll / G1_SCALE_FACTOR;
@@ -84,7 +83,7 @@ uint32_t g1GimbalDriver::setGimabalHome(void)
 
 /**
  * It takes a picture.
- *
+ * 
  * @return The return value is the number of bytes written to the serial port.
  */
 uint32_t g1GimbalDriver::takePic(void)
@@ -95,129 +94,25 @@ uint32_t g1GimbalDriver::takePic(void)
 
 /**
  * The function sets the video state of the gimbal
- *
+ * 
  * @param newState The new state of the video.
- *
+ * 
  * @return The return value is the number of bytes written to the serial port.
  */
-uint32_t g1GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
+uint32_t g1GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
 {
     uint8_t temp = G1::GIMBAL_CMD_CAMERA_REC;
 
     mState.lock();
-    if (state.video == AMOV_GIMBAL_VIDEO_TAKE)
+    if(state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
     {
-        state.video = AMOV_GIMBAL_VIDEO_OFF;
+        state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
     }
     else
     {
-        state.video = AMOV_GIMBAL_VIDEO_TAKE;
+        state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE;
     }
     mState.unlock();
 
     return pack(G1::GIMBAL_CMD_CAMERA, &temp, sizeof(uint8_t));
 }
-
-/**
- * The function `attitudeCorrection` takes in quaternion, velocity, acceleration, and external data,
- * and returns a packed message.
- *
- * @param quaterion The "quaterion" parameter is a structure of type "AMOV_GIMBAL_QUATERNION_T" which
- * contains the following fields:
- * @param speed The "speed" parameter is of type `AMOV_GIMBAL_VELOCITY_T` and represents
- * the velocity of the gimbal. It contains three components: `x`, `y`, and `z`, which represent the
- * velocity in the respective axes.
- * @param acc The "acc" parameter is of type "AMOV_GIMBAL_VELOCITY_T" and represents the
- * acceleration of the gimbal in three dimensions (x, y, z).
- * @param extenData The extenData parameter is a void pointer that can be used to pass additional data
- * to the attitudeCorrection function. In this case, it is being cast to a float pointer and then
- * accessed as an array. The first element of the array is assigned to the temp.yawSetPoint variable,
- * and
- *
- * @return a uint32_t value.
- */
-uint32_t g1GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
-                                            const AMOV_GIMBAL_VELOCITY_T &speed,
-                                            const AMOV_GIMBAL_VELOCITY_T &acc,
-                                            void *extenData)
-{
-    G1::GIMBAL_ATT_CORR_MSG_T temp;
-    temp.q[0] = quaterion.q0;
-    temp.q[1] = quaterion.q1;
-    temp.q[2] = quaterion.q2;
-    temp.q[3] = quaterion.q3;
-
-    temp.acc[0] = acc.x;
-    temp.acc[1] = acc.y;
-    temp.acc[2] = acc.z;
-
-    temp.yawSetPoint = ((float *)extenData)[0];
-    temp.yawSpeedSetPoint = ((float *)extenData)[1];
-
-    return pack(G1::GIMBAL_CMD_SET_STATE, reinterpret_cast<uint8_t *>(&temp), sizeof(G1::GIMBAL_ATT_CORR_MSG_T));
-}
-
-/**
- * The function `attitudeCorrection` calculates the attitude correction for a gimbal based on the given
- * position, velocity, and acceleration values.
- *
- * @param pos The "pos" parameter is of type AMOV_GIMBAL_POS_T and represents the current
- * position of the gimbal. It contains the pitch, roll, and yaw angles of the gimbal.
- * @param seppd seppd stands for "Separate Pointing Device" and it represents the velocity of the
- * gimbal in terms of pitch, roll, and yaw. It is of type `AMOV_GIMBAL_VELOCITY_T` which
- * likely contains three float values for pitch,
- * @param acc The "acc" parameter is of type "AMOV_GIMBAL_VELOCITY_T" and represents the
- * acceleration of the gimbal in three dimensions (x, y, z).
- * @param extenData The `extenData` parameter is a void pointer that can be used to pass additional
- * data to the `attitudeCorrection` function. In this code snippet, it is assumed that `extenData` is a
- * pointer to a float array with two elements.
- *
- * @return a uint32_t value.
- */
-uint32_t g1GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
-                                            const AMOV_GIMBAL_VELOCITY_T &seppd,
-                                            const AMOV_GIMBAL_VELOCITY_T &acc,
-                                            void *extenData)
-{
-    G1::GIMBAL_ATT_CORR_MSG_T temp;
-
-    float pitch = pos.pitch * 0.5f;
-    float roll = pos.roll * 0.5f;
-    float yaw = pos.yaw * 0.5f;
-
-    temp.q[0] = cosf(pitch) * cosf(roll) * cosf(yaw) +
-                sinf(pitch) * sinf(roll) * sinf(yaw);
-    temp.q[1] = cosf(pitch) * sinf(roll) * cosf(yaw) -
-                sinf(pitch) * cosf(roll) * sinf(yaw);
-    temp.q[2] = sinf(pitch) * cosf(roll) * cosf(yaw) +
-                cosf(pitch) * sinf(roll) * sinf(yaw);
-    temp.q[3] = cosf(pitch) * sinf(roll) * sinf(yaw) -
-                sinf(pitch) * cosf(roll) * cosf(yaw);
-
-    temp.acc[0] = acc.x;
-    temp.acc[1] = acc.y;
-    temp.acc[2] = acc.z;
-
-    temp.yawSetPoint = ((float *)extenData)[0];
-    temp.yawSpeedSetPoint = ((float *)extenData)[1];
-
-    return pack(G1::GIMBAL_CMD_SET_STATE, reinterpret_cast<uint8_t *>(&temp), sizeof(G1::GIMBAL_ATT_CORR_MSG_T));
-}
-
-/**
- * The function `extensionFuntions` in the `g1GimbalDriver` class takes a void pointer `cmd`, casts it
- * to a `G1::GIMBAL_STD_MSG_T` pointer, and returns the result of calling the `pack` function with the
- * `cmd`'s `cmd`, `data`, and `len` members as arguments.
- * 
- * @param cmd The "cmd" parameter is a void pointer, which means it can point to any type of data. In
- * this case, it is being cast to a G1::GIMBAL_STD_MSG_T pointer using reinterpret_cast.
- * 
- * @return the result of the `pack` function, which is of type `uint32_t`.
- */
-uint32_t g1GimbalDriver::extensionFuntions(void *cmd)
-{
-    G1::GIMBAL_STD_MSG_T *tempCmd;
-
-    tempCmd = reinterpret_cast<G1::GIMBAL_STD_MSG_T *>(cmd);
-    return pack(tempCmd->cmd, tempCmd->data, tempCmd->len);
-}

gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h

@@ -3,8 +3,8 @@
  * @Author: L LC @amov
  * @Date: 2022-10-27 18:10:07
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:29:48
+ * @LastEditTime: 2023-03-17 18:12:57
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/G1/g1_gimbal_struct.h
+ * @FilePath: \gimbal-sdk-multi-platform\src\G1\g1_gimbal_struct.h
  */
 #ifndef G1_GIMBAL_STRUCT_H
 #define G1_GIMBAL_STRUCT_H
@@ -20,7 +20,6 @@ namespace G1
 
     typedef enum
     {
-        GIMBAL_CMD_SET_STATE = 0X01,
         GIMBAL_CMD_SET_POS = 0X85,
         GIMBAL_CMD_CAMERA = 0X86,
         GIMBAL_CMD_RCV_POS = 0X87
@@ -86,22 +85,6 @@ namespace G1
         int16_t HALL_yaw;
     } GIMBAL_RCV_POS_MSG_T;
 
-    typedef struct
-    {
-        float q[4];
-        float acc[3];
-        float yawSetPoint;
-        float yawSpeedSetPoint;
-    } GIMBAL_ATT_CORR_MSG_T;
-
-
-    typedef struct 
-    {
-        uint8_t cmd;
-        uint8_t data[256];
-        uint8_t len;
-    }GIMBAL_STD_MSG_T;
-    
 #pragma pack()
 
 }

gimbal_ctrl/driver/src/G2/g2_gimbal_crc.h

@@ -0,0 +1,166 @@
+
+#ifndef __G2_GIMBAL_CHECK_H
+#define __G2_GIMBAL_CHECK_H
+
+namespace G2
+{
+#include "stdint.h"
+
+    const uint16_t crc16_tab[256] = {
+        0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
+        0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
+        0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+        0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
+        0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
+        0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+        0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
+        0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
+        0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+        0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
+        0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
+        0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+        0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
+        0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
+        0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+        0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
+        0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
+        0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+        0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
+        0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
+        0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+        0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+        0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
+        0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+        0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
+        0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
+        0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+        0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
+        0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
+        0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+        0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
+        0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
+
+    /**
+     * "For each byte in the data, shift the CRC register left by 8 bits, XOR the CRC register with the CRC
+     * table value for the byte, and then shift the CRC register right by 8 bits."
+     *
+     * The CRC table is a 256-byte array of 16-bit values.  The index into the table is the byte value.
+     * The value in the table is the CRC value for that byte.  The CRC table is generated by the following
+     * function:
+     *
+     * @param data pointer to the data to be checked
+     * @param len the length of the data to be checked
+     *
+     * @return The CRC value.
+     * @note 16 bit CRC with polynomial x^16+x^12+x^5+1
+     */
+    static inline uint16_t checkCrc16(uint8_t *pData, uint32_t len)
+    {
+        uint16_t crc = 0XFFFF;
+        uint32_t idx = 0;
+
+        for (idx = 0; idx < len; idx++)
+        {
+            crc = crc16_tab[((crc >> 8) ^ pData[idx]) & 0xFF] ^ (crc << 8);
+        }
+        return crc;
+    }
+
+    const unsigned int Crc32Table[256] = {
+        0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B,
+        0x1A864DB2, 0x1E475005, 0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61,
+        0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD, 0x4C11DB70, 0x48D0C6C7,
+        0x4593E01E, 0x4152FDA9, 0x5F15ADAC, 0x5BD4B01B, 0x569796C2, 0x52568B75,
+        0x6A1936C8, 0x6ED82B7F, 0x639B0DA6, 0x675A1011, 0x791D4014, 0x7DDC5DA3,
+        0x709F7B7A, 0x745E66CD, 0x9823B6E0, 0x9CE2AB57, 0x91A18D8E, 0x95609039,
+        0x8B27C03C, 0x8FE6DD8B, 0x82A5FB52, 0x8664E6E5, 0xBE2B5B58, 0xBAEA46EF,
+        0xB7A96036, 0xB3687D81, 0xAD2F2D84, 0xA9EE3033, 0xA4AD16EA, 0xA06C0B5D,
+        0xD4326D90, 0xD0F37027, 0xDDB056FE, 0xD9714B49, 0xC7361B4C, 0xC3F706FB,
+        0xCEB42022, 0xCA753D95, 0xF23A8028, 0xF6FB9D9F, 0xFBB8BB46, 0xFF79A6F1,
+        0xE13EF6F4, 0xE5FFEB43, 0xE8BCCD9A, 0xEC7DD02D, 0x34867077, 0x30476DC0,
+        0x3D044B19, 0x39C556AE, 0x278206AB, 0x23431B1C, 0x2E003DC5, 0x2AC12072,
+        0x128E9DCF, 0x164F8078, 0x1B0CA6A1, 0x1FCDBB16, 0x018AEB13, 0x054BF6A4,
+        0x0808D07D, 0x0CC9CDCA, 0x7897AB07, 0x7C56B6B0, 0x71159069, 0x75D48DDE,
+        0x6B93DDDB, 0x6F52C06C, 0x6211E6B5, 0x66D0FB02, 0x5E9F46BF, 0x5A5E5B08,
+        0x571D7DD1, 0x53DC6066, 0x4D9B3063, 0x495A2DD4, 0x44190B0D, 0x40D816BA,
+        0xACA5C697, 0xA864DB20, 0xA527FDF9, 0xA1E6E04E, 0xBFA1B04B, 0xBB60ADFC,
+        0xB6238B25, 0xB2E29692, 0x8AAD2B2F, 0x8E6C3698, 0x832F1041, 0x87EE0DF6,
+        0x99A95DF3, 0x9D684044, 0x902B669D, 0x94EA7B2A, 0xE0B41DE7, 0xE4750050,
+        0xE9362689, 0xEDF73B3E, 0xF3B06B3B, 0xF771768C, 0xFA325055, 0xFEF34DE2,
+        0xC6BCF05F, 0xC27DEDE8, 0xCF3ECB31, 0xCBFFD686, 0xD5B88683, 0xD1799B34,
+        0xDC3ABDED, 0xD8FBA05A, 0x690CE0EE, 0x6DCDFD59, 0x608EDB80, 0x644FC637,
+        0x7A089632, 0x7EC98B85, 0x738AAD5C, 0x774BB0EB, 0x4F040D56, 0x4BC510E1,
+        0x46863638, 0x42472B8F, 0x5C007B8A, 0x58C1663D, 0x558240E4, 0x51435D53,
+        0x251D3B9E, 0x21DC2629, 0x2C9F00F0, 0x285E1D47, 0x36194D42, 0x32D850F5,
+        0x3F9B762C, 0x3B5A6B9B, 0x0315D626, 0x07D4CB91, 0x0A97ED48, 0x0E56F0FF,
+        0x1011A0FA, 0x14D0BD4D, 0x19939B94, 0x1D528623, 0xF12F560E, 0xF5EE4BB9,
+        0xF8AD6D60, 0xFC6C70D7, 0xE22B20D2, 0xE6EA3D65, 0xEBA91BBC, 0xEF68060B,
+        0xD727BBB6, 0xD3E6A601, 0xDEA580D8, 0xDA649D6F, 0xC423CD6A, 0xC0E2D0DD,
+        0xCDA1F604, 0xC960EBB3, 0xBD3E8D7E, 0xB9FF90C9, 0xB4BCB610, 0xB07DABA7,
+        0xAE3AFBA2, 0xAAFBE615, 0xA7B8C0CC, 0xA379DD7B, 0x9B3660C6, 0x9FF77D71,
+        0x92B45BA8, 0x9675461F, 0x8832161A, 0x8CF30BAD, 0x81B02D74, 0x857130C3,
+        0x5D8A9099, 0x594B8D2E, 0x5408ABF7, 0x50C9B640, 0x4E8EE645, 0x4A4FFBF2,
+        0x470CDD2B, 0x43CDC09C, 0x7B827D21, 0x7F436096, 0x7200464F, 0x76C15BF8,
+        0x68860BFD, 0x6C47164A, 0x61043093, 0x65C52D24, 0x119B4BE9, 0x155A565E,
+        0x18197087, 0x1CD86D30, 0x029F3D35, 0x065E2082, 0x0B1D065B, 0x0FDC1BEC,
+        0x3793A651, 0x3352BBE6, 0x3E119D3F, 0x3AD08088, 0x2497D08D, 0x2056CD3A,
+        0x2D15EBE3, 0x29D4F654, 0xC5A92679, 0xC1683BCE, 0xCC2B1D17, 0xC8EA00A0,
+        0xD6AD50A5, 0xD26C4D12, 0xDF2F6BCB, 0xDBEE767C, 0xE3A1CBC1, 0xE760D676,
+        0xEA23F0AF, 0xEEE2ED18, 0xF0A5BD1D, 0xF464A0AA, 0xF9278673, 0xFDE69BC4,
+        0x89B8FD09, 0x8D79E0BE, 0x803AC667, 0x84FBDBD0, 0x9ABC8BD5, 0x9E7D9662,
+        0x933EB0BB, 0x97FFAD0C, 0xAFB010B1, 0xAB710D06, 0xA6322BDF, 0xA2F33668,
+        0xBCB4666D, 0xB8757BDA, 0xB5365D03, 0xB1F740B4};
+
+    /**
+     * For each byte in the input data, XOR the current CRC value with the byte, then shift the CRC value
+     * left 8 bits, and XOR the CRC value with the CRC table value for the byte
+     *
+     * @param pData pointer to the data to be CRC'd
+     * @param Length The length of the data to be CRC'd.
+     *
+     * @return The CRC32 value of the data.
+     */
+    static inline uint32_t checkCRC32(uint8_t *pData, uint32_t Length)
+    {
+        unsigned int nReg;
+        unsigned int nTemp = 0;
+        unsigned short i, n;
+
+        nReg = 0xFFFFFFFF;
+        for (n = 0; n < Length; n++)
+        {
+            nReg ^= (unsigned int)pData[n];
+
+            for (i = 0; i < 4; i++)
+            {
+                nTemp = Crc32Table[(unsigned char)((nReg >> 24) & 0xff)];
+                nReg <<= 8;
+                nReg ^= nTemp;
+            }
+        }
+        return nReg;
+    }
+
+    /**
+     * It takes a pointer to an array of bytes and the length of the array, and returns the sum of the
+     * bytes in the array
+     *
+     * @param pData The data to be calculated
+     * @param Lenght The length of the data to be sent.
+     *
+     * @return The sum of the bytes in the array.
+     */
+    static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght)
+    {
+        unsigned short temp = 0;
+        unsigned short i = 0;
+        for (i = 0; i < Lenght; i++)
+        {
+            temp += pData[i];
+        }
+        return temp & 0XFF;
+    }
+
+}
+
+#endif

gimbal_ctrl/driver/src/G2/g2_gimbal_driver.cpp

@@ -0,0 +1,243 @@
+/*
+ * @Description:
+ * @Author: L LC @amov
+ * @Date: 2023-03-01 10:12:58
+ * @LastEditors: L LC @amov
+ * @LastEditTime: 2023-04-11 17:33:42
+ * @FilePath: /gimbal-sdk-multi-platform/src/G2/g2_gimbal_driver.cpp
+ */
+
+#include "g2_gimbal_driver.h"
+#include "g2_gimbal_crc.h"
+#include "string.h"
+
+/**
+ * The function creates a new instance of the g2GimbalDriver class, which is a subclass of the
+ * IamovGimbalBase class
+ *
+ * @param _IO The IOStreamBase class that is used to communicate with the gimbal.
+ */
+g2GimbalDriver::g2GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO)
+{
+    memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T));
+    memset(&txQueue, 0, sizeof(RING_FIFO_CB_T));
+
+    rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T));
+    if (rxBuffer == NULL)
+    {
+        std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
+        exit(1);
+    }
+    txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T));
+    if (txBuffer == NULL)
+    {
+        free(rxBuffer);
+        std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
+        exit(1);
+    }
+
+    Ring_Fifo_init(&rxQueue, sizeof(G2::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T));
+    Ring_Fifo_init(&txQueue, sizeof(G2::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(G2::GIMBAL_FRAME_T));
+
+    parserState = G2::GIMBAL_SERIAL_STATE_IDEL;
+}
+
+/**
+ * It takes a command, a pointer to a payload, and the size of the payload, and then it puts the
+ * command, the payload, and the CRC into a ring buffer
+ *
+ * @param uint32_t 4 bytes
+ * @param pPayload pointer to the data to be sent
+ * @param payloadSize the size of the payload in bytes
+ *
+ * @return The number of bytes in the packet.
+ */
+uint32_t g2GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize)
+{
+    uint32_t ret = 0;
+    G2::GIMBAL_FRAME_T txTemp;
+
+    txTemp.head = G2_SERIAL_HEAD;
+    txTemp.version = G2_SERIAL_VERSION;
+    txTemp.len = payloadSize;
+    txTemp.command = cmd;
+    txTemp.source = self;
+    txTemp.target = remote;
+    memcpy(txTemp.data, pPayload, payloadSize);
+    txTemp.crc.f16 = G2::checkCrc16((uint8_t *)&txTemp, txTemp.len + G2_PAYLOAD_OFFSET);
+    memcpy(txTemp.data + payloadSize, txTemp.crc.f8, sizeof(uint16_t));
+
+    txMutex.lock();
+    if (Ring_Fifo_in_cell(&txQueue, &txTemp))
+    {
+        ret = txTemp.len + G2_PAYLOAD_OFFSET + sizeof(uint16_t);
+    }
+    txMutex.unlock();
+
+    return ret;
+}
+
+/**
+ * > This function is used to get a packet from the receive queue
+ *
+ * @param void This is the type of data that will be stored in the queue.
+ *
+ * @return A boolean value.
+ */
+bool g2GimbalDriver::getRxPack(OUT void *pack)
+{
+    bool state = false;
+    rxMutex.lock();
+    state = Ring_Fifo_out_cell(&rxQueue, pack);
+    rxMutex.unlock();
+    return state;
+}
+
+/**
+ * The function takes a pointer to a buffer, casts it to a pointer to a G2::GIMBAL_FRAME_T, and then
+ * checks the command field of the frame. If the command is G2::IAP_COMMAND_BLOCK_END, it locks the
+ * mutex, and then unlocks it. Otherwise, it prints out the contents of the buffer
+ *
+ * @param buf pointer to the data received from the gimbal
+ */
+void g2GimbalDriver::convert(void *buf)
+{
+    G2::GIMBAL_FRAME_T *temp;
+    temp = reinterpret_cast<G2::GIMBAL_FRAME_T *>(buf);
+    switch (temp->command)
+    {
+    case G2::IAP_COMMAND_BLOCK_END:
+        mState.lock();
+        updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
+                                  state.abs.roll, state.abs.pitch, state.abs.yaw,
+                                  state.fov.x, state.fov.y);
+        mState.unlock();
+        break;
+
+    default:
+        std::cout << "Undefined frame from G2 : ";
+        for (uint16_t i = 0; i < temp->len + G2_PAYLOAD_OFFSET + sizeof(uint32_t); i++)
+        {
+            printf("%02X ", ((uint8_t *)buf)[i]);
+        }
+        std::cout << std::endl;
+        break;
+    }
+}
+
+/**
+ * If the serial port is not busy and is open, then lock the txMutex, get the next byte from the
+ * txQueue, unlock the txMutex, and send the byte
+ */
+void g2GimbalDriver::send(void)
+{
+    if (!IO->isBusy() && IO->isOpen())
+    {
+        bool state = false;
+        txMutex.lock();
+        state = Ring_Fifo_out_cell(&txQueue, &tx);
+        txMutex.unlock();
+        if (state)
+        {
+            IO->outPutBytes((uint8_t *)&tx, tx.len + G2_PAYLOAD_OFFSET + sizeof(uint16_t));
+        }
+    }
+}
+
+/**
+ * The function is called every time a byte is received from the serial port. It parses the byte and
+ * stores it in a buffer. When the buffer is full, it checks the CRC and if it's correct, it stores the
+ * buffer in a queue
+ *
+ * @param uint8_t unsigned char
+ *
+ * @return The parser function is returning a boolean value.
+ */
+bool g2GimbalDriver::parser(IN uint8_t byte)
+{
+    bool state = false;
+    static uint8_t payloadLenghte = 0;
+    static uint8_t *pRx = NULL;
+
+    switch (parserState)
+    {
+    case G2::GIMBAL_SERIAL_STATE_IDEL:
+        if (byte == G2_SERIAL_HEAD)
+        {
+            rx.head = byte;
+            parserState = G2::GIMBAL_SERIAL_STATE_HEAD_RCV;
+        }
+        break;
+
+    case G2::GIMBAL_SERIAL_STATE_HEAD_RCV:
+        if (byte == G2_SERIAL_VERSION)
+        {
+            rx.version = byte;
+            parserState = G2::GIMBAL_SERIAL_STATE_VERSION_RCV;
+        }
+        else
+        {
+            rx.head = 0;
+            parserState = G2::GIMBAL_SERIAL_STATE_IDEL;
+        }
+        break;
+
+    case G2::GIMBAL_SERIAL_STATE_VERSION_RCV:
+        rx.target = byte;
+        parserState = G2::GIMBAL_SERIAL_STATE_TARGET_RCV;
+        break;
+
+    case G2::GIMBAL_SERIAL_STATE_TARGET_RCV:
+        rx.source = byte;
+        parserState = G2::GIMBAL_SERIAL_STATE_SOURCE_RCV;
+        break;
+
+    case G2::GIMBAL_SERIAL_STATE_SOURCE_RCV:
+        rx.len = byte;
+        parserState = G2::GIMBAL_SERIAL_STATE_LENGHT_RCV;
+        pRx = rx.data;
+        payloadLenghte = byte;
+        break;
+
+    case G2::GIMBAL_SERIAL_STATE_LENGHT_RCV:
+        rx.command = byte;
+        parserState = G2::GIMBAL_SERIAL_STATE_DATA_RCV;
+        break;
+
+    case G2::GIMBAL_SERIAL_STATE_DATA_RCV:
+        *pRx = byte;
+        payloadLenghte--;
+        if (payloadLenghte == 0)
+        {
+            parserState = G2::GIMBAL_SERIAL_STATE_CRC_RCV1;
+        }
+        break;
+
+    case G2::GIMBAL_SERIAL_STATE_CRC_RCV1:
+        rx.crc.f8[1] = byte;
+        parserState = G2::GIMBAL_SERIAL_STATE_END;
+        break;
+
+    case G2::GIMBAL_SERIAL_STATE_END:
+        rx.crc.f8[0] = byte;
+
+        if (rx.crc.f16 == G2::checkCrc16((uint8_t *)&rx, G2_PAYLOAD_OFFSET + rx.len))
+        {
+            state = true;
+            rxMutex.lock();
+            Ring_Fifo_in_cell(&rxQueue, &rx);
+            rxMutex.unlock();
+        }
+        else
+        {
+            memset(&rx, 0, sizeof(G2::GIMBAL_FRAME_T));
+        }
+        parserState = G2::GIMBAL_SERIAL_STATE_IDEL;
+        break;
+
+    default:
+        parserState = G2::GIMBAL_SERIAL_STATE_IDEL;
+        break;
+    }
+    return state;
+}

gimbal_ctrl/driver/src/G2/g2_gimbal_driver.h

@@ -0,0 +1,76 @@
+/*
+ * @Description:
+ * @Author: L LC @amov
+ * @Date: 2023-03-01 10:02:24
+ * @LastEditors: L LC @amov
+ * @LastEditTime: 2023-03-13 12:29:33
+ * @FilePath: \gimbal-sdk-multi-platform\src\G2\g2_gimbal_driver.h
+ */
+#include "../amov_gimbal.h"
+#include "g2_gimbal_struct.h"
+#include <mutex>
+#include <malloc.h>
+#include <iostream>
+
+#ifndef __G2_DRIVER_H
+#define __G2_DRIVER_H
+
+extern "C"
+{
+#include "Ring_Fifo.h"
+}
+
+class g2GimbalDriver : protected amovGimbal::IamovGimbalBase
+{
+private:
+    G2::GIMBAL_CMD_PARSER_STATE_T parserState;
+    G2::GIMBAL_FRAME_T rx;
+    G2::GIMBAL_FRAME_T tx;
+
+    std::mutex rxMutex;
+    uint8_t *rxBuffer;
+    RING_FIFO_CB_T rxQueue;
+    std::mutex txMutex;
+    uint8_t *txBuffer;
+    RING_FIFO_CB_T txQueue;
+
+    uint8_t self;
+    uint8_t remote;
+
+    bool parser(IN uint8_t byte);
+    void send(void);
+
+    void convert(void *buf);
+    uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
+    bool getRxPack(OUT void *pack);
+
+public:
+    void nodeSet(SET uint32_t _self, SET uint32_t _remote)
+    {
+        self = _self;
+        remote = _remote;
+    }
+
+    // funtion
+    uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos);
+    uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed);
+    uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed);
+    uint32_t setGimabalHome(void);
+
+    uint32_t takePic(void);
+    uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
+
+    static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
+    {
+        return new g2GimbalDriver(_IO);
+    }
+
+    g2GimbalDriver(amovGimbal::IOStreamBase *_IO);
+    ~g2GimbalDriver()
+    {
+        free(rxBuffer);
+        free(txBuffer);
+    }
+};
+
+#endif

gimbal_ctrl/driver/src/G2/g2_gimbal_funtion.cpp

@@ -0,0 +1,81 @@
+/*
+ * @Description:
+ * @Author: L LC @amov
+ * @Date: 2023-03-13 11:58:54
+ * @LastEditors: L LC @amov
+ * @LastEditTime: 2023-03-13 12:31:58
+ * @FilePath: \gimbal-sdk-multi-platform\src\G2\g2_gimbal_funtion.cpp
+ */
+#include "g2_gimbal_driver.h"
+#include "g2_gimbal_crc.h"
+#include "string.h"
+
+/**
+ * It sets the gimbal position.
+ *
+ * @param pos the position of the gimbal
+ *
+ * @return The return value is the number of bytes written to the buffer.
+ */
+uint32_t g2GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
+{
+    return 0;
+}
+
+/**
+ * It takes a struct of type amovGimbal::AMOV_GIMBAL_POS_T and converts it to a struct of type
+ * G1::GIMBAL_SET_POS_MSG_T
+ *
+ * @param speed the speed of the gimbal
+ *
+ * @return The return value is the number of bytes written to the buffer.
+ */
+uint32_t g2GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
+{
+    return 0;
+}
+
+/**
+ * This function sets the gimbal's follow speed
+ *
+ * @param followSpeed the speed of the gimbal
+ *
+ * @return The return value is the number of bytes written to the buffer.
+ */
+uint32_t g2GimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
+{
+    return 0;
+}
+
+/**
+ * This function sets the gimbal to its home position
+ *
+ * @return The return value is the number of bytes written to the buffer.
+ */
+uint32_t g2GimbalDriver::setGimabalHome(void)
+{
+    return 0;
+}
+
+/**
+ * It takes a picture.
+ *
+ * @return The return value is the number of bytes written to the serial port.
+ */
+uint32_t g2GimbalDriver::takePic(void)
+{
+    return 0;
+}
+
+/**
+ * The function sets the video state of the gimbal
+ *
+ * @param newState The new state of the video.
+ *
+ * @return The return value is the number of bytes written to the serial port.
+ */
+uint32_t g2GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
+{
+
+    return 0;
+}

gimbal_ctrl/driver/src/G2/g2_gimbal_struct.h

@@ -0,0 +1,81 @@
+/*
+ * @Description:
+ * @Author: L LC @amov
+ * @Date: 2023-03-01 09:21:57
+ * @LastEditors: L LC @amov
+ * @LastEditTime: 2023-04-18 10:13:23
+ * @FilePath: /gimbal-sdk-multi-platform/src/G2/g2_gimbal_struct.h
+ */
+
+#ifndef G2_GIMBAL_STRUCT_H
+#define G2_GIMBAL_STRUCT_H
+
+#include <stdint.h>
+
+namespace G2
+{
+
+#define G2_MAX_GIMBAL_PAYLOAD 64
+#define G2_PAYLOAD_OFFSET 6
+#define G2_SCALE_FACTOR 0.01f
+#define G2_SERIAL_HEAD 0XAF
+#define G2_SERIAL_VERSION 0X02
+
+    typedef enum
+    {
+        IAP_COMMAND_JUMP = 80,
+        IAP_COMMAND_FLASH_ERASE,
+        IAP_COMMAND_BOLCK_INFO,
+        IAP_COMMAND_BLOCK_WRITE,
+        IAP_COMMAND_SOFT_INFO,
+        IAP_COMMAND_HARDWARE_INFO,
+        IAP_COMMAND_BLOCK_START,
+        IAP_COMMAND_BLOCK_END = 117,
+    } GIMBAL_CMD_T;
+
+    typedef enum
+    {
+        IAP_STATE_FAILD = 0,
+        IAP_STATE_SUCCEED,
+        IAP_STATE_READY,
+        IAP_STATE_FIRMWARE_BROKE,
+        IAP_STATE_JUMP_FAILD,
+        IAP_STATE_ADDR_ERR,
+        IAP_STATE_CRC_ERR,
+        IAP_STATE_WRITE_ERR,
+        IAP_STATE_WRITE_TIMEOUT,
+    } GIMBAL_IAP_STATE_T;
+
+    typedef enum
+    {
+        GIMBAL_SERIAL_STATE_IDEL = 0,
+        GIMBAL_SERIAL_STATE_HEAD_RCV,
+        GIMBAL_SERIAL_STATE_VERSION_RCV,
+        GIMBAL_SERIAL_STATE_TARGET_RCV,
+        GIMBAL_SERIAL_STATE_SOURCE_RCV,
+        GIMBAL_SERIAL_STATE_LENGHT_RCV,
+        GIMBAL_SERIAL_STATE_DATA_RCV,
+        GIMBAL_SERIAL_STATE_CRC_RCV1,
+        GIMBAL_SERIAL_STATE_END,
+    } GIMBAL_CMD_PARSER_STATE_T;
+
+#pragma pack(1)
+    typedef struct
+    {
+        uint8_t head;
+        uint8_t version;
+        uint8_t target;
+        uint8_t source;
+        uint8_t len;
+        uint8_t command;
+        uint8_t data[G2_MAX_GIMBAL_PAYLOAD];
+        union
+        {
+            uint8_t f8[2];
+            uint16_t f16;
+        } crc;
+    } GIMBAL_FRAME_T;
+#pragma pack(0)
+
+}
+#endif

gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h

@@ -1,41 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-10-20 16:33:07
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:29:39
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_crc16.h
- */
-#ifndef GX40_GIMBAL_CRC16_H
-#define GX40_GIMBAL_CRC16_H
-
-#include <stdint.h>
-
-namespace GX40
-{
-    const static uint16_t crc16Tab[16] = {
-        0x0000, 0x1021, 0x2042, 0x3063,
-        0x4084, 0x50a5, 0x60c6, 0x70e7,
-        0x8108, 0x9129, 0xa14a, 0xb16b,
-        0xc18c, 0xd1ad, 0xe1ce, 0xf1ef};
-
-    static inline uint16_t CalculateCrc16(const uint8_t *ptr, uint8_t len)
-    {
-        uint16_t crc = 0;
-        uint8_t temp;
-
-        while (len-- != 0)
-        {
-            temp = crc >> 12;
-            crc <<= 4;
-            crc ^= crc16Tab[temp ^ (*ptr >> 4)];
-            temp = crc >> 12;
-            crc <<= 4;
-            crc ^= crc16Tab[temp ^ (*ptr & 0x0F)];
-            ptr++;
-        }
-        crc = (crc >> 8) | (crc << 8);
-        return (crc);
-    }
-}
-#endif

gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp

@@ -1,304 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-10-20 16:08:17
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-06 10:27:28
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.cpp
- */
-#include <string.h>
-#include "GX40_gimbal_driver.h"
-#include "GX40_gimbal_crc16.h"
-#include <math.h>
-
-/**
- * The above function is a constructor for the GX40GimbalDriver class in C++, which initializes member
- * variables and sets the parser state to idle.
- *
- * @param _IO _IO is a pointer to an object of type amovGimbal::IOStreamBase. It is used to communicate
- * with the gimbal device.
- */
-GX40GimbalDriver::GX40GimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO)
-{
-    rxQueue = new fifoRing(sizeof(GX40::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
-    txQueue = new fifoRing(sizeof(GX40::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
-
-    targetPos[0] = 0;
-    targetPos[1] = 0;
-    targetPos[2] = 0;
-
-    parserState = GX40::GIMBAL_FRAME_PARSER_STATE_IDLE;
-    upDataTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()) - std::chrono::milliseconds(2000);
-}
-
-/**
- * The function `nopSend` continuously sends a "no operation" command to a GX40 gimbal driver.
- */
-void GX40GimbalDriver::nopSend(void)
-{
-    while (1)
-    {
-        // 50Hz
-        std::this_thread::sleep_for(std::chrono::milliseconds(20));
-        pack(GX40::GIMBAL_CMD_NOP, nullptr, 0);
-    }
-}
-
-/**
- * The function `parserStart` initializes the gimbal driver by setting the callback function, creating
- * two threads for the main loop and sending NOP commands, and detaching the threads.
- *
- * @param callback The parameter "callback" is of type "amovGimbal::pStateInvoke", which is a function
- * pointer type. It is used to specify a callback function that will be invoked when the gimbal state
- * is updated.
- */
-void GX40GimbalDriver::parserStart(pAmovGimbalStateInvoke callback, void *caller)
-{
-    this->updateGimbalStateCallback = callback;
-    this->updataCaller = caller;
-
-    std::thread mainLoop(&GX40GimbalDriver::mainLoop, this);
-    std::thread sendNop(&GX40GimbalDriver::nopSend, this);
-
-    this->stackThreadHanle = mainLoop.native_handle();
-    this->nopSendThreadHandle = sendNop.native_handle();
-
-    mainLoop.detach();
-    sendNop.detach();
-}
-
-/**
- * The function `pack` in the `GX40GimbalDriver` class is responsible for packing data into a frame for
- * transmission.
- *
- * @param uint32_t The parameter `cmd` is an unsigned 32-bit integer representing the command.
- * @param pPayload The `pPayload` parameter is a pointer to the payload data that needs to be packed.
- * It is of type `uint8_t*`, which means it is a pointer to an array of unsigned 8-bit integers. The
- * payload data is stored in this array.
- * @param payloadSize The parameter `payloadSize` represents the size of the payload data in bytes. It
- * is used to determine the size of the payload data that needs to be packed into the `temp` structure.
- *
- * @return a uint32_t value, which is stored in the variable "ret".
- */
-uint32_t GX40GimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize)
-{
-    uint32_t ret = 0;
-    GX40::GIMBAL_FRAME_T temp;
-    memset(&temp, 0, sizeof(GX40::GIMBAL_FRAME_T));
-    GX40::GIMBAL_PRIMARY_MASTER_FRAME_T *primary = (GX40::GIMBAL_PRIMARY_MASTER_FRAME_T *)temp.primaryData;
-    carrierStateMutex.lock();
-
-    primary->state = 0X00;
-    // 姿态数据&指令数据填充
-    primary->roll = targetPos[0];
-    primary->pitch = targetPos[1];
-    primary->yaw = targetPos[2];
-
-    primary->state |= (0X01 << 2);
-
-    temp.otherData[0] = cmd;
-    memcpy(temp.otherData + 1, pPayload, payloadSize);
-
-    // 固定字节填充
-    temp.head.u16 = XF_SEND_HEAD;
-    temp.version = 0X01;
-    primary->secondaryFlag = 0X01;
-    temp.lenght.u16 = 69 + payloadSize + 1 + 2;
-
-    // 惯导数据填充
-    std::chrono::milliseconds nowTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
-    // over 1s GNSS has losed
-    if ((nowTs.count() - upDataTs.count()) < std::chrono::milliseconds(1500).count() &&
-        cmd == GX40::GIMBAL_CMD_NOP)
-    {
-        primary->selfRoll = (int16_t)(-(carrierPos.roll / 0.01f));
-        primary->selfPitch = (int16_t)(-(carrierPos.pitch / 0.01f));
-        primary->selfYaw = (int16_t)(carrierPos.yaw / 0.01f);
-        primary->accE = (int16_t)(carrierAcc.y / 0.01f);
-        primary->accN = (int16_t)(carrierAcc.x / 0.01f);
-        primary->accUp = (int16_t)(carrierAcc.z / 0.01f);
-        primary->speedE = (int16_t)(carrierSpeed.y / 0.01f);
-        primary->speedN = (int16_t)(carrierSpeed.x / 0.01f);
-        primary->speedUp = (int16_t)(carrierSpeed.z / 0.01f);
-
-        carrierGNSS.GPSweeks = ((nowTs.count() / 1000) - 315964800) / 604800;
-        carrierGNSS.GPSms = nowTs.count() - (carrierGNSS.GPSweeks * 604800000);
-
-        memcpy(temp.secondaryData, &carrierGNSS, sizeof(GX40::GIMBAL_SECONDARY_MASTER_FRAME_T));
-
-        primary->state |= (0X01 << 0);
-    }
-    else
-    {
-        primary->state &= (~(0X01 << 0));
-    }
-
-    carrierStateMutex.unlock();
-
-    // 校验
-    *(uint16_t *)(&temp.otherData[payloadSize + 1]) = GX40::CalculateCrc16((uint8_t *)&temp, 69 + 1 + payloadSize);
-
-    // 添加至发送队列
-    if (txQueue->inCell(&temp))
-    {
-        ret = temp.lenght.u16;
-    }
-    return ret;
-}
-
-/**
- * The function `convert` takes a buffer and extracts data from it to update the state of a gimbal
- * driver.
- *
- * @param buf The `buf` parameter is a void pointer that points to a buffer containing data that needs
- * to be converted.
- */
-void GX40GimbalDriver::convert(void *buf)
-{
-    GX40::GIMBAL_FRAME_T *temp;
-    GX40::GIMBAL_PRIMARY_SLAVE_FRAME_T *primary;
-    GX40::GIMBAL_SECONDARY_SLAVE_FRAME_T *secondary;
-    temp = reinterpret_cast<GX40::GIMBAL_FRAME_T *>(buf);
-    primary = (GX40::GIMBAL_PRIMARY_SLAVE_FRAME_T *)temp->primaryData;
-    secondary = (GX40::GIMBAL_SECONDARY_SLAVE_FRAME_T *)temp->secondaryData;
-
-    mState.lock();
-    this->state.workMode = (AMOV_GIMBAL_SERVO_MODE_T)primary->workMode;
-    this->state.cameraFlag = (AMOV_GIMBAL_CAMERA_FLAG_T)primary->state;
-    // 应该需要再解算一下，才能出具体的框架角度
-    this->state.rel.yaw = -(primary->motorYaw * XF_ANGLE_DPI);
-    this->state.rel.yaw = this->state.rel.yaw < -180.0f ? this->state.rel.yaw + 360.0f : this->state.rel.yaw;
-    this->state.rel.pitch = -(primary->motorPitch * XF_ANGLE_DPI);
-    this->state.rel.roll = -(primary->motorRoll * XF_ANGLE_DPI);
-
-    this->state.abs.yaw = -(primary->yaw * XF_ANGLE_DPI);
-    this->state.abs.yaw = this->state.abs.yaw < -180.0f ? this->state.abs.yaw + 360.0f : this->state.abs.yaw;
-
-    this->state.abs.pitch = -(primary->pitch * XF_ANGLE_DPI);
-    this->state.abs.roll = -(primary->roll * XF_ANGLE_DPI);
-
-    this->state.relSpeed.yaw = -(primary->speedYaw * XF_ANGLE_DPI);
-    this->state.relSpeed.pitch = -(primary->speedPitch * XF_ANGLE_DPI);
-    this->state.relSpeed.roll = -(primary->speedRoll * XF_ANGLE_DPI);
-
-    // 近似值 不准
-    this->state.fov.x = secondary->camera1Zoom * 0.1f;
-    this->state.fov.x = 60.2f / this->state.fov.x;
-    this->state.fov.y = secondary->camera1Zoom * 0.1f;
-    this->state.fov.y = 36.1f / this->state.fov.y;
-
-    updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
-                              state.abs.roll, state.abs.pitch, state.abs.yaw,
-                              state.fov.x, state.fov.y, updataCaller);
-
-    mState.unlock();
-}
-
-/**
- * The function calculates the total length of a data packet by adding the length of the payload to the
- * size of a uint16_t.
- *
- * @param pack The parameter "pack" is a void pointer, which means it can point to any type of data. In
- * this case, it is expected to point to a structure of type "GX40::GIMBAL_FRAME_T".
- *
- * @return the sum of the length of the gimbal frame and the size of a uint16_t.
- */
-uint32_t GX40GimbalDriver::calPackLen(void *pack)
-{
-    return ((GX40::GIMBAL_FRAME_T *)pack)->lenght.u16;
-}
-/**
- * The function `parser` is used to parse incoming data frames in a specific format and returns a
- * boolean value indicating whether the parsing was successful or not.
- *
- * @param uint8_t The parameter `byte` is of type `uint8_t`, which is an unsigned 8-bit integer. It is
- * used to store a single byte of data that is being parsed by the `GX40GimbalDriver::parser` function.
- *
- * @return a boolean value, either true or false.
- */
-bool GX40GimbalDriver::parser(IN uint8_t byte)
-{
-    bool state = false;
-    static uint8_t payloadLenght = 0;
-    static uint8_t *pRx = nullptr;
-
-    switch (parserState)
-    {
-    case GX40::GIMBAL_FRAME_PARSER_STATE_IDLE:
-        if (byte == ((XF_RCV_HEAD >> 8) & 0XFF))
-        {
-            rx.head.u8[0] = byte;
-            parserState = GX40::GIMBAL_FRAME_PARSER_STATE_HEAD;
-        }
-        break;
-
-    case GX40::GIMBAL_FRAME_PARSER_STATE_HEAD:
-        if (byte == ((XF_RCV_HEAD >> 0) & 0XFF))
-        {
-            rx.head.u8[1] = byte;
-            parserState = GX40::GIMBAL_FRAME_PARSER_STATE_LEN1;
-        }
-        else
-        {
-            parserState = GX40::GIMBAL_FRAME_PARSER_STATE_IDLE;
-            rx.head.u16 = 0;
-        }
-        break;
-
-    case GX40::GIMBAL_FRAME_PARSER_STATE_LEN1:
-        rx.lenght.u8[0] = byte;
-        parserState = GX40::GIMBAL_FRAME_PARSER_STATE_LEN2;
-        break;
-
-    case GX40::GIMBAL_FRAME_PARSER_STATE_LEN2:
-        rx.lenght.u8[1] = byte;
-        parserState = GX40::GIMBAL_FRAME_PARSER_STATE_VERSION;
-        break;
-
-    case GX40::GIMBAL_FRAME_PARSER_STATE_VERSION:
-        if (byte == XF_VERSION)
-        {
-            rx.version = byte;
-            parserState = GX40::GIMBAL_FRAME_PARSER_STATE_PAYLOAD;
-            pRx = rx.primaryData;
-            payloadLenght = rx.lenght.u16 - 5;
-        }
-        else
-        {
-            parserState = GX40::GIMBAL_FRAME_PARSER_STATE_IDLE;
-            rx.head.u16 = 0;
-            rx.lenght.u16 = 0;
-        }
-        break;
-
-    case GX40::GIMBAL_FRAME_PARSER_STATE_PAYLOAD:
-        *pRx = byte;
-        payloadLenght--;
-        pRx++;
-        if (payloadLenght <= 0)
-        {
-            if (*(uint16_t *)(pRx - sizeof(uint16_t)) == GX40::CalculateCrc16((uint8_t *)&rx, rx.lenght.u16 - 2))
-            {
-                state = true;
-                rxQueue->inCell(&rx);
-            }
-            else
-            {
-                memset(&rx, 0, sizeof(GX40::GIMBAL_FRAME_T));
-            }
-
-            parserState = GX40::GIMBAL_FRAME_PARSER_STATE_IDLE;
-            pRx = nullptr;
-            payloadLenght = 0;
-        }
-        break;
-
-    default:
-        parserState = GX40::GIMBAL_FRAME_PARSER_STATE_IDLE;
-        pRx = nullptr;
-        payloadLenght = 0;
-        break;
-    }
-
-    return state;
-}

gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h

@@ -1,86 +0,0 @@
-
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-10-20 16:08:13
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-06 10:27:05
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h
- */
-#ifndef __GX40_DRIVER_H
-#define __GX40_DRIVER_H
-#include "../amov_gimbal_private.h"
-#include "GX40_gimbal_struct.h"
-#include <mutex>
-#include <malloc.h>
-#include <iostream>
-#include <chrono>
-#include <time.h>
-
-class GX40GimbalDriver : public amovGimbal::amovGimbalBase
-{
-    GX40::GIMBAL_FRAME_PARSER_STATE_T parserState;
-    GX40::GIMBAL_FRAME_T rx;
-
-    std::chrono::milliseconds upDataTs;
-    std::mutex carrierStateMutex;
-
-    int16_t targetPos[3];
-
-    AMOV_GIMBAL_POS_T carrierPos;
-    AMOV_GIMBAL_VELOCITY_T carrierSpeed;
-    AMOV_GIMBAL_VELOCITY_T carrierAcc;
-    GX40::GIMBAL_SECONDARY_MASTER_FRAME_T carrierGNSS;
-
-    std::thread::native_handle_type nopSendThreadHandle;
-    void nopSend(void);
-    void parserStart(pAmovGimbalStateInvoke callback, void *caller);
-
-public:
-    uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
-    bool parser(IN uint8_t byte);
-    void convert(void *buf);
-    uint32_t calPackLen(void *pack);
-
-    // funtions
-    uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
-    uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
-    uint32_t setGimabalHome(void);
-
-    uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-    uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-
-    uint32_t takePic(void);
-    uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
-    uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
-                                const AMOV_GIMBAL_VELOCITY_T &seppd,
-                                const AMOV_GIMBAL_VELOCITY_T &acc,
-                                void *extenData);
-
-    uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
-    uint32_t extensionFuntions(void *cmd);
-    static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
-    {
-        return new GX40GimbalDriver(_IO);
-    }
-    GX40GimbalDriver(amovGimbal::IOStreamBase *_IO);
-    ~GX40GimbalDriver()
-    {
-        if (txQueue != nullptr)
-        {
-            delete txQueue;
-        }
-        if (rxQueue != nullptr)
-        {
-            delete rxQueue;
-        }
-
-        pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
-        parserThreadHanle = parserThreadHanle == 0 ? 0 : pthread_cancel(parserThreadHanle);
-        sendThreadHanle = sendThreadHanle == 0 ? 0 : pthread_cancel(sendThreadHanle);
-        stackThreadHanle = stackThreadHanle == 0 ? 0 : pthread_cancel(stackThreadHanle);
-        nopSendThreadHandle = nopSendThreadHandle == 0 ? 0 : pthread_cancel(nopSendThreadHandle);
-    }
-};
-
-#endif

gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp

@@ -1,251 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-11-02 17:50:26
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:29:13
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp
- */
-#include <string.h>
-#include "GX40_gimbal_driver.h"
-
-/**
- * The function sets the target position of a gimbal based on the input roll, pitch, and yaw values.
- *
- * @param pos The parameter "pos" is of type "AMOV_GIMBAL_POS_T". It is a structure that
- * contains the roll, pitch, and yaw values of the gimbal position.
- *
- * @return a packed value of type uint32_t.
- */
-uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
-{
-    carrierStateMutex.lock();
-    targetPos[0] = (int16_t)(pos.roll / 0.01f);
-    targetPos[1] = (int16_t)(-pos.pitch / 0.01f);
-    targetPos[2] = (int16_t)(pos.yaw / 0.01f);
-    carrierStateMutex.unlock();
-    return pack(GX40::GIMBAL_CMD_MODE_EULER, nullptr, 0);
-}
-
-/**
- * The function sets the gimbal speed based on the provided roll, pitch, and yaw values.
- *
- * @param speed The parameter "speed" is of type "AMOV_GIMBAL_POS_T". It is a structure
- * that contains the roll, pitch, and yaw values of the gimbal speed.
- *
- * @return the result of the pack() function, which is of type uint32_t.
- */
-uint32_t GX40GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
-{
-    carrierStateMutex.lock();
-    targetPos[0] = (int16_t)(speed.roll / 0.1f);
-    targetPos[1] = (int16_t)(-speed.pitch / 0.1f);
-    targetPos[2] = (int16_t)(speed.yaw / 0.1f);
-    carrierStateMutex.unlock();
-
-    return pack(GX40::GIMBAL_CMD_MODE_FOLLOW, nullptr, 0);
-}
-
-/**
- * The function sets the gimbal's home position to (0, 0, 0) and sends a command to the gimbal to go to
- * the home position.
- *
- * @return the result of the pack() function call with the arguments GX40::GIMBAL_CMD_HOME, nullptr, and
- * 0.
- */
-uint32_t GX40GimbalDriver::setGimabalHome(void)
-{
-    carrierStateMutex.lock();
-    targetPos[0] = 0;
-    targetPos[1] = 0;
-    targetPos[2] = 0;
-    carrierStateMutex.unlock();
-
-    pack(GX40::GIMBAL_CMD_MODE_FOLLOW, nullptr, 0);
-    return pack(GX40::GIMBAL_CMD_HOME, nullptr, 0);
-}
-
-/**
- * The function `takePic` in the `GX40GimbalDriver` class takes a picture using the GX40 gimbal and
- * returns the packed command.
- *
- * @return a uint32_t value.
- */
-uint32_t GX40GimbalDriver::takePic(void)
-{
-    uint8_t temp = 0X01;
-
-    return pack(GX40::GIMBAL_CMD_TAKEPIC, &temp, 1);
-}
-
-/**
- * The function `setVideo` toggles the video state of a gimbal driver and returns a packed command.
- *
- * @param newState The parameter `newState` is of type `AMOV_GIMBAL_VIDEO_T`, which is an
- * enumeration representing the state of the video in the gimbal. It can have two possible values:
- *
- * @return the result of the `pack` function, which is a `uint32_t` value.
- */
-uint32_t GX40GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
-{
-    uint8_t temp = 0X01;
-
-    mState.lock();
-    if (state.video == AMOV_GIMBAL_VIDEO_TAKE)
-    {
-        state.video = AMOV_GIMBAL_VIDEO_OFF;
-    }
-    else
-    {
-        state.video = AMOV_GIMBAL_VIDEO_TAKE;
-    }
-    mState.unlock();
-
-    return pack(GX40::GIMBAL_CMD_TAKEPIC, &temp, 1);
-}
-
-/**
- * The function `attitudeCorrection` updates the state of a gimbal driver with position, velocity, and
- * acceleration data.
- *
- * @param pos The "pos" parameter is of type "AMOV_GIMBAL_POS_T" and represents the current
- * position of the gimbal. It likely contains information such as the pitch, yaw, and roll angles of
- * the gimbal.
- * @param seppd The parameter `seppd` stands for "Separate Pointing Device" and represents the velocity
- * of the gimbal in separate axes (e.g., pitch, yaw, roll). It is of type
- * `AMOV_GIMBAL_VELOCITY_T`.
- * @param acc The "acc" parameter is of type "AMOV_GIMBAL_VELOCITY_T" and represents the
- * acceleration of the gimbal.
- * @param extenData The extenData parameter is a pointer to additional data that can be passed to the
- * attitudeCorrection function. It can be used to provide any extra information or context that may be
- * needed for the attitude correction calculation. The specific type and structure of the extenData is
- * not provided in the code snippet,
- *
- * @return the size of the data being passed as arguments. The size is calculated by adding the sizes
- * of the three types: sizeof(AMOV_GIMBAL_POS_T),
- * sizeof(AMOV_GIMBAL_VELOCITY_T), and sizeof(AMOV_GIMBAL_VELOCITY_T).
- */
-uint32_t GX40GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
-                                              const AMOV_GIMBAL_VELOCITY_T &seppd,
-                                              const AMOV_GIMBAL_VELOCITY_T &acc,
-                                              void *extenData)
-{
-    carrierStateMutex.lock();
-    carrierPos = pos;
-    carrierSpeed = seppd;
-    carrierAcc = acc;
-    upDataTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
-    carrierStateMutex.unlock();
-    return sizeof(AMOV_GIMBAL_POS_T) + sizeof(AMOV_GIMBAL_VELOCITY_T) + sizeof(AMOV_GIMBAL_VELOCITY_T);
-}
-
-/**
- * The function `extensionFuntions` takes a command as input, converts it to a specific format, and
- * returns a 32-bit unsigned integer.
- *
- * @param cmd The parameter "cmd" is a void pointer, which means it can point to any type of data. In
- * this case, it is being cast to a uint8_t pointer, which means it is expected to point to an array of
- * uint8_t (8-bit unsigned integers).
- *
- * @return a value of type uint32_t.
- */
-uint32_t GX40GimbalDriver::extensionFuntions(void *cmd)
-{
-    uint8_t *temp = (uint8_t *)cmd;
-    return pack(temp[0], &temp[2], temp[1]);
-}
-
-/**
- * The function `setGimbalZoom` in the `GX40GimbalDriver` class sets the zoom level of a gimbal based on
- * the specified zoom type and target rate.
- *
- * @param zoom The "zoom" parameter is of type AMOV_GIMBAL_ZOOM_T, which is an enumeration
- * type. It represents the zoom action to be performed on the gimbal. The possible values for this
- * parameter are:
- * @param targetRate The targetRate parameter is a float value representing the desired zoom rate for
- * the gimbal. It is used to control the zoom functionality of the gimbal.
- *
- * @return a value of type uint32_t.
- */
-uint32_t GX40GimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
-{
-    uint8_t temp[4];
-    uint8_t len = 0;
-    temp[1] = 0X01;
-    if (targetRate == 0.0f)
-    {
-        len = 1;
-        switch (zoom)
-        {
-        case AMOV_GIMBAL_ZOOM_IN:
-            temp[0] = GX40::GIMBAL_CMD_ZOMM_IN;
-            break;
-        case AMOV_GIMBAL_ZOOM_OUT:
-            temp[0] = GX40::GIMBAL_CMD_ZOOM_OUT;
-            break;
-        case AMOV_GIMBAL_ZOOM_STOP:
-            temp[0] = GX40::GIMBAL_CMD_ZOOM_STOP;
-            break;
-        }
-    }
-    else
-    {
-        len = 3;
-        temp[0] = GX40::GIMBAL_CMD_ZOOM;
-        int16_t targetTemp = (int16_t)(-targetRate / 0.1f);
-        temp[2] = (targetTemp >> 0) & 0XFF;
-        temp[3] = (targetTemp >> 8) & 0XFF;
-    }
-
-    return pack(temp[0], &temp[1], len);
-}
-
-/**
- * The function "setGimbalFocus" sets the focus of a gimbal by specifying the zoom level and target
- * rate.
- *
- * @param zoom The zoom parameter is of type AMOV_GIMBAL_ZOOM_T, which is an enumeration
- * type representing different zoom levels for the gimbal. It is used to specify the desired zoom level
- * for the gimbal focus.
- * @param targetRate The targetRate parameter is a float value representing the desired zoom rate for
- * the gimbal.
- *
- * @return the result of the pack() function, which is of type uint32_t.
- */
-uint32_t GX40GimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
-{
-    uint8_t temp = 0X01;
-
-    return pack(GX40::GIMBAL_CMD_FOCUE, &temp, 1);
-}
-
-/**
- * The function sets the GNSS information in the carrierGNSS struct and returns the size of the struct.
- *
- * @param lng The "lng" parameter represents the longitude value of the GNSS (Global Navigation
- * Satellite System) information.
- * @param lat The "lat" parameter represents the latitude value of the GNSS (Global Navigation
- * Satellite System) information.
- * @param alt The "alt" parameter represents the altitude value in meters.
- * @param nState The parameter "nState" represents the state of the GNSS (Global Navigation Satellite
- * System) information. It is of type uint32_t, which means it is an unsigned 32-bit integer. The
- * specific values and their meanings for the "nState" parameter are not provided in the code snippet
- * @param relAlt Relative altitude of the carrier (in meters)
- *
- * @return the size of the structure GX40::GIMBAL_SECONDARY_MASTER_FRAME_T.
- */
-uint32_t GX40GimbalDriver::setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt)
-{
-    carrierStateMutex.lock();
-    carrierGNSS.head = 0X01;
-
-    carrierGNSS.lng = lng / 1E-7;
-    carrierGNSS.lat = lat / 1E-7;
-    carrierGNSS.alt = alt / 1E-3;
-
-    carrierGNSS.relAlt = relAlt / 1E-3;
-    carrierGNSS.nState = nState;
-
-    carrierStateMutex.unlock();
-    return sizeof(GX40::GIMBAL_SECONDARY_MASTER_FRAME_T);
-}

gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h

@@ -1,154 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-10-20 16:08:13
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:28:54
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_struct.h
- */
-#ifndef GX40_GIMBAL_STRUCT_H
-#define GX40_GIMBAL_STRUCT_H
-
-#include <stdint.h>
-
-namespace GX40
-{
-#define XF_SEND_HEAD 0XE5A8
-#define XF_RCV_HEAD 0X8A5E
-#define XF_VERSION 0X00
-#define XF_ANGLE_DPI 0.01f
-
-    typedef enum
-    {
-        GIMBAL_FRAME_PARSER_STATE_IDLE,
-        GIMBAL_FRAME_PARSER_STATE_HEAD,
-        GIMBAL_FRAME_PARSER_STATE_LEN1,
-        GIMBAL_FRAME_PARSER_STATE_LEN2,
-        GIMBAL_FRAME_PARSER_STATE_VERSION,
-        GIMBAL_FRAME_PARSER_STATE_PAYLOAD,
-    } GIMBAL_FRAME_PARSER_STATE_T;
-
-    typedef enum
-    {
-        GIMBAL_CMD_NOP = 0X00,
-        GIMBAL_CMD_CAL = 0X01,
-        GIMBAL_CMD_HOME = 0X03,
-        GIMBAL_CMD_MODE_FPV = 0X10,
-        GIMBAL_CMD_MODE_LOCK = 0X11,
-        GIMBAL_CMD_MODE_FOLLOW = 0X12,
-        GIMBAL_CMD_MODE_OVERLOCK = 0X13,
-        GIMBAL_CMD_MODE_EULER = 0X14,
-        GIMBAL_CMD_MODE_WATCH_POS = 0X15,
-        GIMBAL_CMD_MODE_WATCH = 0X16,
-        GIMBAL_CMD_MODE_TRACK = 0X17,
-        GIMBAL_CMD_MODE_MOVE = 0X1A,
-        GIMBAL_CMD_MODE_MOVE_TRACK = 0X1B,
-        GIMBAL_CMD_TAKEPIC = 0X20,
-        GIMBAL_CMD_TAKEVIDEO = 0X21,
-        GIMBAL_CMD_ZOOM_OUT = 0X22,
-        GIMBAL_CMD_ZOMM_IN = 0X23,
-        GIMBAL_CMD_ZOOM_STOP = 0X24,
-        GIMBAL_CMD_ZOOM = 0X25,
-        GIMBAL_CMD_FOCUE = 0X26,
-        GIMBAL_CMD_VIDEO_MODE = 0X2A,
-        GIMBAL_CMD_NIGHT = 0X2B,
-        GIMBAL_CMD_OSD = 0X73,
-        GIMBAL_CMD_FIX_MODE = 0X74,
-        GIMBAL_CMD_LIGHT = 0X80,
-        GIMBAL_CMD_TAKE_DISTANCE = 0X81,
-    } GIMBAL_CMD_T;
-
-#pragma pack(1)
-
-    typedef struct
-    {
-        union
-        {
-            uint8_t u8[2];
-            uint16_t u16;
-        } head;
-        union
-        {
-            uint8_t u8[2];
-            uint16_t u16;
-        } lenght;
-        uint8_t version;
-        uint8_t primaryData[32];
-        uint8_t secondaryData[32];
-        uint8_t otherData[32];
-        union
-        {
-            uint8_t u8[2];
-            uint16_t u16;
-        } crc16;
-    } GIMBAL_FRAME_T;
-
-    typedef struct
-    {
-        int16_t roll;
-        int16_t pitch;
-        int16_t yaw;
-        uint8_t state;
-        int16_t selfRoll;
-        int16_t selfPitch;
-        uint16_t selfYaw;
-        int16_t accN;
-        int16_t accE;
-        int16_t accUp;
-        int16_t speedN;
-        int16_t speedE;
-        int16_t speedUp;
-        uint8_t secondaryFlag;
-        uint8_t reserve[6];
-    } GIMBAL_PRIMARY_MASTER_FRAME_T;
-
-    typedef struct
-    {
-        uint8_t workMode;
-        uint16_t state;
-        int16_t offsetX;
-        int16_t offsetY;
-        uint16_t motorRoll;
-        uint16_t motorPitch;
-        uint16_t motorYaw;
-        int16_t roll;
-        int16_t pitch;
-        uint16_t yaw;
-        int16_t speedRoll;
-        int16_t speedPitch;
-        int16_t speedYaw;
-        uint8_t reserve[7];
-    } GIMBAL_PRIMARY_SLAVE_FRAME_T;
-
-    typedef struct
-    {
-        uint8_t head;
-        int32_t lng;
-        int32_t lat;
-        int32_t alt;
-        uint8_t nState;
-        uint32_t GPSms;
-        int32_t GPSweeks;
-        int32_t relAlt;
-        uint8_t reserve[8];
-    } GIMBAL_SECONDARY_MASTER_FRAME_T;
-
-    typedef struct
-    {
-        uint8_t head;
-        uint8_t versionHW;
-        uint8_t versionSoft;
-        uint8_t type;
-        uint16_t error;
-        int32_t targetDistance;
-        int32_t targetLng;
-        int32_t targetLat;
-        int32_t targetAlt;
-        uint16_t camera1Zoom;
-        uint16_t camera2Zoom;
-        uint8_t reserve[6];
-    } GIMBAL_SECONDARY_SLAVE_FRAME_T;
-#pragma pack()
-}
-
-#endif

gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h

@@ -3,11 +3,12 @@
  * @Author: L LC @amov
  * @Date: 2022-10-27 18:10:06
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:28:29
+ * @LastEditTime: 2023-03-23 17:24:23
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_crc32.h
+ * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_crc32.h
  */
 #ifndef Q10F_GIMBAL_CRC32_H
 #define Q10F_GIMBAL_CRC32_H
+
 namespace Q10f
 {
     static inline unsigned char CheckSum(unsigned char *pData, unsigned short Lenght)

gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp

@@ -3,8 +3,8 @@
  * @Author: L LC @amov
  * @Date: 2022-10-27 18:10:06
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:23:15
+ * @LastEditTime: 2023-04-11 17:29:58
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.cpp
+ * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_driver.cpp
  */
 #include "Q10f_gimbal_driver.h"
 #include "Q10f_gimbal_crc32.h"
@@ -16,10 +16,27 @@
  *
  * @param _IO The IOStreamBase object that will be used to communicate with the gimbal.
  */
-Q10fGimbalDriver::Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::amovGimbalBase(_IO)
+Q10fGimbalDriver::Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(_IO)
 {
-    rxQueue = new fifoRing(sizeof(Q10f::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
+    memset(&rxQueue, 0, sizeof(RING_FIFO_CB_T));
-    txQueue = new fifoRing(sizeof(Q10f::GIMBAL_FRAME_T), MAX_QUEUE_SIZE);
+    memset(&txQueue, 0, sizeof(RING_FIFO_CB_T));
+
+    rxBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T));
+    if (rxBuffer == NULL)
+    {
+        std::cout << "Receive buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
+        exit(1);
+    }
+    txBuffer = (uint8_t *)malloc(MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T));
+    if (txBuffer == NULL)
+    {
+        free(rxBuffer);
+        std::cout << "Send buffer creation failed! Size : " << MAX_QUEUE_SIZE << std::endl;
+        exit(1);
+    }
+
+    Ring_Fifo_init(&rxQueue, sizeof(Q10f::GIMBAL_FRAME_T), rxBuffer, MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T));
+    Ring_Fifo_init(&txQueue, sizeof(Q10f::GIMBAL_FRAME_T), txBuffer, MAX_QUEUE_SIZE * sizeof(Q10f::GIMBAL_FRAME_T));
 
     parserState = Q10f::GIMBAL_SERIAL_STATE_IDLE;
 
@@ -60,14 +77,32 @@ uint32_t Q10fGimbalDriver::pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payl
     }
     txTemp.len = payloadSize;
 
-    if (txQueue->inCell(&txTemp))
+    txMutex.lock();
+    if (Ring_Fifo_in_cell(&txQueue, &txTemp))
     {
         ret = payloadSize + sizeof(uint32_t) + sizeof(uint8_t);
     }
+    txMutex.unlock();
 
     return ret;
 }
 
+/**
+ * > This function is used to get a packet from the receive queue
+ *
+ * @param void This is the type of data that will be stored in the queue.
+ *
+ * @return A boolean value.
+ */
+bool Q10fGimbalDriver::getRxPack(OUT void *pack)
+{
+    bool state = false;
+    rxMutex.lock();
+    state = Ring_Fifo_out_cell(&rxQueue, pack);
+    rxMutex.unlock();
+    return state;
+}
+
 void Q10fGimbalDriver::convert(void *buf)
 {
     Q10f::GIMBAL_FRAME_T *temp;
@@ -81,12 +116,12 @@ void Q10fGimbalDriver::convert(void *buf)
         state.abs.yaw = tempPos->yawIMUAngle * Q10F_SCALE_FACTOR_ANGLE;
         state.abs.roll = tempPos->rollIMUAngle * Q10F_SCALE_FACTOR_ANGLE;
         state.abs.pitch = tempPos->pitchIMUAngle * Q10F_SCALE_FACTOR_ANGLE;
-        state.rel.yaw = tempPos->yawStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
+        state.rel.yaw = tempPos->rollStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
         state.rel.roll = tempPos->rollStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
         state.rel.pitch = tempPos->pitchStatorRotorAngle * Q10F_SCALE_FACTOR_SPEED;
         updateGimbalStateCallback(state.rel.roll, state.rel.pitch, state.rel.yaw,
                                   state.abs.roll, state.abs.pitch, state.abs.yaw,
-                                  state.fov.x, state.fov.y, updataCaller);
+                                  state.fov.x, state.fov.y);
         mState.unlock();
 
         break;
@@ -101,9 +136,32 @@ void Q10fGimbalDriver::convert(void *buf)
     }
 }
 
-uint32_t Q10fGimbalDriver::calPackLen(void *pack)
+/**
+ * The function is called by the main thread to send a command to the gimbal.
+ *
+ * The function first checks to see if the serial port is busy and if it is open. If it is not busy and
+ * it is open, the function locks the txMutex and then checks to see if there is a command in the
+ * txQueue. If there is a command in the txQueue, the function copies the command to the tx buffer and
+ * then unlocks the txMutex. The function then sends the command to the gimbal.
+ *
+ * The txQueue is a ring buffer that holds commands that are waiting to be sent to the gimbal. The
+ * txQueue is a ring buffer because the gimbal can only process one command at a time. If the gimbal is
+ * busy processing a command, the command will be placed in the txQueue and sent to the gimbal when the
+ * gimbal is ready to receive the command.
+ */
+void Q10fGimbalDriver::send(void)
 {
-    return ((Q10f::GIMBAL_FRAME_T *)pack)->len + Q10F_PAYLOAD_OFFSET + sizeof(uint8_t);
+    if (!IO->isBusy() && IO->isOpen())
+    {
+        bool state = false;
+        txMutex.lock();
+        state = Ring_Fifo_out_cell(&txQueue, &tx);
+        txMutex.unlock();
+        if (state)
+        {
+            IO->outPutBytes((uint8_t *)&tx, tx.len + Q10F_PAYLOAD_OFFSET + sizeof(uint8_t));
+        }
+    }
 }
 
 /**
@@ -180,7 +238,9 @@ bool Q10fGimbalDriver::parser(IN uint8_t byte)
         if (byte == suncheck)
         {
             state = true;
-            rxQueue->inCell(&rx);
+            rxMutex.lock();
+            Ring_Fifo_in_cell(&rxQueue, &rx);
+            rxMutex.unlock();
         }
         else
         {

gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h

@@ -3,49 +3,69 @@
  * @Author: L LC @amov
  * @Date: 2022-10-28 12:24:21
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:27:45
+ * @LastEditTime: 2023-03-28 17:01:00
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_driver.h
+ * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_driver.h
  */
-#ifndef __Q10F_DRIVER_H
+#include "../amov_gimbal.h"
-#define __Q10F_DRIVER_H
-
-#include "../amov_gimbal_private.h"
 #include "Q10f_gimbal_struct.h"
 #include <mutex>
 #include <malloc.h>
 #include <iostream>
 
-class Q10fGimbalDriver : protected amovGimbal::amovGimbalBase
+#ifndef __Q10F_DRIVER_H
+#define __Q10F_DRIVER_H
+
+extern "C"
+{
+#include "Ring_Fifo.h"
+}
+
+class Q10fGimbalDriver : protected amovGimbal::IamovGimbalBase
 {
 private:
     Q10f::GIMBAL_SERIAL_STATE_T parserState;
     Q10f::GIMBAL_FRAME_T rx;
+    Q10f::GIMBAL_FRAME_T tx;
+
+    std::mutex rxMutex;
+    uint8_t *rxBuffer;
+    RING_FIFO_CB_T rxQueue;
+    std::mutex txMutex;
+    uint8_t *txBuffer;
+    RING_FIFO_CB_T txQueue;
 
     bool parser(IN uint8_t byte);
+    void send(void);
+
     void convert(void *buf);
     uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize);
-    uint32_t calPackLen(void *pack);
+    bool getRxPack(OUT void *pack);
 
 public:
     // funtions
-    uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
+    uint32_t setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos);
-    uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
+    uint32_t setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed);
-    uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
+    uint32_t setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed);
     uint32_t setGimabalHome(void);
 
-    uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
+    uint32_t setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-    uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
+    uint32_t setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
 
     uint32_t takePic(void);
-    uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
+    uint32_t setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState);
 
     // builds
-    static amovGimbal::amovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
+    static amovGimbal::IamovGimbalBase *creat(amovGimbal::IOStreamBase *_IO)
     {
         return new Q10fGimbalDriver(_IO);
     }
 
     Q10fGimbalDriver(amovGimbal::IOStreamBase *_IO);
+    ~Q10fGimbalDriver()
+    {
+        free(rxBuffer);
+        free(txBuffer);
+    }
 };
 
 #endif

gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp

@@ -3,8 +3,8 @@
  * @Author: L LC @amov
  * @Date: 2023-03-02 10:00:52
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:27:39
+ * @LastEditTime: 2023-03-29 11:47:18
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_funtion.cpp
+ * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_funtion.cpp
  */
 #include "Q10f_gimbal_driver.h"
 #include "Q10f_gimbal_crc32.h"
@@ -17,7 +17,7 @@
  *
  * @return The return value is the number of bytes written to the buffer.
  */
-uint32_t Q10fGimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
+uint32_t Q10fGimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
 {
     Q10f::GIMBAL_SET_POS_MSG_T temp;
     temp.modeR = Q10f::GIMBAL_CMD_POS_MODE_ANGLE_SPEED;
@@ -41,7 +41,7 @@ uint32_t Q10fGimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
  *
  * @return The return value is the number of bytes written to the buffer.
  */
-uint32_t Q10fGimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
+uint32_t Q10fGimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
 {
     Q10f::GIMBAL_SET_POS_MSG_T temp;
     temp.modeR = Q10f::GIMBAL_CMD_POS_MODE_SPEED;
@@ -64,7 +64,7 @@ uint32_t Q10fGimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
  *
  * @return The return value is the number of bytes written to the buffer.
  */
-uint32_t Q10fGimbalDriver::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
+uint32_t Q10fGimbalDriver::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
 {
     state.maxFollow.pitch = followSpeed.pitch / 0.1220740379f;
     state.maxFollow.roll = followSpeed.roll / 0.1220740379f;
@@ -108,25 +108,25 @@ uint32_t Q10fGimbalDriver::takePic(void)
  *
  * @return The return value is the number of bytes written to the serial port.
  */
-uint32_t Q10fGimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
+uint32_t Q10fGimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
 {
     uint8_t cmd[2] = {0X01, 0XFF};
 
-    if (newState == AMOV_GIMBAL_VIDEO_TAKE)
+    if (newState == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
     {
         cmd[0] = 0X02;
-        state.video = AMOV_GIMBAL_VIDEO_TAKE;
+        state.video = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE;
     }
     else
     {
         cmd[0] = 0X03;
-        state.video = AMOV_GIMBAL_VIDEO_OFF;
+        state.video = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
     }
 
     return pack(Q10f::GIMBAL_CMD_CAMERA, (uint8_t *)cmd, sizeof(cmd));
 }
 
-uint32_t Q10fGimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
+uint32_t Q10fGimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
 {
     uint8_t cmd[5] = {0X00, 0X00, 0X00, 0X00, 0XFF};
     if (targetRate == 0.0f)
@@ -134,13 +134,13 @@ uint32_t Q10fGimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRa
         cmd[1] = 0XFF;
         switch (zoom)
         {
-        case AMOV_GIMBAL_ZOOM_IN:
+        case amovGimbal::AMOV_GIMBAL_ZOOM_IN:
             cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN;
             break;
-        case AMOV_GIMBAL_ZOOM_OUT:
+        case amovGimbal::AMOV_GIMBAL_ZOOM_OUT:
             cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT;
             break;
-        case AMOV_GIMBAL_ZOOM_STOP:
+        case amovGimbal::AMOV_GIMBAL_ZOOM_STOP:
             cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP;
             break;
         default:
@@ -159,18 +159,18 @@ uint32_t Q10fGimbalDriver::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRa
     }
 }
 
-uint32_t Q10fGimbalDriver::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
+uint32_t Q10fGimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
 {
     uint8_t cmd[2] = {0X00, 0XFF};
     switch (zoom)
     {
-    case AMOV_GIMBAL_ZOOM_IN:
+    case amovGimbal::AMOV_GIMBAL_ZOOM_IN:
         cmd[0] = Q10f::GIMBAL_CMD_ZOOM_IN;
         break;
-    case AMOV_GIMBAL_ZOOM_OUT:
+    case amovGimbal::AMOV_GIMBAL_ZOOM_OUT:
         cmd[0] = Q10f::GIMBAL_CMD_ZOOM_OUT;
         break;
-    case AMOV_GIMBAL_ZOOM_STOP:
+    case amovGimbal::AMOV_GIMBAL_ZOOM_STOP:
         cmd[0] = Q10f::GIMBAL_CMD_ZOOM_STOP;
         break;
     default:

gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h

@@ -3,8 +3,8 @@
  * @Author: L LC @amov
  * @Date: 2022-10-27 18:10:07
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:27:27
+ * @LastEditTime: 2023-03-28 18:15:47
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/Q10f/Q10f_gimbal_struct.h
+ * @FilePath: /gimbal-sdk-multi-platform/src/Q10f/Q10f_gimbal_struct.h
  */
 #ifndef Q10F_GIMBAL_STRUCT_H
 #define Q10F_GIMBAL_STRUCT_H

gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h

@@ -1,111 +0,0 @@
-/*
- * @Description:    External interface of amov gimbals
- * @Author: L LC @amov
- * @Date: 2022-10-27 18:34:26
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:37:09
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h
- */
-
-#ifndef AMOV_GIMBAL_H
-#define AMOV_GIMBAL_H
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <iostream>
-
-#include "amov_gimbal_struct.h"
-
-namespace amovGimbal
-{
-#define IN
-#define OUT
-#define SET
-
-#ifndef MAX_QUEUE_SIZE
-#define MAX_QUEUE_SIZE 100
-#endif
-
-    static inline void idleCallback(double frameAngleRoll, double frameAnglePitch, double frameAngleYaw,
-                                    double imuAngleRoll, double imuAnglePitch, double imuAngleYaw,
-                                    double fovX, double fovY, void *caller)
-    {
-    }
-    static inline void idleMsgCallback(void *msg, void *caller)
-    {
-    }
-
-    // Control data input and output
-    class IOStreamBase
-    {
-    public:
-        IOStreamBase() {}
-        virtual ~IOStreamBase() {}
-
-        virtual bool open() = 0;
-        virtual bool close() = 0;
-        virtual bool isOpen() = 0;
-        virtual bool isBusy() = 0;
-        // These two functions need to be thread-safe
-        virtual uint32_t inPutBytes(IN uint8_t *byte) = 0;
-        virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) = 0;
-    };
-
-    class gimbal
-    {
-    private:
-        std::string typeName;
-        // Instantiated device handle
-        void *devHandle;
-
-    public:
-        static void inBytesCallback(uint8_t *pData, uint32_t len, gimbal *handle);
-        // Protocol stack function items
-        void startStack(void);
-        void parserAuto(pAmovGimbalStateInvoke callback = idleCallback, void *caller = nullptr);
-        void setParserCallback(pAmovGimbalStateInvoke callback, void *caller = nullptr);
-        void setMsgCallback(pAmovGimbalMsgInvoke callback, void *caller = nullptr);
-        void setRcvBytes(pAmovGimbalInputBytesInvoke callbaclk, void *caller = nullptr);
-        void setSendBytes(pAmovGimbalOutputBytesInvoke callbaclk, void *caller = nullptr);
-        AMOV_GIMBAL_STATE_T getGimabalState(void);
-
-        // non-block functions
-        uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
-        uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
-        uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
-
-        uint32_t setGimabalHome(void);
-        uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-        uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-        uint32_t setGimbalROI(const AMOV_GIMBAL_ROI_T area);
-        uint32_t takePic(void);
-        uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
-        uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
-                                    const AMOV_GIMBAL_VELOCITY_T &speed,
-                                    const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
-        uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
-                                    const AMOV_GIMBAL_VELOCITY_T &seppd,
-                                    const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
-        uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
-        uint32_t extensionFuntions(void *cmd);
-
-        // block functions
-        bool setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos);
-        bool setGimabalHomeBlock(void);
-        bool setGimbalZoomBlock(float targetRate);
-        bool takePicBlock(void);
-        bool calibrationBlock(void);
-
-        std::string name()
-        {
-            return typeName;
-        }
-
-        gimbal(const std::string &type, IOStreamBase *_IO,
-               uint32_t _self = 0x02, uint32_t _remote = 0X80);
-
-        ~gimbal();
-    };
-
-}
-#endif

gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_c.h

@@ -1,55 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-11-24 16:01:22
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-06 11:35:58
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_c.h
- */
-#ifndef AMOV_GIMBAL_C_H
-#define AMOV_GIMBAL_C_H
-
-#include <stdint.h>
-#include "amov_gimbal_struct.h"
-
-extern "C"
-{
-    // initialization funtions
-    void amovGimbalCreat(char *type, uint32_t selfId, uint32_t gimbalId, void *handle);
-    void amovGimbalInBytesCallback(uint8_t *pData, uint32_t len, void *handle);
-    void amovGimbalSetRcvBytes(pAmovGimbalInputBytesInvoke callbaclk, void *handle, void *caller);
-    void amovGimbalSetSendBytes(pAmovGimbalOutputBytesInvoke callbaclk, void *handle, void *caller);
-    void amovGimbalChangeStateCallback(pAmovGimbalStateInvoke callback, void *handle, void *caller);
-    void amovGimbalSetMsgCallback(pAmovGimbalMsgInvoke callback, void *handle, void *caller);
-    void amovGimbalStart(pAmovGimbalStateInvoke callback, void *handle, void *caller);
-
-    // non-block functions
-    uint32_t amovGimbalSetGimabalPos(AMOV_GIMBAL_POS_T *pos, void *handle);
-    uint32_t amovGimbalSetGimabalSpeed(AMOV_GIMBAL_POS_T *speed, void *handle);
-    uint32_t amovGimbalSetGimabalFollowSpeed(AMOV_GIMBAL_POS_T *followSpeed, void *handle);
-    uint32_t amovGimbalSetGimabalHome(void *handle);
-    uint32_t amovGimbalSetGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate, void *handle);
-    uint32_t amovGimbalSetGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate, void *handle);
-    uint32_t amovGimbalSetGimbalROI(AMOV_GIMBAL_ROI_T *area, void *handle);
-    uint32_t amovGimbalTakePic(void *handle);
-    uint32_t amovGimbalSetVideo(AMOV_GIMBAL_VIDEO_T newState, void *handle);
-    uint32_t amovGimbalAttitudeCorrectionQ(AMOV_GIMBAL_QUATERNION_T *quaterion,
-                                           AMOV_GIMBAL_VELOCITY_T *speed,
-                                           AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle);
-    uint32_t amovGimbalAttitudeCorrectionE(AMOV_GIMBAL_POS_T *pos,
-                                           AMOV_GIMBAL_VELOCITY_T *speed,
-                                           AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle);
-    uint32_t amovGimbalSetGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt, void *handle);
-    uint32_t amovGimbalExtensionFuntions(void *cmd, void *handle);
-    void getGimabalState(AMOV_GIMBAL_STATE_T *state, void *handle);
-    void getGimbalType(char *type, void *handle);
-
-    // block functions
-    bool amovGimbalSetGimbalPosBlock(AMOV_GIMBAL_POS_T *pos, void *handle);
-    bool amovGimbalSetGimabalHomeBlock(void *handle);
-    bool amovGimbalSetGimbalZoomBlock(float targetRate, void *handle);
-    bool amovGimbalTakePicBlock(void *handle);
-    bool amovGimbalCalibrationBlock(void *handle);
-}
-
-#endif

gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_struct.h

@@ -1,102 +0,0 @@
-/*
- * @Description: Common Data Structures of gimbal
- * @Author: L LC @amov
- * @Date: 2022-10-31 11:56:43
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:03:02
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amovGimbal/amov_gimbal_struct.h
- */
-
-#include <stdint.h>
-
-#ifndef __AMOV_GIMABL_STRUCT_H
-#define __AMOV_GIMABL_STRUCT_H
-
-typedef void (*pAmovGimbalStateInvoke)(double frameAngleRoll, double frameAnglePitch, double frameAngleYaw,
-                                       double imuAngleRoll, double imuAnglePitch, double imuAngleYaw,
-                                       double fovX, double fovY, void *caller);
-typedef void (*pAmovGimbalMsgInvoke)(void *msg, void *caller);
-typedef uint32_t (*pAmovGimbalInputBytesInvoke)(uint8_t *pData, void *caller);
-typedef uint32_t (*pAmovGimbalOutputBytesInvoke)(uint8_t *pData, uint32_t len, void *caller);
-
-typedef enum
-{
-    AMOV_GIMBAL_SERVO_MODE_FPV = 0X10,
-    AMOV_GIMBAL_SERVO_MODE_LOCK = 0X11,
-    AMOV_GIMBAL_SERVO_MODE_FOLLOW = 0X12,
-    AMOV_GIMBAL_SERVO_MODE_OVERLOOK = 0X13,
-    AMOV_GIMBAL_SERVO_MODE_EULER = 0X14,
-    AMOV_GIMBAL_SERVO_MODE_WATCH = 0X16,
-    AMOV_GIMBAL_SERVO_MODE_TRACK = 0X17,
-} AMOV_GIMBAL_SERVO_MODE_T;
-
-typedef enum
-{
-    AMOV_GIMBAL_CAMERA_FLAG_INVERSION = 0X1000,
-    AMOV_GIMBAL_CAMERA_FLAG_IR = 0X0200,
-    AMOV_GIMBAL_CAMERA_FLAG_RF = 0X0100,
-    AMOV_GIMBAL_CAMERA_FLAG_LOCK = 0X0001,
-} AMOV_GIMBAL_CAMERA_FLAG_T;
-
-typedef enum
-{
-    AMOV_GIMBAL_VIDEO_TAKE,
-    AMOV_GIMBAL_VIDEO_OFF
-} AMOV_GIMBAL_VIDEO_T;
-
-typedef enum
-{
-    AMOV_GIMBAL_ZOOM_IN,
-    AMOV_GIMBAL_ZOOM_OUT,
-    AMOV_GIMBAL_ZOOM_STOP
-} AMOV_GIMBAL_ZOOM_T;
-
-typedef struct
-{
-    double yaw;
-    double roll;
-    double pitch;
-} AMOV_GIMBAL_POS_T;
-
-typedef struct
-{
-    double x;
-    double y;
-} AMOV_GIMBAL_FOV_T;
-
-typedef struct
-{
-    AMOV_GIMBAL_SERVO_MODE_T workMode;
-    AMOV_GIMBAL_CAMERA_FLAG_T cameraFlag;
-    AMOV_GIMBAL_VIDEO_T video;
-    AMOV_GIMBAL_POS_T abs;
-    AMOV_GIMBAL_POS_T rel;
-    AMOV_GIMBAL_POS_T relSpeed;
-    AMOV_GIMBAL_POS_T maxFollow;
-    AMOV_GIMBAL_FOV_T fov;
-} AMOV_GIMBAL_STATE_T;
-
-typedef struct
-{
-    uint32_t centreX;
-    uint32_t centreY;
-    uint32_t hight;
-    uint32_t width;
-} AMOV_GIMBAL_ROI_T;
-
-typedef struct
-{
-    double q0;
-    double q1;
-    double q2;
-    double q3;
-} AMOV_GIMBAL_QUATERNION_T;
-
-typedef struct
-{
-    double x; // or N
-    double y; // or E
-    double z; // or UP
-} AMOV_GIMBAL_VELOCITY_T;
-
-#endif

gimbal_ctrl/driver/src/amov_gimabl.cpp

@@ -0,0 +1,239 @@
+/*
+ * @Description:
+ * @Author: L LC @amov
+ * @Date: 2022-10-28 11:54:11
+ * @LastEditors: L LC @amov
+ * @LastEditTime: 2023-04-11 18:13:25
+ * @FilePath: /gimbal-sdk-multi-platform/src/amov_gimabl.cpp
+ */
+
+#include "amov_gimbal.h"
+#include "g1_gimbal_driver.h"
+#include "g2_gimbal_driver.h"
+#include "Q10f_gimbal_driver.h"
+
+#include <iostream>
+#include <thread>
+#include <map>
+#include <iterator>
+
+#define MAX_PACK_SIZE 280
+typedef enum
+{
+    AMOV_GIMBAL_TYPE_NULL,
+    AMOV_GIMBAL_TYPE_G1 = 1,
+    AMOV_GIMBAL_TYPE_G2,
+    AMOV_GIMBAL_TYPE_Q10,
+} AMOV_GIMBAL_TYPE_T;
+
+namespace amovGimbal
+{
+    typedef amovGimbal::IamovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO);
+    typedef std::map<std::string, createCallback> callbackMap;
+    std::map<std::string, AMOV_GIMBAL_TYPE_T> amovGimbalTypeList =
+        {
+            {"G1", AMOV_GIMBAL_TYPE_G1},
+            {"G2", AMOV_GIMBAL_TYPE_G2},
+            {"Q10f", AMOV_GIMBAL_TYPE_Q10}};
+
+    callbackMap amovGimbals =
+        {
+            {"G1", g1GimbalDriver::creat},
+            {"G2", g2GimbalDriver::creat},
+            {"Q10f", Q10fGimbalDriver::creat}};
+}
+
+/* The amovGimbalCreator class is a factory class that creates an instance of the amovGimbal class */
+// Factory used to create the gimbal instance
+class amovGimbalCreator
+{
+public:
+    static amovGimbal::IamovGimbalBase *createAmovGimbal(const std::string &type, amovGimbal::IOStreamBase *_IO)
+    {
+        amovGimbal::callbackMap::iterator temp = amovGimbal::amovGimbals.find(type);
+
+        if (temp != amovGimbal::amovGimbals.end())
+        {
+            return (temp->second)(_IO);
+        }
+        std::cout << type << " is Unsupported device type!" << std::endl;
+        return NULL;
+    }
+
+private:
+    amovGimbalCreator()
+    {
+    }
+    static amovGimbalCreator *pInstance;
+    static amovGimbalCreator *getInstance()
+    {
+        if (pInstance == NULL)
+        {
+            pInstance = new amovGimbalCreator();
+        }
+        return pInstance;
+    }
+
+    ~amovGimbalCreator();
+};
+
+/**
+ * "If the input byte is available, then parse it."
+ *
+ * The function is a loop that runs forever. It calls the IO->inPutByte() function to get a byte from
+ * the serial port. If the byte is available, then it calls the parser() function to parse the byte
+ */
+void amovGimbal::IamovGimbalBase::parserLoop(void)
+{
+    uint8_t temp;
+
+    while (1)
+    {
+        if (IO->inPutByte(&temp))
+        {
+            parser(temp);
+        }
+    }
+}
+
+void amovGimbal::IamovGimbalBase::sendLoop(void)
+{
+    while (1)
+    {
+        send();
+    }
+}
+
+void amovGimbal::IamovGimbalBase::mainLoop(void)
+{
+    uint8_t tempBuffer[MAX_PACK_SIZE];
+
+    while (1)
+    {
+        if (getRxPack(tempBuffer))
+        {
+            convert(tempBuffer);
+        }
+    }
+}
+
+/**
+ * It starts two threads, one for reading data from the serial port and one for sending data to the
+ * serial port
+ */
+void amovGimbal::IamovGimbalBase::startStack(void)
+{
+    if (!IO->isOpen())
+    {
+        IO->open();
+    }
+
+    std::thread mainLoop(&IamovGimbalBase::parserLoop, this);
+    std::thread sendLoop(&IamovGimbalBase::sendLoop, this);
+    mainLoop.detach();
+    sendLoop.detach();
+}
+
+/**
+ * The function creates a thread that runs the mainLoop function
+ */
+void amovGimbal::IamovGimbalBase::parserAuto(pStateInvoke callback)
+{
+    this->updateGimbalStateCallback = callback;
+    std::thread mainLoop(&IamovGimbalBase::mainLoop, this);
+    mainLoop.detach();
+}
+
+amovGimbal::AMOV_GIMBAL_STATE_T amovGimbal::IamovGimbalBase::getGimabalState(void)
+{
+    mState.lock();
+    AMOV_GIMBAL_STATE_T temp = state;
+    mState.unlock();
+    return temp;
+}
+
+amovGimbal::IamovGimbalBase::~IamovGimbalBase()
+{
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    IO->close();
+}
+
+/**
+ * Default implementation of interface functions, not pure virtual functions for ease of extension.
+ */
+void amovGimbal::IamovGimbalBase::nodeSet(SET uint32_t _self, SET uint32_t _remote)
+{
+    return;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos)
+{
+    return 0;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed)
+{
+    return 0;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::setGimabalFollowSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &followSpeed)
+{
+    return 0;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::setGimabalHome(void)
+{
+    return 0;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
+{
+    return 0;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
+{
+    return 0;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::setGimbalROI(const amovGimbal::AMOV_GIMBAL_ROI_T area)
+{
+    return 0;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::takePic(void)
+{
+    return 0;
+}
+
+uint32_t amovGimbal::IamovGimbalBase::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState)
+{
+    return 0;
+}
+
+/**
+ * The function creates a new gimbal object, which is a pointer to a new amovGimbal object, which is a
+ * pointer to a new Gimbal object, which is a pointer to a new IOStreamBase object
+ *
+ * @param type the type of the device, which is the same as the name of the class
+ * @param _IO The IOStreamBase object that is used to communicate with the device.
+ * @param _self the node ID of the device
+ * @param _remote the node ID of the remote device
+ */
+amovGimbal::gimbal::gimbal(const std::string &type, IOStreamBase *_IO,
+                           uint32_t _self, uint32_t _remote)
+{
+    typeName = type;
+    IO = _IO;
+
+    dev = amovGimbalCreator::createAmovGimbal(typeName, IO);
+
+    dev->nodeSet(_self, _remote);
+}
+
+amovGimbal::gimbal::~gimbal()
+{
+    // 先干掉请求线程
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    delete dev;
+}

gimbal_ctrl/driver/src/amov_gimbal.h

@@ -0,0 +1,118 @@
+/*
+ * @Description:    External interface of amov gimbals
+ * @Author: L LC @amov
+ * @Date: 2022-10-27 18:34:26
+ * @LastEditors: L LC @amov
+ * @LastEditTime: 2023-04-18 11:42:05
+ * @FilePath: /spirecv-gimbal-sdk/gimbal_ctrl/driver/src/amov_gimbal.h
+ */
+
+#ifndef AMOV_GIMBAL_H
+#define AMOV_GIMBAL_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <iostream>
+
+#include <thread>
+#include <unistd.h>
+#include <mutex>
+
+#include "amov_gimbal_struct.h"
+
+#define MAX_QUEUE_SIZE 50
+
+namespace amovGimbal
+{
+#define IN
+#define OUT
+#define SET
+
+    static inline void idleCallback(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
+                                    double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
+                                    double &fovX, double &fovY)
+    {
+    }
+
+    // Control data input and output
+    class IOStreamBase
+    {
+    public:
+        IOStreamBase() {}
+        virtual ~IOStreamBase() {}
+
+        virtual bool open() = 0;
+        virtual bool close() = 0;
+        virtual bool isOpen() = 0;
+        virtual bool isBusy() = 0;
+        // These two functions need to be thread-safe
+        virtual bool inPutByte(IN uint8_t *byte) = 0;
+        virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght) = 0;
+    };
+
+    class IamovGimbalBase
+    {
+    protected:
+        AMOV_GIMBAL_STATE_T state;
+        std::mutex mState;
+        IOStreamBase *IO;
+        pStateInvoke updateGimbalStateCallback;
+
+        virtual bool parser(IN uint8_t byte) = 0;
+        virtual void send(void) = 0;
+        virtual void convert(void *buf) = 0;
+        virtual uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize) = 0;
+        virtual bool getRxPack(OUT void *pack) = 0;
+
+        void parserLoop(void);
+        void sendLoop(void);
+        void mainLoop(void);
+
+    public:
+        IamovGimbalBase(SET IOStreamBase *_IO)
+        {
+            IO = _IO;
+        }
+        virtual ~IamovGimbalBase();
+
+        void setParserCallback(pStateInvoke callback)
+        {
+            this->updateGimbalStateCallback = callback;
+        }
+
+        // Protocol stack function items
+        virtual void startStack(void);
+        virtual void parserAuto(pStateInvoke callback = idleCallback);
+        virtual void nodeSet(SET uint32_t _self, SET uint32_t _remote);
+
+        // functions
+        virtual AMOV_GIMBAL_STATE_T getGimabalState(void);
+        virtual uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
+        virtual uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
+        virtual uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
+        virtual uint32_t setGimabalHome(void);
+        virtual uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
+        virtual uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
+        virtual uint32_t setGimbalROI(const AMOV_GIMBAL_ROI_T area);
+        virtual uint32_t takePic(void);
+        virtual uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
+    };
+
+    class gimbal
+    {
+    private:
+        std::string typeName;
+        IOStreamBase *IO;
+
+    public:
+        IamovGimbalBase *dev;
+        std::string name()
+        {
+            return typeName;
+        }
+        gimbal(const std::string &type, IOStreamBase *_IO,
+               uint32_t _self = 0x02, uint32_t _remote = 0X80);
+        ~gimbal();
+    };
+}
+#endif

gimbal_ctrl/driver/src/amov_gimbal_factory.cpp

@@ -1,90 +0,0 @@
-/*
- * @Description: 
- * @Author: L LC @amov
- * @Date: 2023-11-24 15:48:47
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:27:10
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_factory.cpp
- */
-
-#include "amov_gimbal_private.h"
-#include "g1_gimbal_driver.h"
-#include "Q10f_gimbal_driver.h"
-#include "AT10_gimbal_driver.h"
-#include "GX40_gimbal_driver.h"
-
-#include <map>
-#include <iterator>
-
-namespace amovGimbalFactory
-{
-    typedef amovGimbal::amovGimbalBase *(*createCallback)(amovGimbal::IOStreamBase *_IO);
-    typedef std::map<std::string, createCallback> callbackMap;
-
-    callbackMap amovGimbals =
-        {
-            {"G1", g1GimbalDriver::creat},
-            {"Q10f", Q10fGimbalDriver::creat},
-            {"AT10", AT10GimbalDriver::creat},
-            {"GX40", GX40GimbalDriver::creat}};
-
-    /* The amovGimbalCreator class is a factory class that creates an instance of the amovGimbal class */
-    // Factory used to create the gimbal instance
-    class amovGimbalCreator
-    {
-    public:
-        static amovGimbal::amovGimbalBase *createAmovGimbal(const std::string &type, amovGimbal::IOStreamBase *_IO)
-        {
-            callbackMap::iterator temp = amovGimbals.find(type);
-
-            if (temp != amovGimbals.end())
-            {
-                return (temp->second)(_IO);
-            }
-            std::cout << type << " is Unsupported device type!" << std::endl;
-            return NULL;
-        }
-
-    private:
-        amovGimbalCreator()
-        {
-        }
-        static amovGimbalCreator *pInstance;
-        static amovGimbalCreator *getInstance()
-        {
-            if (pInstance == NULL)
-            {
-                pInstance = new amovGimbalCreator();
-            }
-            return pInstance;
-        }
-
-        ~amovGimbalCreator();
-    };
-} // namespace amovGimbalFactory
-
-/**
- * The function creates a new gimbal object, which is a pointer to a new amovGimbal object, which is a
- * pointer to a new Gimbal object, which is a pointer to a new IOStreamBase object
- *
- * @param type the type of the device, which is the same as the name of the class
- * @param _IO The IOStreamBase object that is used to communicate with the device.
- * @param _self the node ID of the device
- * @param _remote the node ID of the remote device
- */
-amovGimbal::gimbal::gimbal(const std::string &type, IOStreamBase *_IO,
-                           uint32_t _self, uint32_t _remote)
-{
-    typeName = type;
-
-    devHandle = amovGimbalFactory::amovGimbalCreator::createAmovGimbal(typeName, _IO);
-
-    ((amovGimbalBase *)(devHandle))->nodeSet(_self, _remote);
-}
-
-amovGimbal::gimbal::~gimbal()
-{
-    // 先干掉请求线程
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
-    delete ((amovGimbalBase *)(devHandle));
-}

gimbal_ctrl/driver/src/amov_gimbal_interface.cpp

@@ -1,229 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-11-24 16:00:28
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:18:34
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_interface.cpp
- */
-#include "amov_gimbal_private.h"
-
-// must realize
-void amovGimbal::gimbal::startStack(void)
-{
-    ((amovGimbalBase *)(this->devHandle))->stackStart();
-}
-
-void amovGimbal::gimbal::parserAuto(pAmovGimbalStateInvoke callback, void *caller)
-{
-    ((amovGimbalBase *)(this->devHandle))->parserStart(callback, caller);
-}
-
-void amovGimbal::gimbal::setParserCallback(pAmovGimbalStateInvoke callback, void *caller)
-{
-    ((amovGimbalBase *)(this->devHandle))->updateGimbalStateCallback = callback;
-    ((amovGimbalBase *)(this->devHandle))->updataCaller = caller;
-}
-
-void amovGimbal::gimbal::setMsgCallback(pAmovGimbalMsgInvoke callback, void *caller)
-{
-    ((amovGimbalBase *)(this->devHandle))->msgCustomCallback = callback;
-    ((amovGimbalBase *)(this->devHandle))->msgCaller = caller;
-}
-
-AMOV_GIMBAL_STATE_T amovGimbal::gimbal::getGimabalState(void)
-{
-    ((amovGimbalBase *)(this->devHandle))->mState.lock();
-    AMOV_GIMBAL_STATE_T temp = ((amovGimbalBase *)(this->devHandle))->state;
-    ((amovGimbalBase *)(this->devHandle))->mState.unlock();
-    return temp;
-}
-
-// gimbal funtions maybe realize
-uint32_t amovGimbal::gimbal::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimabalPos(pos);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimabalSpeed(speed);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimabalFollowSpeed(followSpeed);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::setGimabalHome(void)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimabalHome();
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setGimabalHome(void)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimbalZoom(zoom, targetRate);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimbalFocus(zoom, targetRate);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::setGimbalROI(const AMOV_GIMBAL_ROI_T area)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimbalROI(area);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setGimbalROI(const AMOV_GIMBAL_ROI_T area)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::takePic(void)
-{
-    return ((amovGimbalBase *)(this->devHandle))->takePic();
-}
-
-uint32_t amovGimbal::IamovGimbalBase::takePic(void)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setVideo(newState);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
-                                                const AMOV_GIMBAL_VELOCITY_T &speed,
-                                                const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
-{
-    return ((amovGimbalBase *)(this->devHandle))->attitudeCorrection(quaterion, speed, acc, extenData);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
-                                                         const AMOV_GIMBAL_VELOCITY_T &speed,
-                                                         const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
-                                                const AMOV_GIMBAL_VELOCITY_T &speed,
-                                                const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
-{
-    return ((amovGimbalBase *)(this->devHandle))->attitudeCorrection(pos, speed, acc, extenData);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
-                                                         const AMOV_GIMBAL_VELOCITY_T &speed,
-                                                         const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGNSSInfo(lng, lat, alt, nState, relAlt);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt)
-{
-    return 0;
-}
-
-uint32_t amovGimbal::gimbal::extensionFuntions(void *cmd)
-{
-    return ((amovGimbalBase *)(this->devHandle))->extensionFuntions(cmd);
-}
-
-uint32_t amovGimbal::IamovGimbalBase::extensionFuntions(void *cmd)
-{
-    return 0;
-}
-
-bool amovGimbal::gimbal::setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimbalPosBlock(pos);
-}
-
-bool amovGimbal::IamovGimbalBase::setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos)
-{
-    return false;
-}
-
-bool amovGimbal::gimbal::setGimabalHomeBlock(void)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimabalHomeBlock();
-}
-
-bool amovGimbal::IamovGimbalBase::setGimabalHomeBlock(void)
-{
-    return false;
-}
-
-bool amovGimbal::gimbal::setGimbalZoomBlock(float targetRate)
-{
-    return ((amovGimbalBase *)(this->devHandle))->setGimbalZoomBlock(targetRate);
-}
-
-bool amovGimbal::IamovGimbalBase::setGimbalZoomBlock(float targetRate)
-{
-    return false;
-}
-
-bool amovGimbal::gimbal::takePicBlock(void)
-{
-    return ((amovGimbalBase *)(this->devHandle))->takePicBlock();
-}
-
-bool amovGimbal::IamovGimbalBase::takePicBlock(void)
-{
-    return false;
-}
-
-bool amovGimbal::gimbal::calibrationBlock(void)
-{
-    return ((amovGimbalBase *)(this->devHandle))->calibrationBlock();
-}
-
-bool amovGimbal::IamovGimbalBase::calibrationBlock(void)
-{
-    return false;
-}

gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp

@@ -1,152 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-11-27 12:28:32
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-06 11:36:30
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp
- */
-#include "amov_gimbal_private.h"
-#include <string>
-
-void amovGimbalSetRcvBytes(pAmovGimbalInputBytesInvoke callbaclk, void *handle, void *caller)
-{
-    ((amovGimbal::gimbal *)handle)->setRcvBytes(callbaclk, caller);
-}
-
-void amovGimbalSetSendBytes(pAmovGimbalOutputBytesInvoke callbaclk, void *handle, void *caller)
-{
-    ((amovGimbal::gimbal *)handle)->setSendBytes(callbaclk, caller);
-}
-
-void amovGimbalInBytesCallback(uint8_t *pData, uint32_t len, void *handle)
-{
-    amovGimbal::gimbal::inBytesCallback(pData, len, (amovGimbal::gimbal *)handle);
-}
-
-void amovGimbalCreat(char *type, uint32_t selfId, uint32_t gimbalId, void *handle)
-{
-    std::string strType = type;
-    handle = new amovGimbal::gimbal(strType, nullptr, selfId, gimbalId);
-}
-
-void amovGimbalStart(pAmovGimbalStateInvoke callback, void *handle, void *caller)
-{
-    ((amovGimbal::gimbal *)handle)->startStack();
-    ((amovGimbal::gimbal *)handle)->parserAuto(callback, caller);
-}
-
-void amovGimbalChangeStateCallback(pAmovGimbalStateInvoke callback, void *handle, void *caller)
-{
-    ((amovGimbal::gimbal *)handle)->setParserCallback(callback, caller);
-}
-
-void amovGimbalSetMsgCallback(pAmovGimbalMsgInvoke callback, void *handle, void *caller)
-{
-    ((amovGimbal::gimbal *)handle)->setMsgCallback(callback, caller);
-}
-
-uint32_t amovGimbalSetGimabalPos(AMOV_GIMBAL_POS_T *pos, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimabalPos(*pos);
-}
-
-uint32_t amovGimbalSetGimabalSpeed(AMOV_GIMBAL_POS_T *speed, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimabalSpeed(*speed);
-}
-
-uint32_t amovGimbalSetGimabalFollowSpeed(AMOV_GIMBAL_POS_T *followSpeed, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimabalFollowSpeed(*followSpeed);
-}
-
-uint32_t amovGimbalSetGimabalHome(void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimabalHome();
-}
-
-uint32_t amovGimbalSetGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimbalZoom(zoom, targetRate);
-}
-
-uint32_t amovGimbalSetGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimbalFocus(zoom, targetRate);
-}
-
-uint32_t amovGimbalSetGimbalROI(AMOV_GIMBAL_ROI_T *area, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimbalROI(*area);
-}
-
-uint32_t amovGimbalTakePic(void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->takePic();
-}
-
-uint32_t amovGimbalSetVideo(AMOV_GIMBAL_VIDEO_T newState, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setVideo(newState);
-}
-
-uint32_t amovGimbalAttitudeCorrectionQ(AMOV_GIMBAL_QUATERNION_T *quaterion,
-                                       AMOV_GIMBAL_VELOCITY_T *speed,
-                                       AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->attitudeCorrection(*quaterion, *speed, *acc, extenData);
-}
-
-uint32_t amovGimbalAttitudeCorrectionE(AMOV_GIMBAL_POS_T *pos,
-                                       AMOV_GIMBAL_VELOCITY_T *speed,
-                                       AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->attitudeCorrection(*pos, *speed, *acc, extenData);
-}
-
-uint32_t amovGimbalSetGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGNSSInfo(lng, lat, alt, nState, relAlt);
-}
-
-uint32_t amovGimbalExtensionFuntions(void *cmd, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->extensionFuntions(cmd);
-}
-
-void getGimabalState(AMOV_GIMBAL_STATE_T *state, void *handle)
-{
-    *state = ((amovGimbal::gimbal *)handle)->getGimabalState();
-}
-
-void getGimbalType(char *type, void *handle)
-{
-    std::string temp = ((amovGimbal::gimbal *)handle)->name();
-    temp.copy(type, temp.size(), 0);
-}
-
-bool amovGimbalSetGimbalPosBlock(AMOV_GIMBAL_POS_T *pos, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimbalPosBlock(*pos);
-}
-
-bool amovGimbalSetGimabalHomeBlock(void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimabalHomeBlock();
-}
-
-bool amovGimbalSetGimbalZoomBlock(float targetRate, void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->setGimbalZoomBlock(targetRate);
-}
-
-bool amovGimbalTakePicBlock(void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->takePicBlock();
-}
-
-bool amovGimbalCalibrationBlock(void *handle)
-{
-    return ((amovGimbal::gimbal *)handle)->calibrationBlock();
-}

gimbal_ctrl/driver/src/amov_gimbal_private.h

@@ -1,131 +0,0 @@
-/*
- * @Description    :
- * @Author         : Aiyangsky
- * @Date           : 2023-05-13 10:39:20
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:18:06
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_private.h
- */
-#ifndef __AMOV_GIMABL_PRIVATE_H
-#define __AMOV_GIMABL_PRIVATE_H
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <iostream>
-#include <thread>
-#include <unistd.h>
-#include <mutex>
-
-#include "amovGimbal/amov_gimbal.h"
-#include "amovGimbal/amov_gimbal_c.h"
-
-#include "Ring_Fifo.h"
-#include "amov_tool.h"
-namespace amovGimbal
-{
-    class PamovGimbalBase
-    {
-    public:
-        AMOV_GIMBAL_STATE_T state;
-        std::mutex mState;
-
-        // IO类
-        IOStreamBase *IO = nullptr;
-        // 适用于C的函数指针
-        void *inBytesCaller = nullptr;
-        pAmovGimbalInputBytesInvoke inBytes = nullptr;
-        void *outBytesCaller = nullptr;
-        pAmovGimbalOutputBytesInvoke outBytes = nullptr;
-
-        void *updataCaller = nullptr;
-        pAmovGimbalStateInvoke updateGimbalStateCallback;
-        void *msgCaller = nullptr;
-        pAmovGimbalMsgInvoke msgCustomCallback = idleMsgCallback;
-
-        fifoRing *rxQueue;
-        fifoRing *txQueue;
-
-        std::thread::native_handle_type parserThreadHanle = 0;
-        std::thread::native_handle_type sendThreadHanle = 0;
-        std::thread::native_handle_type stackThreadHanle = 0;
-
-        PamovGimbalBase(IOStreamBase *_IO)
-        {
-            IO = _IO;
-        }
-        virtual ~PamovGimbalBase()
-        {
-            if (txQueue != nullptr)
-            {
-                delete txQueue;
-            }
-            if (rxQueue != nullptr)
-            {
-                delete rxQueue;
-            }
-            // set thread kill anytime
-            pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
-            parserThreadHanle = parserThreadHanle == 0 ? 0 : pthread_cancel(parserThreadHanle);
-            sendThreadHanle = sendThreadHanle == 0 ? 0 : pthread_cancel(sendThreadHanle);
-            stackThreadHanle = stackThreadHanle == 0 ? 0 : pthread_cancel(stackThreadHanle);
-        }
-    };
-
-    // Device interface
-    class IamovGimbalBase
-    {
-    public:
-        IamovGimbalBase() {}
-        virtual ~IamovGimbalBase() {}
-
-        // non-block functions
-        virtual uint32_t setGimabalPos(const AMOV_GIMBAL_POS_T &pos);
-        virtual uint32_t setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed);
-        virtual uint32_t setGimabalFollowSpeed(const AMOV_GIMBAL_POS_T &followSpeed);
-
-        virtual uint32_t setGimabalHome(void);
-        virtual uint32_t setGimbalZoom(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-        virtual uint32_t setGimbalFocus(AMOV_GIMBAL_ZOOM_T zoom, float targetRate = 0);
-        virtual uint32_t setGimbalROI(const AMOV_GIMBAL_ROI_T area);
-        virtual uint32_t takePic(void);
-        virtual uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
-        virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, const AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
-        virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
-        virtual uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
-        virtual uint32_t extensionFuntions(void *cmd);
-
-        // block functions
-        virtual bool setGimbalPosBlock(const AMOV_GIMBAL_POS_T &pos);
-        virtual bool setGimabalHomeBlock(void);
-        virtual bool setGimbalZoomBlock(float targetRate);
-        virtual bool takePicBlock(void);
-        virtual bool calibrationBlock(void);
-    };
-
-    class amovGimbalBase : public IamovGimbalBase, public PamovGimbalBase
-    {
-    public:
-        virtual uint32_t pack(IN uint32_t cmd, uint8_t *pPayload, uint8_t payloadSize) = 0;
-        virtual bool parser(IN uint8_t byte) = 0;
-        virtual void convert(void *buf) = 0;
-        virtual uint32_t calPackLen(void *pack) = 0;
-
-        virtual void send(void);
-        virtual bool getRxPack(OUT void *pack);
-
-        virtual void parserLoop(void);
-        virtual void sendLoop(void);
-        virtual void mainLoop(void);
-
-        virtual void stackStart(void);
-        virtual void parserStart(pAmovGimbalStateInvoke callback, void *caller);
-
-        virtual void nodeSet(SET uint32_t _self, SET uint32_t _remote);
-
-    public:
-        amovGimbalBase(IOStreamBase *_IO);
-        virtual ~amovGimbalBase();
-    };
-}
-
-#endif

gimbal_ctrl/driver/src/amov_gimbal_realize.cpp

@@ -1,197 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-11-24 15:55:37
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:19:19
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_gimbal_realize.cpp
- */
-
-#include "amov_gimbal_private.h"
-
-#include <thread>
-
-#define MAX_PACK_SIZE 280
-
-/**
- * This is a constructor for the amovGimbalBase class that initializes its parent classes with an
- * IOStreamBase object.
- *
- * @param _IO _IO is a pointer to an object of type amovGimbal::IOStreamBase, which is the base class
- * for input/output streams used by the amovGimbal class. This parameter is passed to the constructor
- * of amovGimbalBase, which is a derived class of I
- */
-amovGimbal::amovGimbalBase::amovGimbalBase(amovGimbal::IOStreamBase *_IO) : amovGimbal::IamovGimbalBase(), amovGimbal::PamovGimbalBase(_IO)
-{
-}
-
-/**
- * The function is a destructor that sleeps for 50 milliseconds and closes an IO object.
- */
-amovGimbal::amovGimbalBase::~amovGimbalBase()
-{
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
-    IO->close();
-}
-
-/**
- * This function retrieves a packet from a ring buffer queue and returns a boolean value indicating
- * whether the operation was successful or not.
- *
- * @param void void is a keyword in C++ that represents the absence of a type. In this function, it is
- * used to indicate that the function does not return any value.
- *
- * @return a boolean value, which indicates whether or not a data packet was successfully retrieved
- * from a ring buffer queue.
- */
-bool amovGimbal::amovGimbalBase::getRxPack(OUT void *pack)
-{
-    bool state = false;
-    state = rxQueue->outCell(pack);
-    return state;
-}
-
-/**
- * This function sends data from a buffer to an output device if it is not busy and open.
- */
-void amovGimbal::amovGimbalBase::send(void)
-{
-    uint8_t tempBuffer[MAX_PACK_SIZE];
-
-    if (IO == nullptr)
-    {
-        while (1)
-        {
-            if (txQueue->outCell(&tempBuffer))
-            {
-                this->outBytes((uint8_t *)&tempBuffer, calPackLen(tempBuffer), outBytesCaller);
-            }
-        }
-    }
-    else
-    {
-        while (1)
-        {
-            if (!IO->isBusy() && IO->isOpen())
-            {
-                if (txQueue->outCell(&tempBuffer))
-                {
-                    IO->outPutBytes((uint8_t *)&tempBuffer, calPackLen(tempBuffer));
-                }
-            }
-        }
-    }
-}
-
-/**
- * "If the input byte is available, then parse it."
- *
- * The function is a loop that runs forever. It calls the IO->inPutByte() function to get a byte from
- * the serial port. If the byte is available, then it calls the parser() function to parse the byte
- */
-void amovGimbal::amovGimbalBase::parserLoop(void)
-{
-    uint8_t temp[65536];
-    uint32_t i = 0, getCount = 0;
-    if (IO == nullptr)
-    {
-        while (1)
-        {
-            getCount = inBytes(temp, inBytesCaller);
-
-            for (i = 0; i < getCount; i++)
-            {
-                parser(temp[i]);
-            }
-        }
-    }
-    else
-    {
-        while (1)
-        {
-            getCount = IO->inPutBytes(temp);
-
-            for (i = 0; i < getCount; i++)
-            {
-                parser(temp[i]);
-            }
-        }
-    }
-}
-
-void amovGimbal::amovGimbalBase::sendLoop(void)
-{
-    send();
-}
-
-void amovGimbal::amovGimbalBase::mainLoop(void)
-{
-    uint8_t tempBuffer[MAX_PACK_SIZE];
-
-    while (1)
-    {
-        if (getRxPack(tempBuffer))
-        {
-            msgCustomCallback(tempBuffer, msgCaller);
-            convert(tempBuffer);
-        }
-    }
-}
-
-void amovGimbal::amovGimbalBase::stackStart(void)
-{
-    if (!this->IO->isOpen() && this->IO != nullptr)
-    {
-        this->IO->open();
-    }
-
-    // 当且仅当需要库主动查询时才启用解析器线程
-    if (inBytes != nullptr || this->IO != nullptr)
-    {
-        std::thread parserLoop(&amovGimbalBase::parserLoop, this);
-        this->parserThreadHanle = parserLoop.native_handle();
-        parserLoop.detach();
-    }
-
-    std::thread sendLoop(&amovGimbalBase::sendLoop, this);
-    this->sendThreadHanle = sendLoop.native_handle();
-    sendLoop.detach();
-}
-
-void amovGimbal::amovGimbalBase::parserStart(pAmovGimbalStateInvoke callback, void *caller)
-{
-    this->updateGimbalStateCallback = callback;
-    this->updataCaller = caller;
-
-    std::thread mainLoop(&amovGimbalBase::mainLoop, this);
-
-    this->stackThreadHanle = mainLoop.native_handle();
-
-    mainLoop.detach();
-}
-
-void amovGimbal::amovGimbalBase::nodeSet(SET uint32_t _self, SET uint32_t _remote)
-{
-    return;
-}
-
-void amovGimbal::gimbal::setRcvBytes(pAmovGimbalInputBytesInvoke callbaclk, void *caller)
-{
-    ((amovGimbal::amovGimbalBase *)(this->devHandle))->inBytes = callbaclk;
-    ((amovGimbal::amovGimbalBase *)(this->devHandle))->inBytesCaller = caller;
-}
-
-void amovGimbal::gimbal::setSendBytes(pAmovGimbalOutputBytesInvoke callbaclk, void *caller)
-{
-    ((amovGimbal::amovGimbalBase *)(this->devHandle))->outBytes = callbaclk;
-    ((amovGimbal::amovGimbalBase *)(this->devHandle))->outBytesCaller = caller;
-}
-
-void amovGimbal::gimbal::inBytesCallback(uint8_t *pData, uint32_t len, gimbal *handle)
-{
-    uint32_t i = 0;
-    for (i = 0; i < len; i++)
-    {
-        ((amovGimbalBase *)((handle)->devHandle))->parser(pData[i]);
-    }
-}

gimbal_ctrl/driver/src/amov_gimbal_struct.h

@@ -0,0 +1,74 @@
+/*
+ * @Description: Common Data Structures of gimbal
+ * @Author: L LC @amov
+ * @Date: 2022-10-31 11:56:43
+ * @LastEditors: L LC @amov
+ * @LastEditTime: 2023-04-18 10:12:33
+ * @FilePath: /gimbal-sdk-multi-platform/src/amov_gimbal_struct.h
+ */
+
+#include <stdint.h>
+
+#ifndef  __AMOV_GIMABL_STRUCT_H
+#define  __AMOV_GIMABL_STRUCT_H
+
+namespace amovGimbal
+{
+    typedef void (*pStateInvoke)(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
+                                 double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
+                                 double &fovX, double &fovY);
+
+    typedef enum
+    {
+        AMOV_GIMBAL_MODE_LOCK,
+        AMOV_GIMBAL_MODE_NULOCK,
+    } AMOV_GIMBAL_MODE_T;
+
+    typedef enum
+    {
+        AMOV_GIMBAL_VIDEO_TAKE,
+        AMOV_GIMBAL_VIDEO_OFF
+    } AMOV_GIMBAL_VIDEO_T;
+
+    typedef enum
+    {
+        AMOV_GIMBAL_ZOOM_IN,
+        AMOV_GIMBAL_ZOOM_OUT,
+        AMOV_GIMBAL_ZOOM_STOP
+    } AMOV_GIMBAL_ZOOM_T;
+
+    typedef struct
+    {
+        double yaw;
+        double roll;
+        double pitch;
+    } AMOV_GIMBAL_POS_T;
+
+    typedef struct
+    {
+        double x;
+        double y;
+    }AMOV_GIMBAL_FOV_T;
+    
+
+    typedef struct
+    {
+        AMOV_GIMBAL_MODE_T workMode;
+        AMOV_GIMBAL_VIDEO_T video;
+        AMOV_GIMBAL_POS_T abs;
+        AMOV_GIMBAL_POS_T rel;
+        AMOV_GIMBAL_POS_T maxFollow;
+        AMOV_GIMBAL_FOV_T fov;
+    } AMOV_GIMBAL_STATE_T;
+
+    typedef struct
+    {
+        uint32_t centreX;
+        uint32_t centreY;
+        uint32_t hight;
+        uint32_t width;
+    } AMOV_GIMBAL_ROI_T;
+
+} // namespace amovGimbal
+
+#endif

gimbal_ctrl/driver/src/amov_tool.h

@@ -1,32 +0,0 @@
-/*
- * @Description:
- * @Author: L LC @amov
- * @Date: 2023-07-31 18:30:33
- * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 16:26:05
- * @FilePath: /SpireCV/gimbal_ctrl/driver/src/amov_tool.h
- */
-
-#ifndef __AMOVGIMABL_TOOL_H
-#define __AMOVGIMABL_TOOL_H
-namespace amovGimbalTools
-{
-    static inline unsigned short conversionBigLittle(unsigned short value)
-    {
-        unsigned short temp = 0;
-        temp |= ((value >> 8) & 0X00FF);
-        temp |= ((value << 8) & 0XFF00);
-        return temp;
-    }
-
-    static inline unsigned int conversionBigLittle(unsigned int value)
-    {
-        unsigned int temp = 0;
-        temp |= ((value << 24) & 0XFF000000);
-        temp |= ((value << 8) & 0X00FF0000);
-        temp |= ((value >> 8) & 0X0000FF00);
-        temp |= ((value << 24) & 0X000000FF);
-        return temp;
-    }
-}
-#endif

gimbal_ctrl/sv_gimbal.cpp

@@ -3,10 +3,11 @@
  * @Author: L LC @amov
  * @Date: 2023-04-12 09:12:52
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:33:29
+ * @LastEditTime: 2023-04-18 11:37:42
- * @FilePath: /SpireCV/gimbal_ctrl/sv_gimbal.cpp
+ * @FilePath: /spirecv-gimbal-sdk/gimbal_ctrl/sv_gimbal.cpp
  */
-#include "amovGimbal/amov_gimbal.h"
+#include "amov_gimbal.h"
+#include "amov_gimbal_struct.h"
 
 #include "sv_gimbal.h"
 #include "sv_gimbal_io.hpp"
@@ -14,52 +15,6 @@
 #include <iostream>
 #include <string>
 #include <thread>
-namespace sv
-{
-    std::map<std::string, void *> Gimbal::IOList;
-    std::mutex Gimbal::IOListMutex;
-
-    typedef struct
-    {
-        std::string name;
-        GimbalLink supportLink;
-    } gimbalTrait;
-
-    std::map<GimbalType, gimbalTrait> gimbaltypeList =
-        {
-            {GimbalType::G1, {"G1", GimbalLink::SERIAL}},
-            {GimbalType::Q10f, {"Q10f", GimbalLink::SERIAL}},
-            {GimbalType::AT10, {"AT10", GimbalLink::SERIAL | GimbalLink::ETHERNET_TCP}},
-            {GimbalType::GX40, {"GX40", GimbalLink::SERIAL | GimbalLink::ETHERNET_TCP | GimbalLink::ETHERNET_UDP}}};
-
-    /**
-     * The function `svGimbalType2Str` converts a `GimbalType` enum value to its corresponding string
-     * representation.
-     *
-     * @return a reference to a string.
-     */
-    std::string &svGimbalType2Str(const GimbalType &type)
-    {
-        std::map<GimbalType, gimbalTrait>::iterator temp = gimbaltypeList.find(type);
-        if (temp != gimbaltypeList.end())
-        {
-            return (temp->second).name;
-        }
-        throw "Error: Unsupported gimbal device type!!!!";
-        exit(-1);
-    }
-
-    GimbalLink &svGimbalTypeFindLinkType(const GimbalType &type)
-    {
-        std::map<GimbalType, gimbalTrait>::iterator temp = gimbaltypeList.find(type);
-        if (temp != gimbaltypeList.end())
-        {
-            return (temp->second).supportLink;
-        }
-        throw "Error: Unsupported gimbal device type!!!!";
-        exit(-1);
-    }
-}
 
 /**
  * This function sets the serial port for a Gimbal object.
@@ -123,29 +78,17 @@ void sv::Gimbal::setNetIp(const std::string &ip)
 }
 
 /**
- * The function sets the TCP network port for the Gimbal object.
+ * This function sets the network port for a Gimbal object in C++.
  *
- * @param port The parameter "port" is an integer that represents the TCP network port number.
+ * @param port The "port" parameter is an integer value that represents the network port number that
+ * the Gimbal object will use for communication. This function sets the value of the "m_net_port"
+ * member variable of the Gimbal object to the value passed in as the "port" parameter.
  */
-void sv::Gimbal::setTcpNetPort(const int &port)
+void sv::Gimbal::setNetPort(const int &port)
 {
     this->m_net_port = port;
 }
 
-/**
- * The function sets the UDP network ports for receiving and sending data.
- *
- * @param recvPort The recvPort parameter is the port number that the Gimbal object will use to receive
- * UDP packets.
- * @param sendPort The sendPort parameter is the port number used for sending data over UDP (User
- * Datagram Protocol) network communication.
- */
-void sv::Gimbal::setUdpNetPort(const int &recvPort, const int &sendPort)
-{
-    this->m_net_recv_port = recvPort;
-    this->m_net_send_port = sendPort;
-}
-
 /**
  * The function sets a parser callback for a gimbal device.
  *
@@ -156,85 +99,7 @@ void sv::Gimbal::setUdpNetPort(const int &recvPort, const int &sendPort)
 void sv::Gimbal::setStateCallback(sv::PStateInvoke callback)
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
-    m_callback = callback;
+    pdevTemp->dev->setParserCallback(callback);
-    pdevTemp->setParserCallback(sv::Gimbal::gimbalUpdataCallback, this);
-}
-
-/**
- * The function `sv::Gimbal::creatIO` creates an IO object based on the specified gimbal type and link
- * type, and returns a pointer to the created object.
- *
- * @param dev The "dev" parameter is a pointer to an object of type "sv::Gimbal". It is used to access
- * the member variables of the Gimbal object, such as "m_serial_port", "m_serial_baud_rate",
- * "m_serial_timeout", etc. These variables store information about
- *
- * @return a void pointer.
- */
-void *sv::Gimbal::creatIO(sv::Gimbal *dev)
-{
-    IOListMutex.lock();
-    std::map<std::string, void *>::iterator list = IOList.find(dev->m_serial_port);
-    std::pair<std::string, void *> key("NULL", nullptr);
-    GimbalLink link = svGimbalTypeFindLinkType(dev->m_gimbal_type);
-
-    if ((dev->m_gimbal_link & svGimbalTypeFindLinkType(dev->m_gimbal_type)) == GimbalLink::NONE)
-    {
-        throw std::runtime_error("gimbal Unsupported linktype !!!");
-    }
-
-    if (list == IOList.end())
-    {
-        if (dev->m_gimbal_link == GimbalLink::SERIAL)
-        {
-            UART *ser;
-            ser = new UART(dev->m_serial_port,
-                           (uint32_t)dev->m_serial_baud_rate,
-                           serial::Timeout::simpleTimeout(dev->m_serial_timeout),
-                           (serial::bytesize_t)dev->m_serial_byte_size,
-                           (serial::parity_t)dev->m_serial_parity,
-                           (serial::stopbits_t)dev->m_serial_stopbits,
-                           (serial::flowcontrol_t)dev->m_serial_flowcontrol);
-            key.first = dev->m_serial_port;
-            key.second = (void *)ser;
-            IOList.insert(key);
-        }
-        else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP)
-        {
-            TCPClient *tcp;
-            tcp = new TCPClient(dev->m_net_ip, dev->m_net_port);
-            key.first = dev->m_net_ip;
-            key.second = (void *)tcp;
-            IOList.insert(key);
-        }
-        else if (dev->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP)
-        {
-            UDP *udp;
-            udp = new UDP(dev->m_net_ip, dev->m_net_recv_port, dev->m_net_send_port);
-            key.first = dev->m_net_ip;
-            key.second = (void *)udp;
-            IOList.insert(key);
-        }
-    }
-    else
-    {
-        std::cout << "Error: gimbal IO has opened!!!" << std::endl;
-    }
-    IOListMutex.unlock();
-
-    return key.second;
-}
-
-/**
- * The function removes a Gimbal device from the IOList.
- *
- * @param dev dev is a pointer to an object of type sv::Gimbal.
- */
-void sv::Gimbal::removeIO(sv::Gimbal *dev)
-{
-    IOListMutex.lock();
-    IOList.erase(dev->m_serial_port);
-
-    IOListMutex.unlock();
 }
 
 /**
@@ -250,24 +115,53 @@ void sv::Gimbal::removeIO(sv::Gimbal *dev)
  */
 bool sv::Gimbal::open(PStateInvoke callback)
 {
-    bool ret = false;
+    if (this->m_gimbal_link == GimbalLink::SERIAL)
-
-    this->IO = creatIO(this);
-
-    if (this->IO != nullptr)
     {
-        std::string driverName;
+        this->IO = new UART(this->m_serial_port,
-        driverName = sv::svGimbalType2Str(this->m_gimbal_type);
+                            (uint32_t)this->m_serial_baud_rate,
-        this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO);
+                            serial::Timeout::simpleTimeout(this->m_serial_timeout),
-        amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
+                            (serial::bytesize_t)this->m_serial_byte_size,
-        pdevTemp->startStack();
+                            (serial::parity_t)this->m_serial_parity,
-        m_callback = callback;
+                            (serial::stopbits_t)this->m_serial_stopbits,
-        pdevTemp->parserAuto(sv::Gimbal::gimbalUpdataCallback, this);
+                            (serial::flowcontrol_t)this->m_serial_flowcontrol);
-
-        ret = true;
     }
+    // Subsequent additions
+    else if (this->m_gimbal_link == sv::GimbalLink::ETHERNET_TCP)
+    {
+        return false;
+    }
+    else if (this->m_gimbal_link == sv::GimbalLink::ETHERNET_UDP)
+    {
+        return false;
+    }
+    else
+    {
+        throw "Error: Unsupported communication interface class!!!";
+        return false;
+    }
+    std::string driverName;
+    switch (this->m_gimbal_type)
+    {
+    case sv::GimbalType::G1:
+        driverName = "G1";
+        break;
+    case sv::GimbalType::Q10f:
+        driverName = "Q10f";
+        break;
 
-    return ret;
+    default:
+        throw "Error: Unsupported driver!!!";
+        return false;
+        break;
+    }
+    this->dev = new amovGimbal::gimbal(driverName, (amovGimbal::IOStreamBase *)this->IO);
+
+    amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
+
+    pdevTemp->dev->startStack();
+    pdevTemp->dev->parserAuto(callback);
+
+    return true;
 }
 
 /**
@@ -280,7 +174,7 @@ bool sv::Gimbal::open(PStateInvoke callback)
 bool sv::Gimbal::setHome()
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
-    if (pdevTemp->setGimabalHome() > 0)
+    if (pdevTemp->dev->setGimabalHome() > 0)
     {
         return true;
     }
@@ -309,7 +203,7 @@ bool sv::Gimbal::setZoom(double x)
         return false;
     }
 
-    if (pdevTemp->setGimbalZoom(AMOV_GIMBAL_ZOOM_STOP, x) > 0)
+    if (pdevTemp->dev->setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_STOP, x) > 0)
     {
         return true;
     }
@@ -333,7 +227,7 @@ bool sv::Gimbal::setAutoZoom(int state)
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
 
-    if (pdevTemp->setGimbalZoom((AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0)
+    if (pdevTemp->dev->setGimbalZoom((amovGimbal::AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0)
     {
         return true;
     }
@@ -359,7 +253,7 @@ bool sv::Gimbal::setAutoFocus(int state)
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
 
-    if (pdevTemp->setGimbalFocus((AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0)
+    if (pdevTemp->dev->setGimbalFocus((amovGimbal::AMOV_GIMBAL_ZOOM_T)state, 0.0f) > 0)
     {
         return true;
     }
@@ -379,7 +273,7 @@ bool sv::Gimbal::takePhoto()
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
 
-    if (pdevTemp->takePic() > 0)
+    if (pdevTemp->dev->takePic() > 0)
     {
         return true;
     }
@@ -403,21 +297,21 @@ bool sv::Gimbal::takeVideo(int state)
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
 
-    AMOV_GIMBAL_VIDEO_T newState;
+    amovGimbal::AMOV_GIMBAL_VIDEO_T newState;
     switch (state)
     {
     case 0:
-        newState = AMOV_GIMBAL_VIDEO_OFF;
+        newState = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
         break;
     case 1:
-        newState = AMOV_GIMBAL_VIDEO_TAKE;
+        newState = amovGimbal::AMOV_GIMBAL_VIDEO_TAKE;
         break;
     default:
-        newState = AMOV_GIMBAL_VIDEO_OFF;
+        newState = amovGimbal::AMOV_GIMBAL_VIDEO_OFF;
         break;
     }
 
-    if (pdevTemp->setVideo(newState) > 0)
+    if (pdevTemp->dev->setVideo(newState) > 0)
     {
         return true;
     }
@@ -438,13 +332,13 @@ int sv::Gimbal::getVideoState()
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
     int ret;
-    AMOV_GIMBAL_STATE_T state;
+    amovGimbal::AMOV_GIMBAL_STATE_T state;
-    state = pdevTemp->getGimabalState();
+    state = pdevTemp->dev->getGimabalState();
-    if (state.video == AMOV_GIMBAL_VIDEO_TAKE)
+    if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_TAKE)
     {
         ret = 1;
     }
-    else if (state.video == AMOV_GIMBAL_VIDEO_OFF)
+    else if (state.video == amovGimbal::AMOV_GIMBAL_VIDEO_OFF)
     {
         ret = 0;
     }
@@ -472,7 +366,7 @@ void sv::Gimbal::setAngleEuler(double roll, double pitch, double yaw,
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
 
-    AMOV_GIMBAL_POS_T temp;
+    amovGimbal::AMOV_GIMBAL_POS_T temp;
 
     if (pitch_rate == 0.0f)
         pitch_rate = 360;
@@ -484,11 +378,11 @@ void sv::Gimbal::setAngleEuler(double roll, double pitch, double yaw,
     temp.pitch = pitch_rate;
     temp.roll = roll_rate;
     temp.yaw = yaw_rate;
-    pdevTemp->setGimabalFollowSpeed(temp);
+    pdevTemp->dev->setGimabalFollowSpeed(temp);
     temp.pitch = pitch;
     temp.roll = roll;
     temp.yaw = yaw;
-    pdevTemp->setGimabalPos(temp);
+    pdevTemp->dev->setGimabalPos(temp);
 }
 
 /**
@@ -503,60 +397,15 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y
 {
     amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
 
-    AMOV_GIMBAL_POS_T temp;
+    amovGimbal::AMOV_GIMBAL_POS_T temp;
     temp.pitch = pitch_rate;
     temp.roll = roll_rate;
     temp.yaw = yaw_rate;
-    pdevTemp->setGimabalSpeed(temp);
+    pdevTemp->dev->setGimabalSpeed(temp);
-}
-void sv::Gimbal::attitudeCorrection(const GimbalQuaternionT &quaterion,
-                                    const GimbalVelocityT &speed,
-                                    const GimbalVelocityT &acc, void *extenData)
-{
-    amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
-    AMOV_GIMBAL_QUATERNION_T temp_q;
-    temp_q.q0 = quaterion.q0;
-    temp_q.q1 = quaterion.q1;
-    temp_q.q2 = quaterion.q2;
-    temp_q.q3 = quaterion.q3;
-
-    AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
-    temp_v1.x = speed.x;
-    temp_v1.y = speed.y;
-    temp_v1.z = speed.z;
-
-    temp_v2.x = acc.x;
-    temp_v2.y = acc.y;
-    temp_v2.z = acc.z;
-
-    pdevTemp->attitudeCorrection(temp_q, temp_v1, temp_v2, extenData);
-}
-
-void sv::Gimbal::attitudeCorrection(const GimbalPosT &pos,
-                                    const GimbalVelocityT &speed,
-                                    const GimbalVelocityT &acc, void *extenData)
-{
-    amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
-    AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
-    temp_v1.x = speed.x;
-    temp_v1.y = speed.y;
-    temp_v1.z = speed.z;
-
-    temp_v2.x = acc.x;
-    temp_v2.y = acc.y;
-    temp_v2.z = acc.z;
-
-    AMOV_GIMBAL_POS_T temp_p;
-    temp_p.pitch = pos.pitch;
-    temp_p.yaw = pos.yaw;
-    temp_p.roll = pos.roll;
-
-    pdevTemp->attitudeCorrection(temp_p, temp_v1, temp_v2, extenData);
 }
 
 sv::Gimbal::~Gimbal()
 {
-    removeIO(this);
     delete (amovGimbal::gimbal *)this->dev;
     delete (amovGimbal::IOStreamBase *)this->IO;
 }

gimbal_ctrl/sv_gimbal_io.hpp

@@ -3,236 +3,65 @@
  * @Author: L LC @amov
  * @Date: 2023-04-12 12:22:09
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:38:59
+ * @LastEditTime: 2023-04-13 10:17:21
- * @FilePath: /SpireCV/gimbal_ctrl/sv_gimbal_io.hpp
+ * @FilePath: /spirecv-gimbal-sdk/gimbal_ctrl/sv_gimbal_io.hpp
  */
 #ifndef __SV_GIMABL_IO_H
 #define __SV_GIMABL_IO_H
 
-#include "amovGimbal/amov_gimbal.h"
+#include "amov_gimbal.h"
 #include "serial/serial.h"
 
-#include <string.h>
+class UART : public amovGimbal::IOStreamBase
-#include <stdio.h>
-#if defined(_WIN32)
-#include <winsock2.h>
-#include <ws2tcpip.h>
-#endif
-
-#if defined(__linux__)
-#include <arpa/inet.h>
-#endif
-
-#include <unistd.h>
-namespace sv
 {
-    class UART : public amovGimbal::IOStreamBase
+private:
-    {
+    serial::Serial *ser;
-    private:
-        serial::Serial *ser;
 
-    public:
+public:
-        virtual bool open()
+    virtual bool open()
-        {
-            ser->open();
-            return true;
-        }
-        virtual bool close()
-        {
-            ser->close();
-            return true;
-        }
-        virtual bool isOpen()
-        {
-            return ser->isOpen();
-        }
-        virtual bool isBusy()
-        {
-            return false;
-        }
-        virtual uint32_t inPutBytes(IN uint8_t *byte)
-        {
-            if (ser->read(byte, 1))
-            {
-                return 1;
-            }
-            return 0;
-        }
-        virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
-        {
-            return ser->write(byte, lenght);
-        }
-        UART(const std::string &port, uint32_t baudrate, serial::Timeout timeout,
-             serial::bytesize_t bytesize, serial::parity_t parity, serial::stopbits_t stopbits,
-             serial::flowcontrol_t flowcontrol)
-        {
-            ser = new serial::Serial(port, baudrate, timeout, bytesize, parity, stopbits, flowcontrol);
-        }
-        ~UART()
-        {
-            ser->close();
-        }
-    };
-
-    int scoketClose(int scoket)
     {
-#if defined(_WIN32)
+        ser->open();
-        return closesocket(scoket);
+        return true;
-#endif
-#if defined(__linux__)
-        return close(scoket);
-#endif
     }
-    class TCPClient : public amovGimbal::IOStreamBase
+    virtual bool close()
     {
-    private:
+        ser->close();
-        int scoketFd;
+        return true;
-        sockaddr_in scoketAddr;
+    }
-
+    virtual bool isOpen()
-    public:
-        virtual bool open()
-        {
-            return true;
-        }
-        virtual bool close()
-        {
-            sv::scoketClose(scoketFd);
-            return true;
-        }
-        virtual bool isOpen()
-        {
-            return true;
-        }
-        virtual bool isBusy()
-        {
-            return false;
-        }
-        virtual uint32_t inPutBytes(IN uint8_t *byte)
-        {
-            return recv(scoketFd, (char *)byte, 65536, 0);
-        }
-        virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
-        {
-            return send(scoketFd, (const char *)byte, lenght, 0);
-        }
-
-        TCPClient(const std::string &addr, const uint16_t port)
-        {
-            if ((scoketFd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
-            {
-                throw std::runtime_error("scoket creat failed");
-            }
-            memset(&scoketAddr, 0, sizeof(scoketAddr));
-            scoketAddr.sin_family = AF_INET;
-            scoketAddr.sin_addr.s_addr = inet_addr(addr.c_str());
-
-            if (scoketAddr.sin_addr.s_addr == INADDR_NONE ||
-                scoketAddr.sin_addr.s_addr == INADDR_ANY)
-            {
-                sv::scoketClose(scoketFd);
-
-                throw std::runtime_error("scoket addr errr");
-            }
-            scoketAddr.sin_port = htons(port);
-
-            if (connect(scoketFd, (struct sockaddr *)&scoketAddr, sizeof(scoketAddr)) != 0)
-            {
-                sv::scoketClose(scoketFd);
-
-                throw std::runtime_error("scoket connect failed !");
-            }
-        }
-        ~TCPClient()
-        {
-            close();
-        }
-    };
-
-    class UDP : public amovGimbal::IOStreamBase
     {
-    private:
+        return ser->isOpen();
-        int rcvScoketFd, sendScoketFd;
+    }
-        sockaddr_in rcvScoketAddr, sendScoketAddr;
+    virtual bool isBusy()
+    {
+        return false;
+    }
 
-    public:
+    virtual bool inPutByte(IN uint8_t *byte)
-        virtual bool open()
+    {
+        if (ser->read(byte, 1))
         {
             return true;
         }
-        virtual bool close()
+        return false;
-        {
+    }
-            sv::scoketClose(rcvScoketFd);
-            sv::scoketClose(sendScoketFd);
-            return true;
-        }
-        virtual bool isOpen()
-        {
-            return true;
-        }
-        virtual bool isBusy()
-        {
-            return false;
-        }
-        virtual uint32_t inPutBytes(IN uint8_t *byte)
-        {
-            sockaddr_in remoteAddr;
-            int len = sizeof(remoteAddr);
 
-            return recvfrom(rcvScoketFd, (char *)byte, 65536, 0,
+    virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
-                            (struct sockaddr *)&remoteAddr, reinterpret_cast<socklen_t *>(&len));
+    {
-        }
+        return ser->write(byte, lenght);
-        virtual uint32_t outPutBytes(IN uint8_t *byte, uint32_t lenght)
+    }
-        {
-            return sendto(sendScoketFd, (const char *)byte, lenght, 0,
-                          (struct sockaddr *)&sendScoketAddr, sizeof(sendScoketAddr));
-        }
 
-        UDP(const std::string &remoteAddr, const uint16_t rcvPort, uint16_t sendPort)
+    UART(const std::string &port, uint32_t baudrate, serial::Timeout timeout,
-        {
+         serial::bytesize_t bytesize, serial::parity_t parity, serial::stopbits_t stopbits,
-            if ((rcvScoketFd = socket(AF_INET, SOCK_DGRAM, 0)) == -1 ||
+         serial::flowcontrol_t flowcontrol)
-                (sendScoketFd = socket(AF_INET, SOCK_DGRAM, 0)) == -1)
+    {
-            {
+        ser = new serial::Serial(port, baudrate, timeout, bytesize, parity, stopbits, flowcontrol);
-                sv::scoketClose(rcvScoketFd);
+    }
-                sv::scoketClose(sendScoketFd);
+    ~UART()
-                throw std::runtime_error("scoket creat failed");
+    {
-            }
+        ser->close();
-            memset(&rcvScoketAddr, 0, sizeof(rcvScoketAddr));
+        delete ser;
-            memset(&sendScoketAddr, 0, sizeof(sendScoketAddr));
+    }
-            sendScoketAddr.sin_family = AF_INET;
+};
-            sendScoketAddr.sin_addr.s_addr = inet_addr(remoteAddr.c_str());
-            sendScoketAddr.sin_port = htons(sendPort);
-
-            if (sendScoketAddr.sin_addr.s_addr == INADDR_NONE ||
-                sendScoketAddr.sin_addr.s_addr == INADDR_ANY)
-            {
-                sv::scoketClose(sendScoketFd);
-
-                throw std::runtime_error("scoket addr errr");
-            }
-
-            rcvScoketAddr.sin_family = AF_INET;
-            rcvScoketAddr.sin_addr.s_addr = INADDR_ANY;
-            rcvScoketAddr.sin_port = htons(rcvPort);
-
-            if (rcvScoketAddr.sin_addr.s_addr == INADDR_NONE)
-            {
-                sv::scoketClose(rcvScoketFd);
-
-                throw std::runtime_error("scoket addr errr");
-            }
-
-            if (bind(rcvScoketFd, (struct sockaddr *)&rcvScoketAddr, sizeof(rcvScoketAddr)) == -1)
-            {
-                sv::scoketClose(rcvScoketFd);
-
-                throw std::runtime_error("scoket bind failed !");
-            }
-        }
-        ~UDP()
-        {
-            close();
-        }
-    };
-}
 
 #endif

include/sv_algorithm_base.h

@@ -46,7 +46,6 @@ class ArucoDetector : public CameraAlgorithm
 public:
   ArucoDetector();
   void detect(cv::Mat img_, TargetsInFrame& tgts_);
-  void getIdsWithLengths(std::vector<int>& ids_, std::vector<double>& lengths_);
 private:
   void _load();
   bool _params_loaded;
@@ -105,6 +104,10 @@ public:
   std::string getAlgorithm();
   int getBackend();
   int getTarget();
+  double getObjectWs();
+  double getObjectHs();
+  int useWidthOrHeight();
+
 protected:
   virtual bool setupImpl();
   virtual void initImpl(cv::Mat img_, const cv::Rect& bounding_box_);
@@ -114,6 +117,9 @@ protected:
   std::string _algorithm;
   int _backend;
   int _target;
+  int _use_width_or_height;
+  double _object_ws;
+  double _object_hs;
 };
 
 
@@ -140,8 +146,6 @@ public:
   double getThrsConf();
   int useWidthOrHeight();
   bool withSegmentation();
-  std::string getModel();
-  int getBatchSize();
 protected:
   virtual bool setupImpl();
   virtual void detectImpl(
@@ -168,8 +172,6 @@ protected:
   double _thrs_conf;
   int _use_width_or_height;
   bool _with_segmentation;
-  std::string _model;
-  int _batch_size;
 };
 
 

include/sv_color_line.h

@@ -25,6 +25,7 @@ public:
   double line_location_a2;
 
   bool is_load_parameter;
+
   std::string line_color;
 
 protected:
@@ -39,4 +40,4 @@ protected:
   void seg(cv::Mat line_area_, cv::Mat line_area_a1_, cv::Mat line_area_a2_, std::string line_color_, cv::Point &center_, int &area_, cv::Point &center_a1_, cv::Point &center_a2_);
 };
 }
-#endif
+#endif

include/sv_common_det.h

@@ -8,36 +8,32 @@
 #include <string>
 #include <chrono>
 
-namespace sv
+
-{
+namespace sv {
 
 class CommonObjectDetectorCUDAImpl;
-class CommonObjectDetectorIntelImpl;
-
 
 class CommonObjectDetector : public CommonObjectDetectorBase
 {
 public:
-  CommonObjectDetector(bool input_4k = false);
+  CommonObjectDetector();
   ~CommonObjectDetector();
-
 protected:
   bool setupImpl();
   void detectImpl(
     cv::Mat img_,
-    std::vector<float> &boxes_x_,
+    std::vector<float>& boxes_x_,
-    std::vector<float> &boxes_y_,
+    std::vector<float>& boxes_y_,
-    std::vector<float> &boxes_w_,
+    std::vector<float>& boxes_w_,
-    std::vector<float> &boxes_h_,
+    std::vector<float>& boxes_h_,
-    std::vector<int> &boxes_label_,
+    std::vector<int>& boxes_label_,
-    std::vector<float> &boxes_score_,
+    std::vector<float>& boxes_score_,
-    std::vector<cv::Mat> &boxes_seg_);
+    std::vector<cv::Mat>& boxes_seg_
+  );
 
-  CommonObjectDetectorCUDAImpl *_cuda_impl;
+  CommonObjectDetectorCUDAImpl* _cuda_impl;
-  CommonObjectDetectorIntelImpl *_intel_impl;
-
-  bool _input_4k;
 };
 
+
 }
 #endif

include/sv_gimbal.h

@@ -3,17 +3,13 @@
  * @Author: jario-jin @amov
  * @Date: 2023-04-12 09:12:52
  * @LastEditors: L LC @amov
- * @LastEditTime: 2023-12-05 17:25:40
+ * @LastEditTime: 2023-04-18 11:49:27
- * @FilePath: /SpireCV/include/sv_gimbal.h
+ * @FilePath: /spirecv-gimbal-sdk/include/sv_gimbal.h
  */
 #ifndef __SV_GIMBAL__
 #define __SV_GIMBAL__
 
 #include <string>
-#include <map>
-#include <iterator>
-#include <thread>
-#include <mutex>
 
 namespace sv
 {
@@ -25,28 +21,15 @@ namespace sv
   enum class GimbalType
   {
     G1,
-    Q10f,
+    Q10f
-    AT10,
-    GX40,
   };
-  enum class GimbalLink : int
+  enum class GimbalLink
   {
-    NONE = 0x00,
+    SERIAL,
-    SERIAL = 0x01,
+    ETHERNET_TCP,
-    ETHERNET_TCP = 0x02,
+    ETHERNET_UDP
-    ETHERNET_UDP = 0x04,
   };
 
-  constexpr GimbalLink operator|(GimbalLink a, GimbalLink b)
-  {
-    return static_cast<GimbalLink>(static_cast<int>(a) | static_cast<int>(b));
-  }
-
-  constexpr GimbalLink operator&(GimbalLink a, GimbalLink b)
-  {
-    return static_cast<GimbalLink>(static_cast<int>(a) & static_cast<int>(b));
-  }
-
   enum class GimablSerialByteSize
   {
     FIVE_BYTES = 5,
@@ -78,28 +61,6 @@ namespace sv
     FLOWCONTROL_HARDWARE = 2,
   };
 
-  typedef struct
-  {
-    double q0;
-    double q1;
-    double q2;
-    double q3;
-  } GimbalQuaternionT;
-
-  typedef struct
-  {
-    double x; // or N
-    double y; // or E
-    double z; // or UP
-  } GimbalVelocityT;
-
-  typedef struct
-  {
-    double yaw;
-    double roll;
-    double pitch;
-  } GimbalPosT;
-
   static inline void emptyCallback(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
                                    double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
                                    double &fovX, double &fovY)
@@ -117,7 +78,6 @@ namespace sv
     // Device pointers
     void *dev;
     void *IO;
-    PStateInvoke m_callback;
 
     // Generic serial interface parameters list & default parameters
     std::string m_serial_port = "/dev/ttyUSB0";
@@ -129,26 +89,12 @@ namespace sv
     int m_serial_timeout = 500;
 
     // Ethernet interface parameters list & default parameters
-    std::string m_net_ip = "192.168.144.121";
+    std::string m_net_ip = "192.168.2.64";
-    int m_net_port = 2332;
+    int m_net_port = 9090;
-    int m_net_recv_port = 2338;
-    int m_net_send_port = 2337;
 
     GimbalType m_gimbal_type;
     GimbalLink m_gimbal_link;
 
-    static std::map<std::string, void *> IOList;
-    static std::mutex IOListMutex;
-    static void *creatIO(Gimbal *dev);
-    static void removeIO(Gimbal *dev);
-
-    static void gimbalUpdataCallback(double frameAngleRoll, double frameAnglePitch, double frameAngleYaw,
-                                     double imuAngleRoll, double imuAnglePitch, double imuAngleYaw,
-                                     double fovX, double fovY, void *handle)
-    {
-      ((Gimbal *)(handle))->m_callback(frameAngleRoll, frameAnglePitch, frameAngleYaw, imuAngleRoll, imuAnglePitch, imuAngleYaw, fovX, fovY);
-    }
-
   public:
     //! Constructor
     /*!
@@ -170,10 +116,7 @@ namespace sv
 
     // set Ethernet interface parameters
     void setNetIp(const std::string &ip);
-    // set tcp port
+    void setNetPort(const int &port);
-    void setTcpNetPort(const int &port);
-    // set udp port
-    void setUdpNetPort(const int &recvPort, const int &sendPort);
 
     // Create a device instance
     void setStateCallback(PStateInvoke callback);
@@ -187,12 +130,6 @@ namespace sv
     bool takePhoto();
     bool takeVideo(int state);
     int getVideoState();
-    void attitudeCorrection(const GimbalQuaternionT &quaterion,
-                                    const GimbalVelocityT &speed,
-                                    const GimbalVelocityT &acc, void *extenData);
-    void attitudeCorrection(const GimbalPosT &pos,
-                                    const GimbalVelocityT &speed,
-                                    const GimbalVelocityT &acc, void *extenData);
 
     //! Set gimbal angles
     /*!

include/sv_landing_det.h

@@ -12,7 +12,6 @@
 namespace sv {
 
 class LandingMarkerDetectorCUDAImpl;
-class LandingMarkerDetectorIntelImpl;
 
 class LandingMarkerDetector : public LandingMarkerDetectorBase
 {
@@ -26,8 +25,7 @@ protected:
     std::vector<int>& output_labels_
   );
 
-  LandingMarkerDetectorCUDAImpl *_cuda_impl;
+  LandingMarkerDetectorCUDAImpl* _cuda_impl;
-  LandingMarkerDetectorIntelImpl *_intel_impl;
 };
 
 

include/sv_mot.h

@@ -15,7 +15,7 @@ namespace sv {
 
 
 class SORT;
-class BYTETracker;
+
 
 class MultipleObjectTracker : public CameraAlgorithm
 {
@@ -24,7 +24,7 @@ public:
     ~MultipleObjectTracker();
 
     void init(CommonObjectDetector* detector_);
-    sv::TargetsInFrame track(cv::Mat img_, TargetsInFrame& tgts_);
+    void track(cv::Mat img_, TargetsInFrame& tgts_);
 
 private:
     void _load();
@@ -39,12 +39,65 @@ private:
     double _iou_thres;
     int _max_age;
     int _min_hits;
-    int _frame_rate;
-    int _track_buffer;
     
     SORT* _sort_impl;
-    BYTETracker* _bytetrack_impl;
     CommonObjectDetector* _detector;
 };
+
+
+// define the tracklet struct to store the tracked objects.
+struct Tracklet
+{
+/* data */
+public:
+    Eigen::Vector4d bbox;  // double x, y, w, h;
+    int id = 0;
+    int age;
+    int hits;
+    int misses;
+    int frame_id = 0;
+    bool tentative;
+    std::vector<double> features;
+    Eigen::Matrix<double, 8, 1> mean;
+    Eigen::Matrix<double, 8, 8> covariance;
+};
+
+
+class KalmanFilter {
+public:
+    KalmanFilter();
+    ~KalmanFilter();
+    std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > initiate(Eigen::Vector4d &bbox);
+    std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > update(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances, Box &box);
+    std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > predict(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
+private:
+    Eigen::Matrix<double, 8, 8> _F;
+    Eigen::Matrix<double, 4, 8> _H;
+    Eigen::Matrix<double, 9, 1> _chi2inv95;
+    double _std_weight_position;
+    double _std_weight_vel;
+};
+
+
+class SORT {
+public:
+    SORT(double iou_threshold, int max_age, int min_hits): _iou_threshold(iou_threshold), _max_age(max_age), _min_hits(min_hits), _next_tracklet_id(0) {};
+    ~SORT();
+    void update(TargetsInFrame &tgts);
+    std::vector<Tracklet> getTracklets() const;
+private:
+    double _iou(Tracklet &tracklet, Box &box);
+    std::vector<std::pair<int,int>> _hungarian(std::vector<std::vector<double>> costMatrix);
+    bool _augment(const std::vector<std::vector<double>>& costMatrix, int row, std::vector<int>& rowMatch, std::vector<int>& colMatch, std::vector<bool>& visited);
+
+    double _iou_threshold;
+    int _max_age;
+    int _min_hits;
+    int _next_tracklet_id;
+    std::vector <Tracklet> _tracklets;
+    std::vector <Tracklet> _new_tracklets;
+};
+
+
 }
 #endif

include/sv_veri_det.h

@@ -1,44 +0,0 @@
-#ifndef __SV_VERI_DET__
-#define __SV_VERI_DET__
-
-#include "sv_core.h"
-#include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
-#include <opencv2/tracking.hpp>
-#include <string>
-#include <chrono>
-
-namespace sv
-{
-
-class VeriDetectorCUDAImpl;
-class VeriDetectorIntelImpl;
-
-class VeriDetector : public LandingMarkerDetectorBase
-{
-public:
-  VeriDetector();
-  ~VeriDetector();
-
-  void detect(cv::Mat img_, const cv::Rect &bounding_box_, sv::Target &tgt);
-
-protected:
-  void _load();
-  bool setupImpl();
-  void roiCNN(
-    std::vector<cv::Mat> &input_rois_,
-    std::vector<float> &output_labels_);
-  void getSubwindow(cv::Mat &dstCrop, cv::Mat &srcImg, int originalSz, int resizeSz);
-
-  std::string vehicle_id;
-    
-  // Save the target bounding box for each frame.
-  std::vector<float> targetSz = {0, 0};  // H and W of bounding box
-  std::vector<float> targetPos = {0, 0}; // center point of bounding box (x, y)
-
-  VeriDetectorCUDAImpl *_cuda_impl;
-  VeriDetectorIntelImpl *_intel_impl;
-};
-
-}
-#endif

include/sv_video_base.h

@@ -90,9 +90,6 @@ public:
   double los_ay;
   //! The angle of the target in the image coordinate system,  (unit: degree) [-180, 180].
   double yaw_a;
-  
-  //! Similarity, Confidence, (0, 1]
-  double sim_score;
 
   //! Whether the height&width of the target can be obtained.
   bool has_hw;
@@ -128,7 +125,6 @@ public:
   
   bool getBox(Box& b);
   bool getAruco(int& id, std::vector<cv::Point2f> &corners);
-  bool getAruco(int& id, std::vector<cv::Point2f> &corners, cv::Vec3d &rvecs, cv::Vec3d &tvecs);
   bool getEllipse(double& xc_, double& yc_, double& a_, double& b_, double& rad_);
   std::string getJsonStr();
 
@@ -327,13 +323,13 @@ protected:
 };
 
 
-enum class CameraType {NONE, WEBCAM, V4L2CAM, MIPI, RTSP, VIDEO, G1, Q10, GX40};
+enum class CameraType {NONE, WEBCAM, G1, Q10, MIPI};
 
 class CameraBase {
 public:
   CameraBase(CameraType type=CameraType::NONE, int id=0);
   ~CameraBase();
-  void open(CameraType type=CameraType::V4L2CAM, int id=0);
+  void open(CameraType type=CameraType::WEBCAM, int id=0);
   bool read(cv::Mat& image);
   void release();
   
@@ -347,14 +343,10 @@ public:
   double getSaturation();
   double getHue();
   double getExposure();
-  std::string getFourcc();
   bool isRunning();
-  void setFourcc(std::string fourcc);
   void setWH(int width, int height);
   void setFps(int fps);
   void setIp(std::string ip);
-  void setRtspUrl(std::string rtsp_url);
-  void setVideoPath(std::string video_path);
   void setPort(int port);
   void setBrightness(double brightness);
   void setContrast(double contrast);
@@ -377,15 +369,12 @@ protected:
   int _height;
   int _fps;
   std::string _ip;
-  std::string _rtsp_url;
-  std::string _video_path;
   int _port;
   double _brightness;
   double _contrast;
   double _saturation;
   double _hue;
   double _exposure;
-  std::string _fourcc;
 };
 
 
@@ -401,16 +390,6 @@ void drawTargetsInFrame(
   bool with_aruco=false,
   bool with_yaw=false
 );
-cv::Mat drawTargetsInFrame(
-  const cv::Mat img_,
-  const TargetsInFrame& tgts_,
-  const double scale,
-  bool with_all=true,
-  bool with_category=false,
-  bool with_tid=false,
-  bool with_seg=false,
-  bool with_box=false
-);
 std::string get_home();
 bool is_file_exist(std::string& fn);
 void list_dir(std::string dir, std::vector<std::string>& files, std::string suffixs="", bool r=false);

include/sv_world.h

@@ -7,7 +7,6 @@
 #include "sv_sot.h"
 #include "sv_mot.h"
 #include "sv_color_line.h"
-#include "sv_veri_det.h"
 #include "sv_video_input.h"
 #include "sv_video_output.h"
 

params/a-params/sv_algorithm_params.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 640,
-        "withSegmentation": true,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": true,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["h"],

params/a-params/sv_algorithm_params_1280_wo_mask.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 1280,
-        "withSegmentation": false,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": false,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

params/a-params/sv_algorithm_params_640_w_mask.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 640,
-        "withSegmentation": true,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": true,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -125,9 +123,8 @@
         "reid_num_samples": 10,
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
-        "min_hits": 3,
+        "max_age": 10,
-        "frame_rate": 30,
+        "min_hits": 3
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

params/a-params/sv_algorithm_params_640_wo_mask.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 640,
-        "withSegmentation": false,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": false,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

params/a-params/sv_algorithm_params_coco_1280.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 1280,
-        "withSegmentation": false,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": false,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

params/a-params/sv_algorithm_params_coco_640.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 640,
-        "withSegmentation": false,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": true,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

params/a-params/sv_algorithm_params_csrt.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 640,
-        "withSegmentation": true,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": true,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

params/a-params/sv_algorithm_params_kcf.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 640,
-        "withSegmentation": true,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": true,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

params/a-params/sv_algorithm_params_nano.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 640,
-        "withSegmentation": true,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": true,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

params/a-params/sv_algorithm_params_siamrpn.json

@@ -1,13 +1,11 @@
 {
     "CommonObjectDetector": {
         "dataset": "COCO",
-        "model": "s",
-        "batchSize": 1,
         "inputSize": 640,
-        "withSegmentation": true,
         "nmsThrs": 0.6,
         "scoreThrs": 0.4,
         "useWidthOrHeight": 1,
+        "withSegmentation": true,
         "datasetPersonVehicle": {
             "person": [0.5, 1.8],
             "car": [4.1, 1.5],
@@ -126,9 +124,7 @@
         "reid_match_thres": 2.0,
         "iou_thres": 0.5,
         "max_age": 10,
-        "min_hits": 3,
+        "min_hits": 3
-        "frame_rate": 30,
-        "track_buffer": 30
     },
     "LandingMarkerDetector": {
         "labels": ["x", "h"],

samples/SpireCVDet.cpp

@@ -18,9 +18,9 @@ const static int kInputH_HD = 1280;
 const static int kInputW_HD = 1280;
 const static int kOutputSize = kMaxNumOutputBbox * sizeof(Detection) / sizeof(float) + 1;
 
-bool parse_args(int argc, char** argv, std::string& wts, std::string& engine, bool& is_p6, float& gd, float& gw, std::string& img_dir, int& n_classes, int& n_batch) {
+bool parse_args(int argc, char** argv, std::string& wts, std::string& engine, bool& is_p6, float& gd, float& gw, std::string& img_dir, int& n_classes) {
   if (argc < 4) return false;
-  if (std::string(argv[1]) == "-s" && (argc == 6 || argc == 7 || argc == 8)) {
+  if (std::string(argv[1]) == "-s" && (argc == 6 || argc == 8)) {
     wts = std::string(argv[2]);
     engine = std::string(argv[3]);
     n_classes = atoi(argv[4]);
@@ -51,9 +51,6 @@ bool parse_args(int argc, char** argv, std::string& wts, std::string& engine, bo
     if (net.size() == 2 && net[1] == '6') {
       is_p6 = true;
     }
-    if (argc == 7) {
-      n_batch = atoi(argv[6]);
-    }
   } else {
     return false;
   }
@@ -102,20 +99,18 @@ int main(int argc, char** argv) {
   float gd = 0.0f, gw = 0.0f;
   std::string img_dir;
   int n_classes;
-  int n_batch = 1;
 
-  if (!parse_args(argc, argv, wts_name, engine_name, is_p6, gd, gw, img_dir, n_classes, n_batch)) {
+  if (!parse_args(argc, argv, wts_name, engine_name, is_p6, gd, gw, img_dir, n_classes)) {
     std::cerr << "arguments not right!" << std::endl;
     std::cerr << "./SpireCVDet -s [.wts] [.engine] n_classes [n/s/m/l/x/n6/s6/m6/l6/x6 or c/c6 gd gw]  // serialize model to plan file" << std::endl;
     // std::cerr << "./SpireCVDet -d [.engine] ../images  // deserialize plan file and run inference" << std::endl;
     return -1;
   }
   std::cout << "n_classes: " << n_classes << std::endl;
-  std::cout << "max_batch: " << n_batch << std::endl;
 
   // Create a model using the API directly and serialize it to a file
   if (!wts_name.empty()) {
-    serialize_engine(n_batch, is_p6, gd, gw, wts_name, engine_name, n_classes);
+    serialize_engine(kBatchSize, is_p6, gd, gw, wts_name, engine_name, n_classes);
     return 0;
   }
 

samples/SpireCVSeg.cpp

@@ -18,47 +18,44 @@ const static int kInputW = 640;
 const static int kOutputSize1 = kMaxNumOutputBbox * sizeof(Detection) / sizeof(float) + 1;
 const static int kOutputSize2 = 32 * (kInputH / 4) * (kInputW / 4);
 
-bool parse_args(int argc, char** argv, std::string& wts, std::string& engine, float& gd, float& gw, std::string& img_dir, std::string& labels_filename, int& n_classes, int& n_batch) {
+bool parse_args(int argc, char** argv, std::string& wts, std::string& engine, float& gd, float& gw, std::string& img_dir, std::string& labels_filename, int& n_classes) {
-  if (argc < 4) return false;
+    if (argc < 4) return false;
-  if (std::string(argv[1]) == "-s" && (argc == 6 || argc == 7 || argc == 8)) {
+    if (std::string(argv[1]) == "-s" && (argc == 6 || argc == 8)) {
-    wts = std::string(argv[2]);
+        wts = std::string(argv[2]);
-    engine = std::string(argv[3]);
+        engine = std::string(argv[3]);
-    n_classes = atoi(argv[4]);
+        n_classes = atoi(argv[4]);
-    if (n_classes < 1)
+        if (n_classes < 1)
-      return false;
+          return false;
-    auto net = std::string(argv[5]);
+        auto net = std::string(argv[5]);
-    if (net[0] == 'n') {
+        if (net[0] == 'n') {
-      gd = 0.33;
+            gd = 0.33;
-      gw = 0.25;
+            gw = 0.25;
-    } else if (net[0] == 's') {
+        } else if (net[0] == 's') {
-      gd = 0.33;
+            gd = 0.33;
-      gw = 0.50;
+            gw = 0.50;
-    } else if (net[0] == 'm') {
+        } else if (net[0] == 'm') {
-      gd = 0.67;
+            gd = 0.67;
-      gw = 0.75;
+            gw = 0.75;
-    } else if (net[0] == 'l') {
+        } else if (net[0] == 'l') {
-      gd = 1.0;
+            gd = 1.0;
-      gw = 1.0;
+            gw = 1.0;
-    } else if (net[0] == 'x') {
+        } else if (net[0] == 'x') {
-      gd = 1.33;
+            gd = 1.33;
-      gw = 1.25;
+            gw = 1.25;
-    } else if (net[0] == 'c' && argc == 8) {
+        } else if (net[0] == 'c' && argc == 8) {
-      gd = atof(argv[6]);
+            gd = atof(argv[6]);
-      gw = atof(argv[7]);
+            gw = atof(argv[7]);
+        } else {
+            return false;
+        }
+    } else if (std::string(argv[1]) == "-d" && argc == 5) {
+        engine = std::string(argv[2]);
+        img_dir = std::string(argv[3]);
+        labels_filename = std::string(argv[4]);
     } else {
-      return false;
+        return false;
     }
-    if (argc == 7) {
+    return true;
-      n_batch = atoi(argv[6]);
-    }
-  } else if (std::string(argv[1]) == "-d" && argc == 5) {
-    engine = std::string(argv[2]);
-    img_dir = std::string(argv[3]);
-    labels_filename = std::string(argv[4]);
-  } else {
-    return false;
-  }
-  return true;
 }
 
 
@@ -101,21 +98,19 @@ int main(int argc, char** argv) {
   std::string labels_filename = "";
   float gd = 0.0f, gw = 0.0f;
   int n_classes;
-  int n_batch = 1;
 
   std::string img_dir;
-  if (!parse_args(argc, argv, wts_name, engine_name, gd, gw, img_dir, labels_filename, n_classes, n_batch)) {
+  if (!parse_args(argc, argv, wts_name, engine_name, gd, gw, img_dir, labels_filename, n_classes)) {
     std::cerr << "arguments not right!" << std::endl;
     std::cerr << "./SpireCVSeg -s [.wts] [.engine] n_classes [n/s/m/l/x or c gd gw]  // serialize model to plan file" << std::endl;
     // std::cerr << "./SpireCVSeg -d [.engine] ../images coco.txt  // deserialize plan file, read the labels file and run inference" << std::endl;
     return -1;
   }
   std::cout << "n_classes: " << n_classes << std::endl;
-  std::cout << "max_batch: " << n_batch << std::endl;
 
   // Create a model using the API directly and serialize it to a file
   if (!wts_name.empty()) {
-    serialize_engine(n_batch, gd, gw, wts_name, engine_name, n_classes);
+    serialize_engine(kBatchSize, gd, gw, wts_name, engine_name, n_classes);
     return 0;
   }
 

samples/calib/calibrate_camera_charuco.cpp

@@ -36,7 +36,6 @@ the use of this software, even if advised of the possibility of such damage.
 #include <opencv2/imgproc.hpp>
 #include <vector>
 #include <iostream>
-#include <sv_world.h>
 #include "aruco_samples_utility.hpp"
 
 using namespace std;
@@ -59,10 +58,7 @@ const char* keys  =
         "DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16}"
         "{cd       |       | Input file with custom dictionary }"
         "{@outfile |<none> | Output file with calibrated camera parameters }"
-        "{imh      |       | Camera Image Height }"
+        "{v        |       | Input from video file, if ommited, input comes from camera }"
-        "{imw      |       | Camera Image Width }"
-        "{fps      |       | Camera FPS }"
-        "{tp       |       | 1:WEBCAM, 2:V4L2CAM, 3:G1, 4:Q10, 5:MIPI, 6:GX40 }"
         "{ci       | 0     | Camera id if input doesnt come from video (-v) }"
         "{dp       |       | File of marker detector parameters }"
         "{rs       | false | Apply refind strategy }"
@@ -111,47 +107,31 @@ int main(int argc, char *argv[]) {
 
     bool refindStrategy = parser.get<bool>("rs");
     int camId = parser.get<int>("ci");
-    int imW = 800;
+    String video;
-    int imH = 600;
-    int fps = 30;
-    int tp = 1;
 
-    if(parser.has("imh")) {
+    if(parser.has("v")) {
-        imH = parser.get<int>("imh");
+        video = parser.get<String>("v");
     }
-    if(parser.has("imw")) {
-        imW = parser.get<int>("imw");
-    }
-    if(parser.has("fps")) {
-        fps = parser.get<int>("fps");
-    }
-    if(parser.has("tp")) {
-        tp = parser.get<int>("tp");
-    }
-    sv::CameraType c_type = sv::CameraType::WEBCAM;
-    if (2 == tp)
-      c_type = sv::CameraType::V4L2CAM;
-    else if (3 == tp)
-      c_type = sv::CameraType::G1;
-    else if (4 == tp)
-      c_type = sv::CameraType::Q10;
-    else if (5 == tp)
-      c_type = sv::CameraType::MIPI;
-    else if (6 == tp)
-      c_type = sv::CameraType::GX40;
 
     if(!parser.check()) {
         parser.printErrors();
         return 0;
     }
 
-    // VideoCapture inputVideo;
+    VideoCapture inputVideo;
-    sv::Camera inputVideo;
+   
-    inputVideo.setWH(imW, imH);
+   
-    inputVideo.setFps(fps);
+    int waitTime;
-    inputVideo.open(c_type, camId);
+    if(!video.empty()) {
+        inputVideo.open(video);
+        waitTime = 0;
+    } else {
+        inputVideo.open(camId);
+        waitTime = 10;
+    }
+    
   
-    int waitTime = 10;
+    
 
     aruco::Dictionary dictionary = aruco::getPredefinedDictionary(0);
     if (parser.has("d")) {
@@ -181,9 +161,11 @@ int main(int argc, char *argv[]) {
     vector< Mat > allImgs;
     Size imgSize;
 
-    while(1) {
+    while(inputVideo.grab()) {
         Mat image, imageCopy;
-        inputVideo.read(image);
+        inputVideo.retrieve(image);
+        cv::resize(image, image, cv::Size(640, 480));
+       
 
         vector< int > ids;
         vector< vector< Point2f > > corners, rejected;

samples/calib/create_marker.cpp

@@ -1,256 +0,0 @@
-#include <opencv2/highgui.hpp>
-#include <opencv2/objdetect/aruco_detector.hpp>
-#include <iostream>
-#include "aruco_samples_utility.hpp"
-
-using namespace cv;
-
-namespace {
-const char* about = "Create an ArUco marker image";
-
-//! [aruco_create_markers_keys]
-const char* keys  =
-    "{@outfile |<none> | Output image }"
-    "{d        |       | dictionary: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2,"
-    "DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, "
-    "DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12,"
-    "DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16}"
-    "{cd       |       | Input file with custom dictionary }"
-    "{id       |       | Marker id in the dictionary }"
-    "{ms       | 200   | Marker size in pixels }"
-    "{bs       | 50    | Border size in pixels }"
-    "{lp       | 50    | Landing Pad Unit in pixels }"
-    "{bb       | 1     | Number of bits in marker borders }"
-    "{si       | false | show generated image }";
-}
-//! [aruco_create_markers_keys]
-
-
-Mat create_marker_with_borders(aruco::Dictionary& dictionary, int markerId, int markerSize, int borderBits, int borderSize) {
-  Mat tmpImg;
-  aruco::generateImageMarker(dictionary, markerId, markerSize, tmpImg, borderBits);
-  Mat tmpImgCopy = Mat::ones(borderSize * 2 + markerSize, borderSize * 2 + markerSize, CV_8UC1) * 255;
-  tmpImg.copyTo(tmpImgCopy(Rect(borderSize, borderSize, markerSize, markerSize)));
-  tmpImg = tmpImgCopy;
-  return tmpImg;
-}
-
-
-int main(int argc, char *argv[]) {
-  CommandLineParser parser(argc, argv, keys);
-  parser.about(about);
-
-  if(argc < 4) {
-    parser.printMessage();
-    return 0;
-  }
-
-  int markerId = parser.get<int>("id");
-  int borderBits = parser.get<int>("bb");
-  int markerSize = parser.get<int>("ms");
-  bool showImage = parser.get<bool>("si");
-
-  int borderSize = 0;
-  if (parser.has("bs")) {
-    borderSize = parser.get<int>("bs");
-  }
-  int landingPadUnit = 0;
-  if (parser.has("lp")) {
-    landingPadUnit = parser.get<int>("lp");
-    borderSize = landingPadUnit;
-    borderBits = 1;
-    markerSize = landingPadUnit * 4;
-  }
-
-  String out = parser.get<String>(0);
-
-  if(!parser.check()) {
-    parser.printErrors();
-    return 0;
-  }
-
-  aruco::Dictionary dictionary = aruco::getPredefinedDictionary(0);
-  if (parser.has("d")) {
-    int dictionaryId = parser.get<int>("d");
-    dictionary = aruco::getPredefinedDictionary(aruco::PredefinedDictionaryType(dictionaryId));
-  }
-  else if (parser.has("cd")) {
-    FileStorage fs(parser.get<std::string>("cd"), FileStorage::READ);
-    bool readOk = dictionary.aruco::Dictionary::readDictionary(fs.root());
-    if(!readOk) {
-      std::cerr << "Invalid dictionary file" << std::endl;
-      return 0;
-    }
-  }
-  else {
-    std::cerr << "Dictionary not specified" << std::endl;
-    return 0;
-  }
-
-  Mat markerImg;
-  aruco::generateImageMarker(dictionary, markerId, markerSize, markerImg, borderBits);
-  if (borderSize > 0) {
-    Mat markerImgCopy = Mat::ones(borderSize * 2 + markerSize, borderSize * 2 + markerSize, CV_8UC1) * 255;
-    markerImg.copyTo(markerImgCopy(Rect(borderSize, borderSize, markerSize, markerSize)));
-    markerImg = markerImgCopy;
-  }
-
-  if (landingPadUnit > 0) {
-    Mat markerImgBIG = Mat::ones(landingPadUnit * 62, landingPadUnit * 62, CV_8UC1) * 255;
-    // markerId = 0;
-    markerId --;
-    markerSize = landingPadUnit * 4;
-    borderSize = landingPadUnit;
-    int newSize = markerSize + borderSize * 2;
-
-    for (int i=0; i<3; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(i * landingPadUnit * 5, 0, newSize, newSize)));
-    }
-    for (int i=0; i<5; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 18 + i * landingPadUnit * 5, 0, newSize, newSize)));
-    }
-    for (int i=0; i<3; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 46 + i * landingPadUnit * 5, 0, newSize, newSize)));
-    }
-    
-    for (int i=0; i<2; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 56, landingPadUnit * 5 + i * landingPadUnit * 5, newSize, newSize)));
-    }
-    for (int i=0; i<5; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 56, landingPadUnit * 18 + i * landingPadUnit * 5, newSize, newSize)));
-    }
-    for (int i=0; i<3; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 56, landingPadUnit * 46 + i * landingPadUnit * 5, newSize, newSize)));
-    }
-    
-    for (int i=0; i<2; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 51 - i * landingPadUnit * 5, landingPadUnit * 56, newSize, newSize)));
-    }
-    for (int i=0; i<5; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 38 - i * landingPadUnit * 5, landingPadUnit * 56, newSize, newSize)));
-    }
-    for (int i=0; i<3; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 10 - i * landingPadUnit * 5, landingPadUnit * 56, newSize, newSize)));
-    }
-    
-    for (int i=0; i<2; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(0, landingPadUnit * 51 - i * landingPadUnit * 5, newSize, newSize)));
-    }
-    for (int i=0; i<5; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(0, landingPadUnit * 38 - i * landingPadUnit * 5, newSize, newSize)));
-    }
-    for (int i=0; i<2; i++) {
-      markerId += 1;
-      markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-      markerImg.copyTo(markerImgBIG(Rect(0, landingPadUnit * 10 - i * landingPadUnit * 5, newSize, newSize)));
-    }
-    
-    
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 28, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 23, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 33, landingPadUnit * 28, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 33, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 23, landingPadUnit * 28, newSize, newSize)));
-    
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 18, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 38, landingPadUnit * 28, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 38, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 18, landingPadUnit * 28, newSize, newSize)));
-    
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 5, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 51, landingPadUnit * 28, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 51, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 5, landingPadUnit * 28, newSize, newSize)));
-    
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 10, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 46, landingPadUnit * 28, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 28, landingPadUnit * 46, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 10, landingPadUnit * 28, newSize, newSize)));
-    
-    // markerId = 90;
-    markerSize = landingPadUnit * 4 * 4;
-    borderSize = landingPadUnit * 4;
-    newSize = markerSize + borderSize * 2;
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 5, landingPadUnit * 5, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 5 + newSize + landingPadUnit * 4, landingPadUnit * 5, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 5, landingPadUnit * 5 + newSize + landingPadUnit * 4, newSize, newSize)));
-    markerId += 1;
-    markerImg = create_marker_with_borders(dictionary, markerId, markerSize, borderBits, borderSize);
-    markerImg.copyTo(markerImgBIG(Rect(landingPadUnit * 5 + newSize + landingPadUnit * 4, landingPadUnit * 5 + newSize + landingPadUnit * 4, newSize, newSize)));
-
-    markerImg = Mat::ones(landingPadUnit * 64, landingPadUnit * 64, CV_8UC1) * 255;
-    markerImgBIG.copyTo(markerImg(Rect(landingPadUnit, landingPadUnit, landingPadUnit * 62, landingPadUnit * 62)));
-  }
-
-  if(showImage) {
-    imshow("marker", markerImg);
-    waitKey(0);
-  }
-
-  imwrite(out, markerImg);
-
-  return 0;
-}
-