diff --git a/.gitignore b/.gitignore
index 31ffce55b1fcf9926b1a3184893002b164a22259..47b63fc13658f5c9417837fa6f95fd985216438e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
build
devel
.catkin_workspace
-src/CMakeLists.txt
\ No newline at end of file
+src/CMakeLists.txt
diff --git a/src/binocular/CMakeLists.txt b/src/binocular/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bf88ce21674138e51094b133759039aa5efb191b
--- /dev/null
+++ b/src/binocular/CMakeLists.txt
@@ -0,0 +1,241 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(binocular)
+
+enable_language(C)
+enable_language(CXX)
+enable_language(CUDA)
+find_package(CUDA REQUIRED)
+include_directories(${CUDA_INCLUDE_DIRS})
+cuda_select_nvcc_arch_flags(NVCC_FLAGS "Auto")
+string (REPLACE ";" " " CUDA_ARCH_FLAGS_SPACE_SEPARATED "${NVCC_FLAGS}")
+set(CMAKE_CUDA_FLAGS "${CUDA_ARCH_FLAGS_SPACE_SEPARATED}")
+set(CMAKE_CXX_STANDARD 11)
+find_package(OpenCV)
+add_definitions(-DGPU)
+add_definitions(-DCUDNN)
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ message_generation
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+ config
+ common
+ seuimage
+ darknet
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# CameraProperty.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# CameraInfo.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES vision
+# CATKIN_DEPENDS message_generation roscpp rospy sensor_msgs std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+)
+
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/vision.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(camera src/camera.cpp src/vision_parser.cpp)
+set(CAMERA_FILES src/MYNTeye.cpp)
+add_executable(binocular src/Binocular.cpp ${CAMERA_FILES})
+# add_executable(sim_vision src/SimVision.cpp src/camera/TpCamera.cpp)
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(camera
+# ${catkin_LIBRARIES}
+# MVSDK
+# config
+# )
+
+target_link_libraries(binocular
+ ${catkin_LIBRARIES}
+ cuda
+ cudart
+ darknet
+ seuimage
+ mynteye
+ opencv_core
+ opencv_imgproc
+ opencv_imgcodecs
+ config
+ pthread
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_vision.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/src/binocular/launch/binocular.launch b/src/binocular/launch/binocular.launch
new file mode 100644
index 0000000000000000000000000000000000000000..13ec2cc7577750d3316958dee9be3ec04be1d67d
--- /dev/null
+++ b/src/binocular/launch/binocular.launch
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/binocular/package.xml b/src/binocular/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..cce8ee863618899d3e6f7f591f78fb958c3d3b0d
--- /dev/null
+++ b/src/binocular/package.xml
@@ -0,0 +1,80 @@
+
+
+ binocular
+ 0.0.0
+ The binocular package
+
+
+
+
+ Sciroccogti
+
+
+
+
+
+ Apache2
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ message_generation
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+ common
+ config
+ seuimage
+ darknet
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+ message_generation
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+ message_runtime
+ common
+ seuimage
+ darknet
+
+
+
+
+
+
+
diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..920e493e544dad7e0229cc706d9aab223a0d4fab
--- /dev/null
+++ b/src/binocular/src/Binocular.cpp
@@ -0,0 +1,335 @@
+#include "Binocular.hpp"
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "binocular");
+ ros::NodeHandle node;
+ if (!CameraInit())
+ {
+ ROS_ERROR("failed in opening binocular camera");
+ }
+ BinocularInit();
+ imagePublisher = node.advertise("/result/binocular/compressed", 1);
+ resultPublisher = node.advertise("/result/binocular/imgproc", 1);
+ ros::ServiceServer typeServer = node.advertiseService("/setting/sendtype", SendTypeService);
+ camera->Run();
+ ros::Timer timer = node.createTimer(ros::Duration(0.05), Run);
+ ros::spin();
+ camera->Close();
+ return 0;
+}
+
+void Run(const ros::TimerEvent &event)
+{
+ if (!ros::ok())
+ {
+ return;
+ }
+ bool ret;
+ uint32_t image_time;
+ MYNTdata raw = camera->GetData(image_time);
+ ret = srcMat.upload(raw.color);
+
+ if (!ret)
+ {
+ ROS_ERROR("upload error");
+ return;
+ }
+ if (GET_CHANNELS(camera->type) == 1) // Bayer
+ {
+ ret = CudaBayerToRGB(srcMat, rgbMat, 1.12, 0.85, 1.37);
+ //ret = BayerToRGB(srcMat, rgbMat);
+ ret = Resize(rgbMat, dstMat);
+ ret = dstMat.copyTo(relMat);
+ }
+ else if (GET_CHANNELS(camera->type) == 2) // YUYV
+ {
+ ret = YUV422ToRGB(srcMat, rgbMat);
+ ret = Resize(rgbMat, dstMat);
+ ret = dstMat.copyTo(relMat);
+ }
+ else if (GET_CHANNELS(camera->type) == 3) // RGB
+ {
+ ret = Resize(srcMat, dstMat);
+ ret = dstMat.copyTo(relMat);
+ }
+ else
+ {
+ ROS_ERROR("channel error");
+ }
+ ret = RGBToHSV(relMat, netMat);
+
+ if (!ret)
+ {
+ ROS_ERROR("Resize to net error");
+ return;
+ }
+
+ ret = RGB8uTo32fNorm(netMat, netfMat);
+ if (!ret)
+ {
+ ROS_ERROR("RGB8uTo32fNorm error");
+ return;
+ }
+
+ ret = PackedToPlanar(netfMat, yoloMat);
+ if (!ret)
+ {
+ ROS_ERROR("PackedToPlanar error");
+ return;
+ }
+
+ layer l = yolo.layers[yolo.n - 1];
+ network_predict1(yolo, yoloMat.data());
+ int nboxes = 0;
+ float nms = 0.45;
+ detection *dets = get_network_boxes(&yolo, width, height,
+ 0.5, 0.5, 0, 1, &nboxes, 0);
+
+ if (nms)
+ {
+ do_nms_sort(dets, nboxes, l.classes, nms);
+ }
+
+ std::vector ball_dets, post_dets;
+ ball_dets.clear();
+ post_dets.clear();
+
+ for (int i = 0; i < nboxes; i++)
+ {
+ int id = 0;
+ for (int j = 0; j < l.classes; j++)
+ {
+ if (dets[i].prob[j] > dets[i].prob[id])
+ id = j;
+ }
+ if (id == 0)
+ {
+ if (dets[i].prob[id] >= ballFilter.min_prob)
+ {
+ int bx = (dets[i].bbox.x - dets[i].bbox.w / 2.0) * width;
+ int by = (dets[i].bbox.y - dets[i].bbox.h / 2.0) * height;
+ int bw = dets[i].bbox.w * width, bh = dets[i].bbox.h * height + 1;
+ ball_dets.push_back(DetObject(0, dets[i].prob[id], bx, by, bw, bh));
+ }
+ }
+ else if (id == 1)
+ {
+ if (dets[i].prob[id] >= postFilter.min_prob)
+ {
+ int px = (dets[i].bbox.x - dets[i].bbox.w / 2.0) * width;
+ int py = (dets[i].bbox.y - dets[i].bbox.h / 2.0) * height;
+ int pw = dets[i].bbox.w * width, ph = dets[i].bbox.h * height;
+ post_dets.push_back(DetObject(1, dets[i].prob[id], px, py, pw, ph));
+ }
+ }
+ }
+ std::sort(ball_dets.rbegin(), ball_dets.rend());
+ std::sort(post_dets.rbegin(), post_dets.rend());
+ free_detections(dets, nboxes);
+
+ common::ImageResult imgResult;
+ imgResult.stamp = image_time;
+ if (!ball_dets.empty())
+ {
+ imgResult.has_ball = true;
+ imgResult.ball.x = ball_dets[0].x + ball_dets[0].w / 2;
+ imgResult.ball.y = ball_dets[0].y + ball_dets[0].h;
+ }
+ for (uint32_t i = 0; i < post_dets.size() && i < 2; i++)
+ {
+ imgResult.has_post = true;
+ ObjInfo tmp;
+ tmp.x = ball_dets[i].x + ball_dets[i].w / 2;
+ tmp.y = ball_dets[i].y + ball_dets[i].h / 3 * 2;
+ imgResult.posts.push_back(tmp);
+ }
+ resultPublisher.publish(imgResult);
+
+ if (send_type > 0)
+ {
+ cv::Mat rgb(height, width, CV_8UC3);
+ if (!dstMat.download(rgb))
+ {
+ ROS_ERROR("download error");
+ return;
+ }
+ cv::Mat bgr;
+ cv::cvtColor(rgb, bgr, CV_RGB2BGR);
+
+ if (send_type > 1)
+ {
+ if (!ball_dets.empty())
+ {
+ cv::Rect ball_roi(ball_dets[0].x, ball_dets[0].y, ball_dets[0].w, ball_dets[0].h);
+ cv::rectangle(bgr, ball_roi, cv::Scalar(255, 0, 0), 2);
+ int ball_dist = CountDistance(raw.depth, ball_roi);
+ ROS_INFO("ball_dist=%d", ball_dist); // TODO: 接入接口而不是什么INFO！
+ }
+
+ for (uint32_t i = 0; i < post_dets.size() && i < 2; i++)
+ {
+ cv::Rect post_roi(post_dets[0].x, post_dets[0].y, post_dets[0].w, post_dets[0].h);
+ cv::rectangle(bgr, post_roi, cv::Scalar(0, 0, 255), 2);
+ int post_dist = CountDistance(raw.depth, post_roi);
+ ROS_INFO("post_dist=%d", post_dist); // TODO: 接入接口而不是什么INFO！
+ }
+ }
+ cv::imshow("binocular", bgr);
+ ImagePublish(bgr);
+ }
+}
+
+bool CameraInit()
+{
+ camera = std::make_shared();
+ if (camera->Open())
+ return true;
+ return false;
+}
+
+bool BinocularInit()
+{
+ std::string visionfile;
+ try
+ {
+ ros::param::get("vision_file", visionfile);
+ }
+ catch (ros::InvalidNameException &e)
+ {
+ ROS_ERROR("%s", e.what());
+ return false;
+ }
+
+ std::string vision_root = visionfile.substr(0, visionfile.find_last_of("/") + 1);
+ std::string yolo_cfg, weights;
+ if (visionfile.empty())
+ return false;
+ common::bpt::ptree pt;
+ if (!common::get_tree_from_file(visionfile, pt))
+ {
+ ROS_ERROR("parse file: [%s] failed", visionfile.c_str());
+ return false;
+ }
+ yolo_cfg = vision_root + pt.get("cfg_file");
+ weights = vision_root + pt.get("weights_file");
+ ballFilter.min_prob = pt.get("dets.prob.ball");
+ ballFilter.r_range = get_config_vector(pt, "dets.boxes.ball.ratio");
+ ballFilter.w_range = get_config_vector(pt, "dets.boxes.ball.width");
+ ballFilter.h_range = get_config_vector(pt, "dets.boxes.ball.height");
+ postFilter.min_prob = pt.get("dets.prob.post");
+ postFilter.r_range = get_config_vector(pt, "dets.boxes.post.ratio");
+ postFilter.w_range = get_config_vector(pt, "dets.boxes.post.width");
+ postFilter.h_range = get_config_vector(pt, "dets.boxes.post.height");
+
+ yolo.gpu_index = 0;
+ yolo = parse_network_cfg_custom((char *)(yolo_cfg.c_str()), 1, 0);
+ load_weights(&yolo, const_cast((char *)(weights.c_str())));
+ set_batch_network(&yolo, 1);
+ fuse_conv_batchnorm(yolo);
+ calculate_binary_weights(yolo);
+ srand(2222222);
+ cudaSetDevice(0);
+
+ if (!MallocMemory())
+ {
+ ROS_ERROR("MallocMemory failed");
+ return false;
+ }
+ return true;
+}
+
+int CountDistance(cv::Mat &mat, cv::Rect roi)
+{
+ cv::Mat depth_cop = mat(roi).clone();
+
+ int count[2] = {0, 0};
+
+ std::vector depth_vector;
+ depth_vector.clear();
+
+ ushort *p = depth_cop.ptr();
+ for (size_t i = 0; i < depth_cop.total(); i++)
+ {
+ if (*p != 0)
+ depth_vector.push_back((float)*p);
+ p++;
+ }
+
+ cv::Mat labels;
+ std::vector centers;
+ if (depth_vector.size() < 2) // N < K 时
+ {
+ return 0;
+ }
+ else
+ {
+ // kmeans 只接收CV_32F
+ cv::kmeans(depth_vector, 2, labels, cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 5, 1.0), 3, cv::KMEANS_PP_CENTERS, centers);
+
+ for (size_t i = 0; i < depth_vector.size(); i++)
+ {
+ count[labels.data[i]]++;
+ }
+ return centers[0] > centers[1] ? centers[0] : centers[1];
+ }
+}
+
+void ImagePublish(const cv::Mat &bgr)
+{
+ sensor_msgs::CompressedImage image;
+ std::vector buf;
+ cv::imencode(".jpg", bgr, buf);
+ image.header.stamp = ros::Time::now();
+ image.header.frame_id = "binocular";
+ image.format = "jpeg";
+ image.data.resize(buf.size());
+ memcpy(&image.data[0], &(buf[0]), buf.size());
+ imagePublisher.publish(image);
+}
+
+bool MallocMemory()
+{
+ if (camera.get() == nullptr)
+ return false;
+ bool ret;
+ ret = srcMat.create(camera->height, camera->width, GET_CHANNELS(camera->type));
+ if (!ret)
+ return false;
+
+ ret = rgbMat.create(camera->height, camera->width, 3);
+ if (!ret)
+ return false;
+
+ ret = dstMat.create(height, width, 3);
+ if (!ret)
+ return false;
+
+ ret = relMat.create(height, width, 3);
+ if (!ret)
+ return false;
+
+ ret = hsvMat.create(height, width, 3);
+ if (!ret)
+ return false;
+
+ ret = netMat.create(yolo.h, yolo.w, 3);
+ if (!ret)
+ return false;
+
+ ret = netfMat.create(yolo.h, yolo.w, 3);
+ if (!ret)
+ return false;
+
+ ret = yoloMat.create(yolo.h, yolo.w, 3);
+ if (!ret)
+ return false;
+
+ return true;
+}
+
+bool SendTypeService(SetInt::Request &req, SetInt::Response &res)
+{
+ send_type = req.number;
+ return true;
+}
\ No newline at end of file
diff --git a/src/binocular/src/Binocular.hpp b/src/binocular/src/Binocular.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d4def4ad92c2d10f6c2eb935464a9e9b641261c
--- /dev/null
+++ b/src/binocular/src/Binocular.hpp
@@ -0,0 +1,44 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "MYNTeye.hpp"
+
+using namespace seuimage;
+using namespace common;
+
+bool MallocMemory();
+void NetworkInit(std::string cfg, std::string wts);
+bool BinocularInit();
+bool CameraInit();
+void Run(const ros::TimerEvent &event);
+int CountDistance(cv::Mat &mat, cv::Rect roi);
+
+void ImagePublish(const cv::Mat &bgr);
+bool SendTypeService(SetInt::Request &req, SetInt::Response &res);
+
+const int width = 640; // 仅发布左眼
+const int height = 400;
+CudaMatC srcMat;
+CudaMatC rgbMat;
+CudaMatC dstMat;
+CudaMatC hsvMat;
+CudaMatC relMat;
+CudaMatC netMat;
+CudaMatF netfMat;
+CudaMatF yoloMat;
+network yolo;
+
+ObjectFilter ballFilter, postFilter;
+std::atomic_int send_type(0);
+std::shared_ptr camera;
+ros::Publisher imagePublisher;
+ros::Publisher resultPublisher;
+
diff --git a/src/binocular/src/MYNTeye.cpp b/src/binocular/src/MYNTeye.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0c8eade18e2167182dee21d9ae9e7bc92d3e9999
--- /dev/null
+++ b/src/binocular/src/MYNTeye.cpp
@@ -0,0 +1,91 @@
+#include "MYNTeye.hpp"
+
+using namespace mynteye;
+
+MYNTeye::MYNTeye()
+{
+ raw_.color = cv::Mat(height, width, type);
+ raw_.depth = cv::Mat(height, width, depth_type);
+ alive_ = false;
+}
+
+MYNTeye::~MYNTeye()
+{
+ if (td_.joinable())
+ {
+ td_.join();
+ }
+}
+
+bool MYNTeye::Open()
+{
+ Context context;
+ auto &&devices = context.devices();
+ if (!devices.size())
+ {
+ return false;
+ }
+
+ auto &&device = devices[0];
+ api_ = API::Create(device);
+ if (!api_)
+ {
+ return false;
+ }
+
+ const StreamRequest request(2 * width, height, Format::YUYV, 30);
+ api_->ConfigStreamRequest(request);
+ api_->EnableStreamData(Stream::LEFT_RECTIFIED);
+ api_->EnableStreamData(Stream::RIGHT_RECTIFIED);
+ api_->SetDisparityComputingMethodType(DisparityComputingMethod::BM);
+ api_->EnableStreamData(Stream::DEPTH);
+ // auto exposure
+ api_->SetOptionValue(Option::EXPOSURE_MODE, 0);
+ // max_gain: range [0,255], default 8
+ api_->SetOptionValue(Option::MAX_GAIN, 8);
+ // max_exposure_time: range [0,655], default 333
+ api_->SetOptionValue(Option::MAX_EXPOSURE_TIME, 333);
+ // desired_brightness: range [1,255], default 122
+ api_->SetOptionValue(Option::DESIRED_BRIGHTNESS, 122);
+ // min_exposure_time: range [0,655], default 0
+ api_->SetOptionValue(Option::MIN_EXPOSURE_TIME, 0);
+
+ api_->Start(Source::VIDEO_STREAMING);
+
+ return true;
+}
+
+bool MYNTeye::Close()
+{
+ alive_ = false;
+ return true;
+}
+
+void MYNTeye::Run()
+{
+ alive_ = true;
+ td_ = std::move(std::thread([this]() {
+ while (alive_)
+ {
+ api_->WaitForStreams(); // TODO: 这行应该放在哪？
+ auto &&left_data = api_->GetStreamData(Stream::LEFT_RECTIFIED);
+ auto &&depth_data = api_->GetStreamData(Stream::DEPTH);
+ if (!left_data.frame.empty() && !depth_data.frame.empty()) // && !right_data.frame.empty())
+ {
+ raw_mutex_.lock();
+ time_ = get_clock();
+ memcpy(raw_.color.data, left_data.frame.data, width * height);
+ memcpy(raw_.depth.data, depth_data.frame.data, width * height);
+ raw_mutex_.unlock();
+ }
+ usleep(100);
+ }
+ }));
+}
+
+MYNTdata &MYNTeye::GetData(uint32_t &t)
+{
+ std::lock_guard lk(raw_mutex_);
+ t = time_;
+ return raw_;
+}
\ No newline at end of file
diff --git a/src/binocular/src/MYNTeye.hpp b/src/binocular/src/MYNTeye.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e8e70b450cde7c517e66eab78bacbd9ffb23c194
--- /dev/null
+++ b/src/binocular/src/MYNTeye.hpp
@@ -0,0 +1,40 @@
+#ifndef __MYNTEYE_HPP
+#define __MYNTEYE_HPP
+
+#include
+#include
+#include "mynteye/api/api.h"
+#include "mynteye/device/context.h"
+#define GET_CHANNELS(t) (((t)>>CV_CN_SHIFT)+1)
+
+struct MYNTdata
+{
+ cv::Mat color;
+ cv::Mat depth;
+};
+
+class MYNTeye
+{
+public:
+ MYNTeye();
+ ~MYNTeye();
+ bool Open();
+ bool Close();
+ // bool Available(); 好像没什么意义，Open()返回的就够了
+ void Run();
+ MYNTdata &GetData(uint32_t &t);
+ const int width = 640;
+ const int height = 400;
+ const int type = CV_8UC3;
+ const int depth_type = CV_16UC1;
+
+protected:
+ bool alive_;
+ std::thread td_;
+ std::shared_ptr api_;
+ MYNTdata raw_;
+ uint32_t time_;
+ mutable std::mutex raw_mutex_;
+};
+
+#endif
\ No newline at end of file