fixed bugs

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

Hide file tree

Showing 6 changed files with 72 additions and 54 deletions.
diff --git a/main.cpp b/main.cpp
@@ -1,5 +1,6 @@
 #include "src/calculate_angles.h"
 #include "src/rscapture.h"
+#include "src/rscvcapture.h"
 #include <filesystem>
 #include <vector>
 using namespace aruconavi;
@@ -31,7 +32,8 @@ int main() {
 //  cv::imshow("detected coordinate system", image_copy);
 //  cv::waitKey(0);
 //  cv::destroyAllWindows();
-  bool status = realsense_capture(1280, 720);
+  bool status = rs_with_cv_capture(1280, 720);
+//  bool status = realsense_capture(1280, 720);
   if (status) {
     cout << "launch succeed!" << endl;
   } else {

diff --git a/src/drawfunc.h b/src/drawfunc.h
@@ -38,7 +38,7 @@ namespace aruconavi {
     void draw_detected_markers(
         std::pair<std::vector<int>, std::vector<std::vector<cv::Point2f>>>
             pairs);
-    void draw_text();
+    //    void draw_text();
     ~Painter() = default;
   };
 } // namespace aruconavi

diff --git a/src/rscapture.cpp b/src/rscapture.cpp
@@ -16,30 +16,33 @@
 #include "rscapture.h"
 #include "realsenssutil.h"
 
-bool realsense_capture(int window_width, int window_height) {
-  try {
-    rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
-    window app(1280, 720, "realsense capture example");
+namespace aruconavi {
+  bool rs_capture(int window_width, int window_height) {
+    try {
+      rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
+      window app(1280, 720, "realsense capture example");
 
-    rs2::colorizer color_map;
-    rs2::rates_printer printer;
+      rs2::colorizer color_map;
+      rs2::rates_printer printer;
 
-    rs2::pipeline pipe;
+      rs2::pipeline pipe;
 
-    pipe.start();
+      pipe.start();
 
-    while (app) {
-      rs2::frameset data =
-          pipe.wait_for_frames().apply_filter(printer).apply_filter(color_map);
-      app.show(data);
+      while (app) {
+        rs2::frameset data =
+            pipe.wait_for_frames().apply_filter(printer).apply_filter(
+                color_map);
+        app.show(data);
+      }
+      return true;
+    } catch (const rs2::error& e) {
+      std::cerr << "RealSense error calling " << e.get_failed_function() << "("
+                << e.get_failed_args() << "):\n    " << e.what() << std::endl;
+      return false;
+    } catch (const std::exception& e) {
+      std::cerr << e.what() << std::endl;
+      return false;
     }
-    return true;
-  } catch (const rs2::error& e) {
-    std::cerr << "RealSense error calling " << e.get_failed_function() << "("
-              << e.get_failed_args() << "):\n    " << e.what() << std::endl;
-    return false;
-  } catch (const std::exception& e) {
-    std::cerr << e.what() << std::endl;
-    return false;
   }
 }
diff --git a/src/rscapture.h b/src/rscapture.h
@@ -18,5 +18,7 @@
 #define BLIND_ASSIST_RSCAPTURE_H
 
 #include <librealsense2/rs.hpp>
-bool realsense_capture(int window_width, int window_height);
+namespace aruconavi {
+  bool rs_capture(int window_width, int window_height);
+}
 #endif // BLIND_ASSIST_RSCAPTURE_H
diff --git a/src/rscvcapture.cpp b/src/rscvcapture.cpp
@@ -15,40 +15,49 @@
  */
 #include "rscvcapture.h"
 #include "calculate_angles.h"
+#include "drawfunc.h"
 
-bool rs_with_cv_capture(int win_width, int win_height) {
-  try {
-    rs2::colorizer color_map;
-    rs2::pipeline pipe;
-    pipe.start();
+namespace aruconavi {
+  bool rs_with_cv_capture(int win_width, int win_height) {
+    try {
+      rs2::pipeline pipe;
+      pipe.start();
 
-    using namespace cv;
-    const auto win_name = "display image";
-    namedWindow(win_name, WINDOW_AUTOSIZE);
-    aruconavi::Calc calculator(aruconavi::INTRINSICS640480,
-                               aruconavi::DISTORTION);
-    Mat image_out;
-    while (waitKey(1) < 0 &&
-           getWindowProperty(win_name, WND_PROP_AUTOSIZE) >= 0) {
-      rs2::frameset data =
-          pipe.wait_for_frames(); // wait for next set of frames from the camera
-      rs2::frameset rgb = data.get_color_frame();
+      using namespace cv;
+      const auto win_name = "display image";
+      namedWindow(win_name, WINDOW_AUTOSIZE);
+      aruconavi::Calc calculator(aruconavi::INTRINSICS640480,
+                                 aruconavi::DISTORTION);
+      Mat image_out;
+      while (waitKey(1) < 0 &&
+             getWindowProperty(win_name, WND_PROP_AUTOSIZE) >= 0) {
+        rs2::frameset data = pipe.wait_for_frames(); // wait for next set of frames from the camera
+        auto rgb = data.get_color_frame();
 
-      // query frame size (width and height)
-      const int w = rgb.as<rs2::video_frame>().get_width();
-      const int h = rgb.as<rs2::video_frame>().get_height();
+        // query frame size (width and height)
+        const int w = rgb.as<rs2::video_frame>().get_width();
+        const int h = rgb.as<rs2::video_frame>().get_height();
 
-      Mat image(Size(w, h), CV_8UC3, (void*)rgb.get_data(), Mat::AUTO_STEP);
-      calculator.process(image);
-      image.copyTo(image_out);
+        Mat image(Size(w, h), CV_8UC3, (void*)rgb.get_data(), Mat::AUTO_STEP);
+        calculator.process(image);
+        cv::cvtColor(image, image_out, cv::COLOR_BGR2RGB);
+        if (calculator.draw_ready_) {
+          aruconavi::Painter painter(image_out);
+          painter.draw_detected_markers(calculator.pairs_);
+          painter.draw_coordinate_system(calculator.camera_matrix_,
+                                         calculator.dist_coef_,
+                                         calculator.rvec_, calculator.tvec_);
+        }
+        imshow(win_name, image_out);
+      }
+      return true;
+    } catch (const rs2::error& e) {
+      std::cerr << "RealSense error calling " << e.get_failed_function() << "("
+                << e.get_failed_args() << "):\n    " << e.what() << std::endl;
+      return false;
+    } catch (const std::exception& e) {
+      std::cerr << e.what() << std::endl;
+      return false;
     }
-    return true;
-  } catch (const rs2::error& e) {
-    std::cerr << "RealSense error calling " << e.get_failed_function() << "("
-              << e.get_failed_args() << "):\n    " << e.what() << std::endl;
-    return false;
-  } catch (const std::exception& e) {
-    std::cerr << e.what() << std::endl;
-    return false;
   }
 }
diff --git a/src/rscvcapture.h b/src/rscvcapture.h
@@ -19,5 +19,7 @@
 
 #include <librealsense2/rs.hpp>
 #include <opencv2/opencv.hpp>
-
+namespace aruconavi {
+  bool rs_with_cv_capture(int win_width, int win_height);
+}
 #endif // BLIND_ASSIST_MULTIFRAMES_H