Add get stream data tutorials

samples/tutorials/CMakeLists.txt

@@ -1,5 +1,9 @@
 get_filename_component(DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME)
 
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
 set_outdir(
   "${OUT_DIR}/lib/${DIR_NAME}"
   "${OUT_DIR}/lib/${DIR_NAME}"
@@ -8,9 +12,10 @@ set_outdir(
 
 # make_executable(NAME
 #                 [SRCS src1 src2 ...])
-#                 [WITH_OPENCV])
+#                 [WITH_OPENCV]
+#                 [WITH_PCL])
 macro(make_executable NAME)
-  set(options WITH_OPENCV)
+  set(options WITH_OPENCV WITH_PCL)
   set(oneValueArgs)
   set(multiValueArgs SRCS)
   cmake_parse_arguments(THIS "${options}" "${oneValueArgs}"
@@ -22,11 +27,21 @@ macro(make_executable NAME)
     list(APPEND __link_libs ${OpenCV_LIBS})
     list(APPEND __dll_search_paths ${OpenCV_LIB_SEARCH_PATH})
   endif()
+  if(THIS_WITH_PCL)
+    list(APPEND __link_libs ${PCL_LIBRARIES})
+    #list(APPEND __link_libs pcl::pcl)
+    list(APPEND __dll_search_paths ${PCL_LIB_SEARCH_PATH})
+  endif()
 
   add_executable(${NAME} ${THIS_SRCS})
   target_link_libraries(${NAME} ${__link_libs})
   target_create_scripts(${NAME} DLL_SEARCH_PATHS ${__dll_search_paths})
 
+  if(THIS_WITH_PCL)
+    target_include_directories(${NAME} PRIVATE ${PCL_INCLUDE_DIRS})
+    #target_compile_definitions(${NAME} PRIVATE ${PCL_DEFINITIONS})
+    #target_compile_options(${NAME} PRIVATE ${PCL_COMPILE_OPTIONS})
+  endif()
   if(OS_WIN)
     target_compile_definitions(${NAME}
       PUBLIC GLOG_NO_ABBREVIATED_SEVERITIES
@@ -36,8 +51,46 @@ endmacro()
 
 if(WITH_API)
 
+# If you install PCL to different directory, please set CMAKE_PREFIX_PATH to find it.
+#LIST(APPEND CMAKE_PREFIX_PATH /usr/local/share)
+find_package(PCL)
+if(PCL_FOUND)
+  message(STATUS "Found PCL: ${PCL_VERSION}")
+
+  #message(STATUS "PCL_LIBRARIES: ${PCL_LIBRARIES}")
+  #message(STATUS "PCL_INCLUDE_DIRS: ${PCL_INCLUDE_DIRS}")
+  #message(STATUS "PCL_LIBRARY_DIRS: ${PCL_LIBRARY_DIRS}")
+  #message(STATUS "PCL_DEFINITIONS: ${PCL_DEFINITIONS}")
+  #message(STATUS "PCL_COMPILE_OPTIONS: ${PCL_COMPILE_OPTIONS}")
+
+  if(OS_WIN)
+    get_filename_component(PCL_LIB_SEARCH_PATH "${PCL_LIBRARY_DIRS}/../bin" ABSOLUTE)
+  else()
+    set(PCL_LIB_SEARCH_PATH "${PCL_LIBRARY_DIRS}")
+  endif()
+else()
+  message(WARNING "PCL not found :(")
+endif()
+
+# data
+
 make_executable(get_device_info SRCS data/get_device_info.cc)
-make_executable(get_with_plugin SRCS data/get_with_plugin.cc)
+make_executable(get_img_params SRCS data/get_img_params.cc)
+make_executable(get_imu_params SRCS data/get_imu_params.cc)
+
+make_executable(get_stereo SRCS data/get_stereo.cc WITH_OPENCV)
+make_executable(get_stereo_rectified SRCS data/get_stereo_rectified.cc WITH_OPENCV)
+make_executable(get_disparity SRCS data/get_disparity.cc WITH_OPENCV)
+make_executable(get_depth SRCS data/get_depth.cc WITH_OPENCV)
+if(PCL_FOUND)
+  make_executable(get_points
+    SRCS data/get_points.cc data/pcviewer.cc
+    WITH_OPENCV WITH_PCL
+  )
+endif()
+make_executable(get_with_plugin SRCS data/get_with_plugin.cc WITH_OPENCV)
+
+# control
 
 make_executable(ctrl_infrared SRCS control/infrared.cc WITH_OPENCV)
 

samples/tutorials/data/get_depth.cc

@@ -0,0 +1,42 @@
+#include <opencv2/highgui/highgui.hpp>
+
+#include <glog/logging.h>
+
+#include "mynteye/api.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+
+  api->EnableStreamData(Stream::DEPTH);
+
+  api->Start(Source::VIDEO_STREAMING);
+
+  cv::namedWindow("frame");
+  cv::namedWindow("depth");
+
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+
+    cv::Mat img;
+    cv::hconcat(left_data.frame, right_data.frame, img);
+    cv::imshow("frame", img);
+
+    auto &&depth_data = api->GetStreamData(Stream::DEPTH);
+    if (!depth_data.frame.empty()) {
+      cv::imshow("depth", depth_data.frame);  // CV_16UC1
+    }
+
+    char key = static_cast<char>(cv::waitKey(1));
+    if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
+      break;
+    }
+  }
+
+  api->Stop(Source::VIDEO_STREAMING);
+  return 0;
+}

samples/tutorials/data/get_disparity.cc

@@ -0,0 +1,49 @@
+#include <opencv2/highgui/highgui.hpp>
+
+#include <glog/logging.h>
+
+#include "mynteye/api.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+
+  // api->EnableStreamData(Stream::DISPARITY);
+  api->EnableStreamData(Stream::DISPARITY_NORMALIZED);
+
+  api->Start(Source::VIDEO_STREAMING);
+
+  cv::namedWindow("frame");
+  // cv::namedWindow("disparity");
+  cv::namedWindow("disparity_normalized");
+
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+
+    cv::Mat img;
+    cv::hconcat(left_data.frame, right_data.frame, img);
+    cv::imshow("frame", img);
+
+    // auto &&disp_data = api->GetStreamData(Stream::DISPARITY);
+    // if (!disp_data.frame.empty()) {
+    //   cv::imshow("disparity", disp_data.frame);
+    // }
+
+    auto &&disp_norm_data = api->GetStreamData(Stream::DISPARITY_NORMALIZED);
+    if (!disp_norm_data.frame.empty()) {
+      cv::imshow("disparity_normalized", disp_norm_data.frame);  // CV_8UC1
+    }
+
+    char key = static_cast<char>(cv::waitKey(1));
+    if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
+      break;
+    }
+  }
+
+  api->Stop(Source::VIDEO_STREAMING);
+  return 0;
+}

samples/tutorials/data/get_img_params.cc

@@ -0,0 +1,17 @@
+#include <glog/logging.h>
+
+#include "mynteye/api.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+
+  LOG(INFO) << "Intrinsics left: {" << api->GetIntrinsics(Stream::LEFT) << "}";
+  LOG(INFO) << "Intrinsics right: {" << api->GetIntrinsics(Stream::RIGHT)
+            << "}";
+  LOG(INFO) << "Extrinsics left to right: {"
+            << api->GetExtrinsics(Stream::LEFT, Stream::RIGHT) << "}";
+
+  return 0;
+}

samples/tutorials/data/get_imu_params.cc

@@ -0,0 +1,15 @@
+#include <glog/logging.h>
+
+#include "mynteye/api.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+
+  LOG(INFO) << "Motion intrinsics: {" << api->GetMotionIntrinsics() << "}";
+  LOG(INFO) << "Motion extrinsics left to imu: {"
+            << api->GetMotionExtrinsics(Stream::LEFT) << "}";
+
+  return 0;
+}

samples/tutorials/data/get_points.cc

@@ -0,0 +1,47 @@
+#include <opencv2/highgui/highgui.hpp>
+
+#include <glog/logging.h>
+
+#include "mynteye/api.h"
+
+#include "data/pcviewer.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+
+  api->EnableStreamData(Stream::POINTS);
+
+  api->Start(Source::VIDEO_STREAMING);
+
+  cv::namedWindow("frame");
+  PCViewer pcviewer;
+
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+
+    cv::Mat img;
+    cv::hconcat(left_data.frame, right_data.frame, img);
+    cv::imshow("frame", img);
+
+    auto &&points_data = api->GetStreamData(Stream::POINTS);
+    if (!points_data.frame.empty()) {
+      pcviewer.Draw(points_data.frame);
+    }
+
+    char key = static_cast<char>(cv::waitKey(1));
+    if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
+      break;
+    }
+    if (pcviewer.WasDrew() && pcviewer.WasStopped()) {
+      break;
+    }
+  }
+
+  api->Stop(Source::VIDEO_STREAMING);
+  return 0;
+}

samples/tutorials/data/get_stereo.cc

@@ -0,0 +1,34 @@
+#include <opencv2/highgui/highgui.hpp>
+
+#include <glog/logging.h>
+
+#include "mynteye/api.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+
+  api->Start(Source::VIDEO_STREAMING);
+
+  cv::namedWindow("frame");
+
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+
+    cv::Mat img;
+    cv::hconcat(left_data.frame, right_data.frame, img);
+    cv::imshow("frame", img);
+
+    char key = static_cast<char>(cv::waitKey(1));
+    if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
+      break;
+    }
+  }
+
+  api->Stop(Source::VIDEO_STREAMING);
+  return 0;
+}

samples/tutorials/data/get_stereo_rectified.cc

@@ -0,0 +1,39 @@
+#include <opencv2/highgui/highgui.hpp>
+
+#include <glog/logging.h>
+
+#include "mynteye/api.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+
+  api->EnableStreamData(Stream::LEFT_RECTIFIED);
+  api->EnableStreamData(Stream::RIGHT_RECTIFIED);
+
+  api->Start(Source::VIDEO_STREAMING);
+
+  cv::namedWindow("frame");
+
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT_RECTIFIED);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT_RECTIFIED);
+
+    if (!left_data.frame.empty() && !right_data.frame.empty()) {
+      cv::Mat img;
+      cv::hconcat(left_data.frame, right_data.frame, img);
+      cv::imshow("frame", img);
+    }
+
+    char key = static_cast<char>(cv::waitKey(1));
+    if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
+      break;
+    }
+  }
+
+  api->Stop(Source::VIDEO_STREAMING);
+  return 0;
+}

samples/tutorials/data/pcviewer.cc

@@ -0,0 +1,89 @@
+#include "data/pcviewer.h"
+
+#include <glog/logging.h>
+
+// #include <pcl/common/common_headers.h>
+
+#include <cmath>
+
+std::shared_ptr<pcl::visualization::PCLVisualizer> CustomColorVis(
+    pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc) {
+  // --------------------------------------------
+  // -----Open 3D viewer and add point cloud-----
+  // --------------------------------------------
+  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
+      new pcl::visualization::PCLVisualizer("PointCloud Viewer"));
+  viewer->setBackgroundColor(0, 0, 0);
+  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(
+      pc, 255, 255, 255);
+  viewer->addPointCloud<pcl::PointXYZ>(pc, single_color, "point cloud");
+  viewer->setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "point cloud");
+  // viewer->addCoordinateSystem(1.0);
+  viewer->addCoordinateSystem(1000.0);
+  viewer->initCameraParameters();
+  viewer->setCameraPosition(0, 0, -150, 0, 1, 0);
+  return (viewer);
+}
+
+PCViewer::PCViewer() : viewer_(nullptr) {
+  VLOG(2) << __func__;
+}
+
+PCViewer::~PCViewer() {
+  VLOG(2) << __func__;
+  if (viewer_) {
+    // viewer_->saveCameraParameters("pcl_camera_params.txt");
+    viewer_->close();
+    viewer_ == nullptr;
+  }
+}
+
+void PCViewer::Draw(const cv::Mat &xyz) {
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
+  ConvertMatToPointCloud(xyz, pc);
+  Draw(pc);
+}
+
+void PCViewer::Draw(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc) {
+  if (viewer_ == nullptr) {
+    viewer_ = CustomColorVis(pc);
+  }
+  viewer_->updatePointCloud(pc, "point cloud");
+  viewer_->spinOnce();
+}
+
+bool PCViewer::WasDrew() const {
+  return viewer_ != nullptr;
+}
+
+bool PCViewer::WasStopped() const {
+  return viewer_ == nullptr || viewer_->wasStopped();
+}
+
+void PCViewer::ConvertMatToPointCloud(
+    const cv::Mat &xyz, pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
+  // cv::Mat channels[3];
+  // cv::split(xyz, channels);
+  // double min, max;
+  // cv::minMaxLoc(channels[2], &min, &max);
+
+  for (int i = 0; i < xyz.rows; i++) {
+    for (int j = 0; j < xyz.cols; j++) {
+      auto &&p = xyz.at<cv::Point3f>(i, j);
+      if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) {
+        // LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y
+        // << ", z: " << p.z;
+        pcl::PointXYZ point;
+        point.x = p.x;
+        point.y = p.y;
+        point.z = p.z;
+        // point.z = p.z - min;
+        pc->points.push_back(point);
+      }
+    }
+  }
+
+  pc->width = static_cast<int>(pc->points.size());
+  pc->height = 1;
+}

samples/tutorials/data/pcviewer.h

@@ -0,0 +1,30 @@
+#ifndef MYNTEYE_TUTORIALS_PCVIEWER_H_  // NOLINT
+#define MYNTEYE_TUTORIALS_PCVIEWER_H_
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <memory>
+
+class PCViewer {
+ public:
+  PCViewer();
+  ~PCViewer();
+
+  void Draw(const cv::Mat &xyz);
+
+  void Draw(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc);
+
+  bool WasDrew() const;
+  bool WasStopped() const;
+
+ private:
+  void ConvertMatToPointCloud(
+      const cv::Mat &xyz, pcl::PointCloud<pcl::PointXYZ>::Ptr pc);
+
+  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
+};
+
+#endif  // MYNTEYE_TUTORIALS_PCVIEWER_H_ NOLINT