add communications with realsense.

WEIIEW97 · Nov 13, 2023 · 7d560b2 · 7d560b2
1 parent d161a26
commit 7d560b2
Show file tree

Hide file tree

Showing 13 changed files with 1,768 additions and 39 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -10,6 +10,8 @@ file(GLOB headers ${CMAKE_CURRENT_LIST_DIR}/src/*.h)
 find_package(OpenCV REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(realsense2 REQUIRED)
+find_package(OpenGL REQUIRED)
+find_package(glfw3 REQUIRED)
 
 
 add_executable(blind_assist main.cpp
@@ -21,10 +23,13 @@ target_include_directories(
         ${realsense2_INCLUDE_DIR}
         ${OpenCV_INCLUDE_DIRS}
         ${EIGEN3_INCLUDE_DIR}
+        ${OPENGL_INCLUDE_DIRS}
 )
 
 target_link_libraries(
         ${PROJECT_NAME}
         ${realsense2_LIBRARY}
         ${OpenCV_LIBS}
+        ${OPENGL_LIBRARIES}
+        glfw
 )
diff --git a/main.cpp b/main.cpp
@@ -1,4 +1,5 @@
 #include "src/calculate_angles.h"
+#include "src/rscapture.h"
 #include <filesystem>
 #include <vector>
 using namespace aruconavi;
@@ -17,19 +18,24 @@ int main() {
 //    string full_nanme = img_dir + "/" + name;
 //
 //  }
-  string img_path =
-      "/Users/williamwei/Codes/aruco-navi/aruco_capture/frame_0139.png";
-  Calc calculator(intrinsics640480, distortion);
-  cv::Mat image = cv::imread(img_path, cv::IMREAD_ANYCOLOR);
-  calculator.process(image);
-  cout << calculator.ypr << endl;
-  cout << calculator.trans_cam2world << endl;
-
-  cv::Mat image_copy;
-  image.copyTo(image_copy);
-  draw_coordinate_system(image_copy, calculator.camera_matrix_, calculator.dist_coef_, calculator.rvec_, calculator.tvec_);
-  cv::imshow("detected coordinate system", image_copy);
-  cv::waitKey(0);
-  cv::destroyAllWindows();
+//  string img_path = "/home/william/Codes/aruco-navi/aruco_capture/frame_0139.png";
+//  Calc calculator(INTRINSICS640480, DISTORTION);
+//  cv::Mat image = cv::imread(img_path, cv::IMREAD_ANYCOLOR);
+//  calculator.process(image);
+//  cout << calculator.ypr << endl;
+//  cout << calculator.trans_cam2world << endl;
+//
+//  cv::Mat image_copy;
+//  image.copyTo(image_copy);
+//  draw_coordinate_system(image_copy, calculator.camera_matrix_, calculator.dist_coef_, calculator.rvec_, calculator.tvec_);
+//  cv::imshow("detected coordinate system", image_copy);
+//  cv::waitKey(0);
+//  cv::destroyAllWindows();
+  bool status = realsense_capture(1280, 720);
+  if (status) {
+    cout << "launch succeed!" << endl;
+  } else {
+    cout << "launch failed!" << endl;
+  }
   return 0;
 }
diff --git a/src/calculate_angles.cpp b/src/calculate_angles.cpp
@@ -18,17 +18,6 @@
 #include <iostream>
 
 namespace aruconavi {
-  void draw_coordinate_system(cv::Mat& image,
-                              const Eigen::Matrix3f& intrinsic_matrix,
-                              const Eigen::Vector<float, 5>& dist_coef,
-                              const cv::Vec3d& rvec, const cv::Vec3d& tvec) {
-    cv::Mat intrinsic_matrix_cv, dist_coef_cv;
-    cv::eigen2cv(intrinsic_matrix, intrinsic_matrix_cv);
-    cv::eigen2cv(dist_coef, dist_coef_cv);
-    cv::drawFrameAxes(image, intrinsic_matrix_cv, dist_coef_cv, rvec, tvec,
-                      0.1);
-  }
-
   void Calc::get_rvecs_and_tvecs(
       const std::vector<std::vector<cv::Point2f>>& corners, float marker_len) {
     cv::Mat intrinsic_matrix_cv, dist_coef_cv;
@@ -59,9 +48,16 @@ namespace aruconavi {
   void Calc::process(cv::Mat& image) {
     std::pair<std::vector<int>, std::vector<std::vector<cv::Point2f>>> pairs;
     pairs = detect_markers(image, ArucoDict);
-
+    pairs_ = pairs;
     int chosen_id = find_min_id(pairs);
-    this->get_rvecs_and_tvecs(pairs.second, MarkerLen);
-    this->get_ypr_and_translation(chosen_id);
+    if (chosen_id >= 0) {
+      this->get_rvecs_and_tvecs(pairs.second, MarkerLen);
+      this->get_ypr_and_translation(chosen_id);
+      draw_ready_ = true;
+    } else {
+      trans_cam2world = {0, 0, 0};
+      ypr = {0, 0, 0};
+      draw_ready_ = false;
+    }
   }
 } // namespace aruconavi
diff --git a/src/calculate_angles.h b/src/calculate_angles.h
@@ -29,9 +29,12 @@ namespace aruconavi {
     Eigen::Vector<float, 5> dist_coef_;
     std::vector<cv::Vec3d> rvecs_, tvecs_;
     cv::Vec3d rvec_, tvec_;
+    bool draw_ready_;
+    std::pair<std::vector<int>, std::vector<std::vector<cv::Point2f>>> pairs_;
 
   public:
-    Calc(IntelRealsenseIntrinsics640_480 intrinsic, Eigen::Vector<float, 5> dist_coef) {
+    Calc(IntelRealsenseIntrinsics640_480 intrinsic,
+         Eigen::Vector<float, 5> dist_coef) {
       camera_matrix_ = build_intrinsic(intrinsic);
       dist_coef_ = std::move(dist_coef);
       intrinsic_ = intrinsic;
@@ -46,13 +49,7 @@ namespace aruconavi {
     get_rvecs_and_tvecs(const std::vector<std::vector<cv::Point2f>>& corners,
                         float marker_len);
     void get_ypr_and_translation(int id);
-
-
   };
 
-  void draw_coordinate_system(cv::Mat& image,
-                              const Eigen::Matrix3f& intrinsic_matrix,
-                              const Eigen::Vector<float, 5>& dist_coef,
-                              const cv::Vec3d& rvec, const cv::Vec3d& tvec);
 } // namespace aruconavi
 #endif // BLIND_ASSIST_CALCULATE_ANGLES_H
diff --git a/src/constant.h b/src/constant.h
@@ -27,9 +27,9 @@ namespace aruconavi {
     float cy = 249.345;
   };
 
-  static IntelRealsenseIntrinsics640_480 intrinsics640480;
+  static IntelRealsenseIntrinsics640_480 INTRINSICS640480;
 
-  static Eigen::Vector<float, 5> distortion = {0, 0, 0, 0, 0};
+  static Eigen::Vector<float, 5> DISTORTION = {0, 0, 0, 0, 0};
 
   const cv::aruco::PREDEFINED_DICTIONARY_NAME ArucoDict =
       cv::aruco::DICT_6X6_250;

diff --git a/src/drawfunc.cpp b/src/drawfunc.cpp
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2022-2023, William Wei. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "drawfunc.h"
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/aruco.hpp>
+#include <vector>
+
+namespace aruconavi {
+  void Painter::draw_coordinate_system(const Eigen::Matrix3f& intrinsic_matrix,
+                                       const Eigen::Vector<float, 5>& dist_coef,
+                                       const cv::Vec3d& rvec,
+                                       const cv::Vec3d& tvec) {
+    cv::Mat intrinsic_matrix_cv, dist_coef_cv;
+    cv::eigen2cv(intrinsic_matrix, intrinsic_matrix_cv);
+    cv::eigen2cv(dist_coef, dist_coef_cv);
+    cv::drawFrameAxes(image_, intrinsic_matrix_cv, dist_coef_cv, rvec, tvec,
+                      0.1);
+  }
+
+  void Painter::draw_detected_markers(
+      std::pair<std::vector<int>, std::vector<std::vector<cv::Point2f>>>
+          pairs) {
+    std::vector<int> marked_ids = pairs.first;
+    std::vector<std::vector<cv::Point2f>> marked_corners = pairs.second;
+    if (marked_ids.size() > 0) {
+      cv::aruco::drawDetectedMarkers(image_, marked_corners, marked_ids);
+    }
+  }
+
+} // namespace aruconavi
diff --git a/src/drawfunc.h b/src/drawfunc.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2022-2023, William Wei. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef BLIND_ASSIST_DRAWFUNC_H
+#define BLIND_ASSIST_DRAWFUNC_H
+#include <opencv2/opencv.hpp>
+#include <Eigen/Dense>
+
+namespace aruconavi {
+  void draw_coordinate_system(cv::Mat& image,
+                              const Eigen::Matrix3f& intrinsic_matrix,
+                              const Eigen::Vector<float, 5>& dist_coef,
+                              const cv::Vec3d& rvec, const cv::Vec3d& tvec);
+
+  class Painter {
+  public:
+    cv::Mat image_;
+
+  public:
+    Painter(cv::Mat image) : image_(image) {}
+    // for coordinate axis, we only draw the nearest one(i.e. the smallest id)
+    void draw_coordinate_system(const Eigen::Matrix3f& intrinsic_matrix,
+                                const Eigen::Vector<float, 5>& dist_coef,
+                                const cv::Vec3d& rvec, const cv::Vec3d& tvec);
+    void draw_detected_markers(
+        std::pair<std::vector<int>, std::vector<std::vector<cv::Point2f>>>
+            pairs);
+    void draw_text();
+    ~Painter() = default;
+  };
+} // namespace aruconavi
+#endif // BLIND_ASSIST_DRAWFUNC_H