diff --git a/CMakeLists.txt b/CMakeLists.txt index 0acbd6d4d..3a295e919 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,11 @@ project(onboardsdk) set(CMAKE_VERBOSE_MAKEFILE OFF) +set(DJIOSDK 0) +set(DJIOSDK_MINOR_VERSION 1) +set(DJIOSDK_PATCH_VERSION 0) +set(DJIOSDK_VERSION + ${DJIOSDK_MAJOR_VERSION}.${DJIOSDK_MINOR_VERSION}.${DJIOSDK_PATCH_VERSION}) add_definitions(-DDJIOSDK_MAJOR_VERSION=3) add_definitions(-DDJIOSDK_MINOR_VERSION=8) add_definitions(-DDJIOSDK_PATCH_VERSION=1) @@ -49,6 +54,9 @@ endif() set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/libs) +if (ADVANCED_SENSING) + add_definitions(-DADVANCED_SENSING) +endif() add_subdirectory(osdk-core) if (${CMAKE_SYSTEM_NAME} MATCHES Linux) diff --git a/osdk-core/CMakeLists.txt b/osdk-core/CMakeLists.txt index 4cfbea9e7..1779f8a91 100755 --- a/osdk-core/CMakeLists.txt +++ b/osdk-core/CMakeLists.txt @@ -97,6 +97,46 @@ SET(OSDK_INTERFACE_LIBS pthread) # pthread is assumed available on linux SET(MODULE_BUILD_INTERFACE "") SET(MODULE_INSTALL_INTERFACE "") +## Advanced Sensing +if(ADVANCED_SENSING) + # Add a cmake file to find libusb + set(CMAKE_MODULE_PATH ${CURRENT_CMAKE_MODULE_PATH}) + + find_package(LibUSB REQUIRED) + find_package(AdvancedSensing QUIET) + if(NOT ADVANCED_SENSING_FOUND) + include(${CURRENT_CMAKE_MODULE_PATH}/External_AdvancedSensing.cmake) + add_dependencies(${PROJECT_NAME} advanced-sensing) + endif() + + find_package(FFMPEG REQUIRED) + + if(FFMPEG_FOUND) + message( STATUS "Found FFmpeg ${FFMPEG_VERSION} installed in the system.") + message( STATUS " - Includes: ${FFMPEG_INCLUDE_DIRS}") + message( STATUS " - Libraries: ${FFMPEG_LIBRARIES}") + else() + message("Cannot Find FFMPEG") + endif(FFMPEG_FOUND) + + target_include_directories(${PROJECT_NAME} PUBLIC + $) + target_link_libraries(${PROJECT_NAME} PRIVATE ${ADVANCED_SENSING_LIBRARY}) + + + target_include_directories(${PROJECT_NAME} PUBLIC ${LIBUSB_1_INCLUDE_DIRS}) + target_link_libraries(${PROJECT_NAME} PUBLIC ${LIBUSB_1_LIBRARIES}) + + target_include_directories(${PROJECT_NAME} PUBLIC ${FFMPEG_INCLUDE_DIR}) + target_link_libraries(${PROJECT_NAME} PUBLIC ${FFMPEG_LIBRARIES}) + + set(OSDK_INTERFACE_LIBS ${OSDK_INTERFACE_LIBS} ${LIBUSB_1_LIBRARIES} ${FFMPEG_LIBRARIES}) + set(MODULE_BUILD_INTERFACE ${MODULE_BUILD_INTERFACE} ${ADVANCED_SENSING_LIBRARY}) + set(MODULE_INSTALL_INTERFACE ${MODULE_INSTALL_INTERFACE} $/lib/libadvanced-sensing.a) + + target_compile_definitions(${PROJECT_NAME} PUBLIC -DADVANCED_SENSING) + +endif() ## Once all the modules are done, set the interface_link_libraries property set_property(TARGET ${PROJECT_NAME} PROPERTY INTERFACE_LINK_LIBRARIES @@ -170,6 +210,21 @@ FILE(GLOB OSDK_LIB_HEADERS linker/core/utils/inc/*.h linker/core/root_task/*.h) +# Append advanced sensing headers +if(ADVANCED_SENSING) + install(FILES ${ADVANCED_SENSING_LIBRARY} + DESTINATION "${INSTALL_LIB_DIR}" COMPONENT shlib) + + set(OSDK_LIB_HEADERS + ${OSDK_LIB_HEADERS} + ${ADVANCED_SENSING_INCLUDE_DIRS}/dji_advanced_sensing.hpp + ${ADVANCED_SENSING_INCLUDE_DIRS}/dji_advanced_sensing_protocol.hpp + ${ADVANCED_SENSING_INCLUDE_DIRS}/linux_usb_device.hpp + ${ADVANCED_SENSING_INCLUDE_DIRS}/dji_camera_image.hpp + ${ADVANCED_SENSING_INCLUDE_DIRS}/dji_camera_stream.hpp + ) +endif() + set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER "${OSDK_LIB_HEADERS}") diff --git a/osdk-core/api/inc/dji_ack.hpp b/osdk-core/api/inc/dji_ack.hpp index 9c8e49521..4e80373b3 100644 --- a/osdk-core/api/inc/dji_ack.hpp +++ b/osdk-core/api/inc/dji_ack.hpp @@ -262,6 +262,57 @@ class ACK uint16_t reserved_1; } WayPointStatusPushData; + /*! + * @brief This constant variable defines number of pixels for QVGA images + */ + static const int IMG_240P_SIZE = 240 * 320; + typedef uint8_t Image[IMG_240P_SIZE]; + /*! + * @brief sub-struct for stereo image with raw data and camera name + */ + typedef struct ImageMeta + { + Image image; + char name[12]; + } ImageMeta; // pack(1) + /*! + * @brief This struct captures PushData when subscribe to QVGA images + */ + typedef struct StereoImgData + { + uint32_t frame_index; + uint32_t time_stamp; + uint8_t num_imgs; + /* + * There could be 50 different kinds of images coming from the drone, + * 5 camera pairs and 10 images types. + * Here we use an uint64_t to describe which image is coming + * from the USB line, each bit represents if there's data or not + * Please use AdvancedSensing::ReceivedImgDesc to match them + * For M210, we support up to 4 images at the same time + */ + uint64_t img_desc; + // @note for M210, at most 4 imgs come at the same time. + ImageMeta img_vec[4]; + } StereoImgData; // pack(1) + /*! + * @brief This constant variable defines number of pixels for VGA images + */ + static const int IMG_VGA_SIZE = 640 * 480; + typedef uint8_t VGAImage[IMG_VGA_SIZE]; + /*! + * @brief This struct captures PushData when subscribe to VGA images + */ + typedef struct StereoVGAImgData + { + uint32_t frame_index; + uint32_t time_stamp; + uint8_t num_imgs; + uint8_t direction; + // @note VGA imgs always come in pair + VGAImage img_vec[2]; + } StereoVGAImgData; // pack(1) + /*! * @brief This struct captures PushData when subscribe to UTC & FC time in hardware sync */ @@ -307,10 +358,9 @@ class ACK /* * Push Data from AdvancedSensing protocol - + */ StereoImgData *stereoImgData; StereoVGAImgData *stereoVGAImgData; - */ /* * Push Data from GPS or RTK diff --git a/osdk-core/api/inc/dji_vehicle.hpp b/osdk-core/api/inc/dji_vehicle.hpp index 84f51e5c4..062327c7e 100644 --- a/osdk-core/api/inc/dji_vehicle.hpp +++ b/osdk-core/api/inc/dji_vehicle.hpp @@ -54,6 +54,9 @@ #include "dji_gimbal_manager.hpp" #include "dji_flight_controller.hpp" #include "dji_psdk_manager.hpp" +#ifdef ADVANCED_SENSING +#include "dji_advanced_sensing.hpp" +#endif namespace DJI { @@ -106,6 +109,9 @@ class Vehicle FlightController* flightController; PSDKManager* psdkManager; GimbalManager* gimbalManager; +#ifdef ADVANCED_SENSING + AdvancedSensing* advancedSensing; +#endif int functionalSetUp(); ////////// Blocking calls /////////// @@ -259,6 +265,20 @@ class Vehicle bool initPSDKManager(); bool initGimbalManager(); bool initOSDKHeartBeatThread(); +#ifdef ADVANCED_SENSING + bool initAdvancedSensing(); +#endif + +#ifdef ADVANCED_SENSING + /*! @brief This function takes a frame and calls the right handlers/functions + * based on the nature of the frame (ack, blocking, etc.) + * @param receivedFrame: RecvContainer populated by the AdvancedSensing Protocol + * @return NULL + */ + void processAdvancedSensingImgs(RecvContainer* receivedFrame); + + bool advSensingErrorPrintOnce; +#endif private: void setActivationStatus(bool is_activated); void initCMD_SetSupportMatrix(); diff --git a/osdk-core/api/src/dji_vehicle.cpp b/osdk-core/api/src/dji_vehicle.cpp index 9546aab30..eaba842ad 100644 --- a/osdk-core/api/src/dji_vehicle.cpp +++ b/osdk-core/api/src/dji_vehicle.cpp @@ -81,6 +81,10 @@ Vehicle::Vehicle(Linker* linker) , psdkManager(NULL) , flightController(NULL) , missionManager(NULL) +#ifdef ADVANCED_SENSING + , advancedSensing(NULL) + , advSensingErrorPrintOnce(false) +#endif { ackErrorCode.data = OpenProtocolCMD::ErrorCode::CommonACK::NO_RESPONSE_ERROR; } @@ -205,6 +209,14 @@ Vehicle::init() return false; } +#ifdef ADVANCED_SENSING + if (!initAdvancedSensing()) + { + DERROR("Failed to initialize AdvancedSensing!\n"); + return false; + } +#endif + return true; } @@ -312,6 +324,12 @@ Vehicle::~Vehicle() { delete this->flightController; } + +#ifdef ADVANCED_SENSING + if (this->advancedSensing) + delete this->advancedSensing; +#endif + OsdkOsal_TaskDestroy(sendHeartbeatToFCHandle); } @@ -693,6 +711,75 @@ Vehicle::initVirtualRC() return true; } +#ifdef ADVANCED_SENSING +bool +Vehicle::initAdvancedSensing() +{ + if(this->advancedSensing) + { + DDEBUG("vehicle->advancedSensing already initalized!"); + return true; + } + + this->advancedSensing = new (std::nothrow) AdvancedSensing(this); + if (this->advancedSensing == 0) + { + DERROR("Failed to allocate memory for AdvancedSensing!\n"); + return false; + } + this->advancedSensing->init(); + + return true; +} +#endif + + +#ifdef ADVANCED_SENSING +void +Vehicle::processAdvancedSensingImgs(RecvContainer* receivedFrame) +{ + if (receivedFrame->recvInfo.cmd_id == AdvancedSensingProtocol::PROCESS_IMG_CMD_ID) + { + if (this->advancedSensing->stereoHandler.callback) + { + this->advancedSensing->stereoHandler.callback( + this, *receivedFrame, this->advancedSensing->stereoHandler.userData); + } + else + { + this->advancedSensing->unsubscribeStereoImages(); + if(!advSensingErrorPrintOnce){ + DERROR("No callback registered for 240p stereo images.\n" + "This usually happens when user subscribed to images and restart " + "the program without unsubscribing them.\n" + "Vehicle unsubscribed 240p stereo images automatically.\n"); + advSensingErrorPrintOnce = true; + } + } + } + else if (receivedFrame->recvInfo.cmd_id == + AdvancedSensingProtocol::PROCESS_VGA_CMD_ID) + { + if (this->advancedSensing->vgaHandler.callback) + { + this->advancedSensing->vgaHandler.callback(this, *receivedFrame, + this->advancedSensing->vgaHandler.userData); + } + else + { + this->advancedSensing->unsubscribeVGAImages(); + if(!advSensingErrorPrintOnce){ + DERROR("No callback registered for VGA stereo images.\n" + "This usually happens when user subscribed to images and restart " + "the program without unsubscribing them.\n" + "Vehicle unsubscribed VGA stereo images automatically.\n"); + advSensingErrorPrintOnce = true; + } + } + } +} +#endif + bool Vehicle::initOSDKHeartBeatThread() { /*! create task for OSDK heart beat */ diff --git a/sample/platform/linux/CMakeLists.txt b/sample/platform/linux/CMakeLists.txt index 8a73f0346..5e38162cb 100644 --- a/sample/platform/linux/CMakeLists.txt +++ b/sample/platform/linux/CMakeLists.txt @@ -73,4 +73,10 @@ add_subdirectory(telemetry) add_subdirectory(logging) add_subdirectory(time-sync) add_subdirectory(payload-3rd-party) -add_subdirectory(payloads) \ No newline at end of file +add_subdirectory(payloads) + +if(ADVANCED_SENSING) + add_definitions(-DADVANCED_SENSING) + message(STATUS "AdvancedSensing sample is enabled") + add_subdirectory(advanced-sensing) +endif() \ No newline at end of file diff --git a/sample/platform/linux/advanced-sensing/CMakeLists.txt b/sample/platform/linux/advanced-sensing/CMakeLists.txt new file mode 100755 index 000000000..32050d566 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/CMakeLists.txt @@ -0,0 +1,62 @@ +# * @Copyright (c) 2016-2017 DJI +# * +# * Permission is hereby granted, free of charge, to any person obtaining a copy +# * of this software and associated documentation files (the "Software"), to deal +# * in the Software without restriction, including without limitation the rights +# * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# * copies of the Software, and to permit persons to whom the Software is +# * furnished to do so, subject to the following conditions: +# * +# * The above copyright notice and this permission notice shall be included in +# * all copies or substantial portions of the Software. +# * +# * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# * SOFTWARE. +# * +# * + +cmake_minimum_required(VERSION 2.8) +project(djiosdk-advanced-sensing-sample) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fPIC -pthread -g -O0") + +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/osdk-core/cmake-modules/") + +find_package(LibUSB REQUIRED) +include_directories(${LIBUSB_1_INCLUDE_DIRS}) + +set(HELPER_FUNCTIONS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../common) + +include_directories(${ONBOARDSDK_SOURCE}/api/inc) +include_directories(${ONBOARDSDK_SOURCE}/utility/inc) +include_directories(${ONBOARDSDK_SOURCE}/hal/inc) +include_directories(${ONBOARDSDK_SOURCE}/protocol/inc) +include_directories(${ONBOARDSDK_SOURCE}/platform/linux/inc) +include_directories(${HELPER_FUNCTIONS_DIR}) + +include_directories(${MODULES_HEADER_DIR}) +include_directories(${FLIGHT_MODULES_HEADER_DIR}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../common) + +FILE(GLOB SOURCE_FILES *.hpp *.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../common/dji_linux_environment.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../common/dji_linux_helpers.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/../hal/*.c + ${CMAKE_CURRENT_SOURCE_DIR}/../osal/*.c + ${LINKER_HEADER_SRC} + ) + +add_subdirectory(stereo_vision_single_thread_sample) +add_subdirectory(stereo_vision_multi_thread_sample) +add_subdirectory(camera_stream_poll_sample) +add_subdirectory(camera_stream_callback_sample) +add_subdirectory(stereo_vision_depth_perception_sample) + +if (TARGET_TRACKING_SAMPLE) + add_subdirectory(camera_stream_target_tracking_sample) +endif() diff --git a/sample/platform/linux/advanced-sensing/camera_stream_callback_sample/CMakeLists.txt b/sample/platform/linux/advanced-sensing/camera_stream_callback_sample/CMakeLists.txt new file mode 100755 index 000000000..42ec719b8 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/camera_stream_callback_sample/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 2.8) +project(camera-stream-callback-sample) + +# Try to see if user has OpenCV installed +# if yes, default callback will display the image +find_package( OpenCV QUIET ) +if (OpenCV_FOUND) + message( "\n${PROJECT_NAME}...") + message( STATUS "Found OpenCV installed in the system, will use it to display image in AdvancedSensing APIs") + message( STATUS " - Includes: ${OpenCV_INCLUDE_DIRS}") + message( STATUS " - Libraries: ${OpenCV_LIBRARIES}") + add_definitions(-DOPEN_CV_INSTALLED) +else() + message( STATUS "Did not find OpenCV in the system, image data is inside RecvContainer as raw data") +endif () + +set(HELPER_FUNCTIONS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +add_executable(${PROJECT_NAME} + ${SOURCE_FILES} + ${HELPER_FUNCTIONS_DIR}/dji_linux_environment.cpp + ${HELPER_FUNCTIONS_DIR}/dji_linux_helpers.cpp + + camera-stream-callback-sample.cpp + ) + +target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS}) + +target_link_libraries(${PROJECT_NAME} + djiosdk-core + ) + +if (OpenCV_FOUND) + target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBS} + ) +endif () diff --git a/sample/platform/linux/advanced-sensing/camera_stream_callback_sample/camera-stream-callback-sample.cpp b/sample/platform/linux/advanced-sensing/camera_stream_callback_sample/camera-stream-callback-sample.cpp new file mode 100755 index 000000000..6fb2c35e1 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/camera_stream_callback_sample/camera-stream-callback-sample.cpp @@ -0,0 +1,128 @@ +/*! @file camera_stream_poll_sample.cpp + * @version 3.6 + * @date Feb 1st 2018 + * + * @brief + * AdvancedSensing, Camera Stream API usage in a Linux environment. + * This sample shows how to use a callback function to process (view) the FPV or + * main camera image. + * (Optional) With OpenCV installed, user can visualize the images + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * */ + + +#include "dji_vehicle.hpp" +#include "dji_linux_helpers.hpp" +#include +#include + +#ifdef OPEN_CV_INSTALLED +#include "opencv2/opencv.hpp" +#include "opencv2/highgui/highgui.hpp" +#endif + +using namespace DJI::OSDK; +using namespace cv; +using namespace std; + +void show_rgb(CameraRGBImage img, void *p) +{ + string name = string(reinterpret_cast(p)); + cout << "#### Got image from:\t" << name << endl; +#ifdef OPEN_CV_INSTALLED + Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width*3); + cvtColor(mat, mat, COLOR_RGB2BGR); + imshow(name,mat); + cv::waitKey(1); +#endif +} + +int main(int argc, char** argv) +{ + bool f = false; + bool m = false; + char c = 0; + cout << "Please enter the type of camera stream you want to view\n" + << "m: Main Camera\n" + << "f: FPV Camera" << endl; + cin >> c; + + switch(c) + { + case 'm': + m=true; break; + case 'f': + f=true; break; + default: + cout << "No camera selected"; + return 1; + } + + // Setup OSDK. + bool enableAdvancedSensing = true; + LinuxSetup linuxEnvironment(argc, argv, enableAdvancedSensing); + Vehicle* vehicle = linuxEnvironment.getVehicle(); + const char *acm_dev = linuxEnvironment.getEnvironment()->getDeviceAcm().c_str(); + vehicle->advancedSensing->setAcmDevicePath(acm_dev); + if (vehicle == NULL) + { + std::cout << "Vehicle not initialized, exiting.\n"; + return -1; + } + + char fpvName[] = "FPV_CAM"; + char mainName[] = "MAIN_CAM"; + + bool camResult = false; + if(f) + { + camResult = vehicle->advancedSensing->startFPVCameraStream(&show_rgb, &fpvName); + } + else if(m) + { + camResult = vehicle->advancedSensing->startMainCameraStream(&show_rgb, &mainName); + } + + if(!camResult) + { + cout << "Failed to open selected camera" << endl; + return 1; + } + + //cameraZoomControl(vehicle); //run camera zoom control test + CameraRGBImage camImg; //get the camera's image + + // main thread just sleep + // callback function will be called whenever a new image is ready + sleep(3000); + + if(f) + { + vehicle->advancedSensing->stopFPVCameraStream(); + } + else if(m) + { + vehicle->advancedSensing->stopMainCameraStream(); + } + return 0; +} diff --git a/sample/platform/linux/advanced-sensing/camera_stream_poll_sample/CMakeLists.txt b/sample/platform/linux/advanced-sensing/camera_stream_poll_sample/CMakeLists.txt new file mode 100755 index 000000000..7c5dd959b --- /dev/null +++ b/sample/platform/linux/advanced-sensing/camera_stream_poll_sample/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8) +project(camera-stream-poll-sample) + +# Try to see if user has OpenCV installed +# if yes, default callback will display the image +find_package( OpenCV QUIET ) +if (OpenCV_FOUND) + message( "\n${PROJECT_NAME}...") + message( STATUS "Found OpenCV installed in the system, will use it to display image in AdvancedSensing APIs") + message( STATUS " - Includes: ${OpenCV_INCLUDE_DIRS}") + message( STATUS " - Libraries: ${OpenCV_LIBRARIES}") + add_definitions(-DOPEN_CV_INSTALLED) +else() + message( STATUS "Did not find OpenCV in the system, image data is inside RecvContainer as raw data") +endif () + +set(HELPER_FUNCTIONS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +add_executable(${PROJECT_NAME} + ${SOURCE_FILES} + ${HELPER_FUNCTIONS_DIR}/dji_linux_environment.cpp + ${HELPER_FUNCTIONS_DIR}/dji_linux_helpers.cpp + camera-stream-poll-sample.cpp + ) +target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS}) + +target_link_libraries(${PROJECT_NAME} + djiosdk-core + ) + +if (OpenCV_FOUND) + target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBS} + ) +endif () diff --git a/sample/platform/linux/advanced-sensing/camera_stream_poll_sample/camera-stream-poll-sample.cpp b/sample/platform/linux/advanced-sensing/camera_stream_poll_sample/camera-stream-poll-sample.cpp new file mode 100755 index 000000000..7cf28d1c8 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/camera_stream_poll_sample/camera-stream-poll-sample.cpp @@ -0,0 +1,163 @@ +/*! @file camera_stream_poll_sample.cpp + * @version 3.6 + * @date Feb 1st 2018 + * + * @brief + * AdvancedSensing, Camera Stream API usage in a Linux environment. + * This sample shows how to poll new images from FPV camera or/and + * main camera from the main thread. + * (Optional) With OpenCV installed, user can visualize the images + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * */ + + +#include "dji_vehicle.hpp" +#include +#include "dji_linux_helpers.hpp" + +#ifdef OPEN_CV_INSTALLED + #include "opencv2/opencv.hpp" + #include "opencv2/highgui/highgui.hpp" +#endif + +using namespace DJI::OSDK; +using namespace cv; +using namespace std; + +void show_rgb(CameraRGBImage img, char* name) +{ + cout << "#### Got image from:\t" << string(name) << endl; +#ifdef OPEN_CV_INSTALLED + Mat mat(img.height, img.width, CV_8UC3, img.rawData.data(), img.width*3); + cvtColor(mat, mat, COLOR_RGB2BGR); + imshow(name,mat); + cv::waitKey(1); +#endif +} + +int main(int argc, char** argv) +{ + bool f = false; + bool m = false; + char c = 0; + cout << "Please enter the type of camera stream you want to view\n" + << "m: Main Camera\n" + << "f: FPV Camera\n" + << "b: both Cameras" << endl; + cin >> c; + + switch(c) + { + case 'm': + m=true; break; + case 'f': + f=true; break; + case 'b': + f=true; m=true; break; + default: + cout << "No camera selected"; + return 1; + } + + // Setup OSDK. + bool enableAdvancedSensing = true; + LinuxSetup linuxEnvironment(argc, argv, enableAdvancedSensing); + Vehicle* vehicle = linuxEnvironment.getVehicle(); + const char *acm_dev = linuxEnvironment.getEnvironment()->getDeviceAcm().c_str(); + vehicle->advancedSensing->setAcmDevicePath(acm_dev); + if (vehicle == NULL) + { + std::cout << "Vehicle not initialized, exiting.\n"; + return -1; + } + + bool fpvCamResult = false; + bool mainCamResult = false; + + if(f) + { + fpvCamResult = vehicle->advancedSensing->startFPVCameraStream(); + if(!fpvCamResult) + { + cout << "Failed to open FPV Camera" << endl; + } + } + + if(m) + { + mainCamResult = vehicle->advancedSensing->startMainCameraStream(); + if(!mainCamResult) + { + cout << "Failed to open Main Camera" << endl; + } + } + + if((!fpvCamResult) && (!mainCamResult)) + { + cout << "Failed to Open Either Cameras, exiting" << endl; + return 1; + } + + CameraRGBImage fpvImg; + CameraRGBImage mainImg; + char fpvName[] = "FPV_CAM"; + char mainName[] = "MAIN_CAM"; + for(int i=0; i<1000; i++) + { + if(f && fpvCamResult && vehicle->advancedSensing->newFPVCameraImageIsReady()) + { + if(vehicle->advancedSensing->getFPVCameraImage(fpvImg)) + { + show_rgb(fpvImg, fpvName); + } + else + { + cout << "Time out" << endl; + } + } + + if(m && mainCamResult && vehicle->advancedSensing->newMainCameraImageReady()) + { + if(vehicle->advancedSensing->getMainCameraImage(mainImg)) + { + show_rgb(mainImg, mainName); + } + else + { + cout << "Time out" << endl; + } + } + usleep(2e4); + } + + if(f) + { + vehicle->advancedSensing->stopFPVCameraStream(); + } + else if(m) + { + vehicle->advancedSensing->stopMainCameraStream(); + } + + return 0; +} diff --git a/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/CMakeLists.txt b/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/CMakeLists.txt new file mode 100755 index 000000000..6393ea111 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 2.8) +project(camera-stream-target-tracking-sample) + +# Try to see if user has OpenCV installed +# if yes, default callback will display the image +find_package(OpenCV REQUIRED) +if (OpenCV_FOUND) + message( "\n${PROJECT_NAME}...") + message( STATUS " - Includes: ${OpenCV_INCLUDE_DIRS}") + message( STATUS " - Libraries: ${OpenCV_LIBRARIES}") +else() + message( STATUS "OpenCV is required for this sample to do the target tracking") +endif () + + +unset(KCFcpp_DIR) +find_path(KCFcpp_DIR + NAMES + KCFCpp.sh + PATHS + ${CMAKE_CURRENT_SOURCE_DIR}/KCFcpp + ) + +if(NOT KCFcpp_DIR) + message(STATUS "Target Tracking sample depends on KCFcpp library from https://github.com/joaofaro/KCFcpp.git") + message(STATUS "When you run make, we will try to clone the repo") + message(STATUS "This will happen only once as long as you do not delete it") + + include(${CMAKE_MODULE_PATH}/DownloadProject/DownloadProject.cmake) + download_project(PROJ kcf_cpp + GIT_REPOSITORY https://github.com/joaofaro/KCFcpp.git + GIT_TAG master + ${UPDATE_DISCONNECTED_IF_AVAILABLE} + SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/KCFcpp + ) + +endif() + + + +## To use this this repo, you need to run +## git clone https://github.com/joaofaro/KCFcpp.git +## under the current foler +include_directories(KCFcpp/src) +include_directories(${OpenCV_INCLUDE_DIRS}) + +add_library(kcf_tracker STATIC + KCFcpp/src/fhog.cpp + KCFcpp/src/kcftracker.cpp) +target_link_libraries(kcf_tracker ${OpenCV_LIBRARIES}) + +set(HELPER_FUNCTIONS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +add_executable(${PROJECT_NAME} + ${SOURCE_FILES} + ${HELPER_FUNCTIONS_DIR}/dji_linux_environment.cpp + ${HELPER_FUNCTIONS_DIR}/dji_linux_helpers.cpp + target_tracking.cpp + tracking_utility.cpp + ) +target_link_libraries(${PROJECT_NAME} + djiosdk-core + kcf_tracker + ${OpenCV_LIBRARIES} + ) + diff --git a/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/target_tracking.cpp b/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/target_tracking.cpp new file mode 100755 index 000000000..ab15d7064 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/target_tracking.cpp @@ -0,0 +1,202 @@ +/* + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "dji_vehicle.hpp" +#include +#include "dji_linux_helpers.hpp" +#include "tracking_utility.hpp" + +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "kcftracker.hpp" + +#include +#include + +typedef std::chrono::time_point timer; +typedef std::chrono::duration duration; + +using namespace DJI::OSDK; +using namespace std; +using namespace cv; + +int main(int argc, char** argv) +{ + // Setup OSDK. + bool enableAdvancedSensing = true; + LinuxSetup linuxEnvironment(argc, argv, enableAdvancedSensing); + Vehicle* vehicle = linuxEnvironment.getVehicle(); + const char *acm_dev = linuxEnvironment.getEnvironment()->getDeviceAcm().c_str(); + vehicle->advancedSensing->setAcmDevicePath(acm_dev); + if (vehicle == NULL) + { + std::cout << "Vehicle not initialized, exiting.\n"; + return -1; + } + + // Initialize variables + int functionTimeout = 1; + // Obtain Control Authority + vehicle->obtainCtrlAuthority(functionTimeout); + + bool mainCamResult = vehicle->advancedSensing->startMainCameraStream(); + if(!mainCamResult) + { + cout << "Failed to Open Camera" << endl; + return 1; + } + + CameraRGBImage mainImg; + const char winName[]="My Camera"; + char message1[100]; + char message2[100]; + Rect roi(0,0,0,0); + + KCFTracker *tracker = NULL; + TrackingUtility tu; + + cv::namedWindow(winName,1); + cv::setMouseCallback(winName,TrackingUtility::mouseCallback, (void*)&tu); + + while(1) + { + char c = cv::waitKey(10); + if(c==27) + { + if(tracker != NULL) + { + delete tracker; + tracker = NULL; + } + break; // Quit if ESC is pressed + } + + tu.getKey(c); //Internal states will be updated based on key pressed. + + if(vehicle->advancedSensing->newMainCameraImageReady()) + { + int dx = 0; + int dy = 0; + int yawRate = 0; + int pitchRate = 0; + timer trackerStartTime, trackerFinishTime; + duration trackerTimeDiff; + + vehicle->advancedSensing->getMainCameraImage(mainImg); + Mat frame(mainImg.height, mainImg.width, CV_8UC3, mainImg.rawData.data(), mainImg.width*3); + + + switch(tu.getState()) + { + case TrackingUtility::STATE_IDLE: + roi = tu.getROI(); + sprintf(message2, "Please select ROI and press g"); + break; + + case TrackingUtility::STATE_INIT: + cout << "g pressed, initialize tracker" << endl; + sprintf(message2, "g pressed, initialize tracker"); + roi = tu.getROI(); + tracker = new KCFTracker(true, true, false, false); + tracker->init(roi, frame); + tu.startTracker(); + break; + + case TrackingUtility::STATE_ONGOING: + trackerStartTime = std::chrono::high_resolution_clock::now(); + roi = tracker->update(frame); + trackerFinishTime = std::chrono::high_resolution_clock::now(); + trackerTimeDiff = trackerFinishTime - trackerStartTime; + sprintf(message2, "Tracking: bounding box update time = %.2f ms\n", trackerTimeDiff.count()*1000.0); + + // send gimbal speed command + dx = (int)(roi.x + roi.width/2 - mainImg.width/2); + dy = (int)(roi.y + roi.height/2 - mainImg.height/2); + + yawRate = dx; + pitchRate = -dy; + + if(abs(dx) < 10) + { + yawRate = 0; + } + + if(abs(dy) < 10) + { + pitchRate = 0; + } + + DJI::OSDK::Gimbal::SpeedData gimbalSpeed; + gimbalSpeed.roll = 0; + gimbalSpeed.pitch = pitchRate; + gimbalSpeed.yaw = yawRate; + gimbalSpeed.gimbal_control_authority = 1; + + vehicle->gimbal->setSpeed(&gimbalSpeed); + + break; + + case TrackingUtility::STATE_STOP: + cout << "s pressed, stop tracker" << endl; + sprintf(message2, "s pressed, stop tracker"); + delete tracker; + tracker = NULL; + tu.stopTracker(); + roi = tu.getROI(); + break; + + default: + break; + } + + dx = roi.x + roi.width/2 - mainImg.width/2; + dy = roi.y + roi.height/2 - mainImg.height/2; + + cv::circle(frame, Point(mainImg.width/2, mainImg.height/2), 5, cv::Scalar(255,0,0), 2, 8); + if(roi.width != 0) + { + cv::circle(frame, Point(roi.x + roi.width/2, roi.y + roi.height/2), 3, cv::Scalar(0,0,255), 1, 8); + + cv::line(frame, Point(mainImg.width/2, mainImg.height/2), + Point(roi.x + roi.width/2, roi.y + roi.height/2), + cv::Scalar(0,255,255)); + } + + cv::rectangle(frame, roi, cv::Scalar(0,255,0), 1, 8, 0 ); + cvtColor(frame, frame, COLOR_RGB2BGR); + sprintf(message1,"dx=%04d, dy=%04d",dx, dy); + putText(frame, message1, Point2f(20,30), FONT_HERSHEY_SIMPLEX, 1, Scalar(0,255,0)); + putText(frame, message2, Point2f(20,60), FONT_HERSHEY_SIMPLEX, 1, Scalar(0,255,0)); + cv::imshow(winName, frame); + + } + } + + vehicle->advancedSensing->stopMainCameraStream(); + + if(tracker) + { + delete tracker; + } + + return 0; +} diff --git a/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/tracking_utility.cpp b/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/tracking_utility.cpp new file mode 100755 index 000000000..1596aa44a --- /dev/null +++ b/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/tracking_utility.cpp @@ -0,0 +1,112 @@ +/* + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "tracking_utility.hpp" +#include +void TrackingUtility::mouseCallback(int event, int x, int y, int f, void *p) +{ + TrackingUtility *u = reinterpret_cast(p); + if(u->state != STATE_IDLE) + { + std::cout << "Currently tracking, press s key to stop" << std::endl; + return; + } + + switch(event) + { + case CV_EVENT_LBUTTONDOWN : + u->mouseClicked = true; + u->roiSelected = false; + u->P1 = cv::Point(x,y); + u->P2 = cv::Point(x,y); + break; + + case CV_EVENT_LBUTTONUP : + u->P2 = cv::Point(x,y); + u->mouseClicked=false; + if(u->P2 != u->P1) + { + u->roiSelected = true; + } + break; + + case CV_EVENT_MOUSEMOVE : + if(u->mouseClicked) + { + u->P2 = cv::Point(x,y); + } + break; + + default : + break; + } + + if(u->mouseClicked) + { + u->roi = cv::Rect(u->P1, u->P2); + printf("Current Region of Interest: %d, %d, %d, %d\n", u->roi.tl().x, u->roi.tl().y, u->roi.br().x, u->roi.br().y); + } +} + +cv::Rect TrackingUtility::getROI() +{ + return roi; +} + +TrackingUtility::TrackingState TrackingUtility::getState() +{ + return state; +} + +void TrackingUtility::startTracker() +{ + state = STATE_ONGOING; +} + +void TrackingUtility::stopTracker() +{ + state = STATE_IDLE; +} + +void TrackingUtility::getKey(char c) +{ + switch(c) + { + case 'g': + if( (state == STATE_IDLE) && (roiSelected == true)) + { + state = STATE_INIT; + } + break; + + case 's': + if( state == STATE_ONGOING ) + { + state = STATE_STOP; + roi = cv::Rect(0,0,0,0); //when we press s, should clear bounding box + } + break; + + default: + break; + } +} \ No newline at end of file diff --git a/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/tracking_utility.hpp b/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/tracking_utility.hpp new file mode 100755 index 000000000..4fc6b1046 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/camera_stream_target_tracking_sample/tracking_utility.hpp @@ -0,0 +1,56 @@ +// +// Created by zyli on 12/11/17. +// + +#ifndef ONBOARDSDK_TRACKINGUTILITY_HPP +#define ONBOARDSDK_TRACKINGUTILITY_HPP + +#include "opencv2/opencv.hpp" +/*! @brief + * The TrackingUtility Class handles simple start and stop tracking logic + */ +class TrackingUtility +{ +public: + TrackingUtility() + : P1(0,0), + P2(0,0), + roi(0,0,0,0), + mouseClicked(false), + roiSelected(false), + state(STATE_IDLE) + { + } + + typedef enum TrackingState + { + STATE_IDLE, + STATE_INIT, + STATE_ONGOING, + STATE_STOP + } TrackingState; + + static void mouseCallback(int event, int x, int y, int f, void *p); + + cv::Point P1; + cv::Point P2; + bool mouseClicked; + cv::Rect roi; + + /*! + * start_tracking is set true when you select a region and press 'g' + * is set to false when you press 's' + */ + bool roiSelected; + TrackingState state; + TrackingState getState(); + + void startTracker(); + void stopTracker(); + + cv::Rect getROI(); + void getKey(char c); +}; + + +#endif //ONBOARDSDK_TRACKINGUTILITY_HPP diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/CMakeLists.txt b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/CMakeLists.txt new file mode 100755 index 000000000..85c0f9fae --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/CMakeLists.txt @@ -0,0 +1,111 @@ +cmake_minimum_required(VERSION 2.8) +project(stereo-vision-depth-perception-sample) + +find_path(OPENCV_CONTRIB_IMG_PROC + NAMES + disparity_filter.hpp + PATHS + /usr/include/opencv2/ximgproc + /usr/local/include/opencv2/ximgproc + /opt/local/include/opencv2/ximgproc + /sw/include/opencv2/ximgproc + PATH_SUFFIXES + opencv2/ximgproc + ) + +find_path(OPENCV_VIZ + NAMES + viz3d.hpp + PATHS + /usr/include/opencv2/viz + /usr/local/include/opencv2/viz + /opt/local/include/opencv2/viz + /sw/include/opencv2/viz + PATH_SUFFIXES + opencv2/viz + ) + +# Try to see if user has OpenCV installed +# If there's version 3.3.0+, depth perception samples will be compiled +# If it's not version 3.3.0+, the sample will not be compiled +find_package( OpenCV 3.3.0 QUIET ) +if (OpenCV_FOUND) + message( "\n${PROJECT_NAME}...") + message( STATUS "Found OpenCV ${OpenCV_VERSION} installed in the system, will use it for depth perception sample") + message( STATUS " - Includes: ${OpenCV_INCLUDE_DIRS}") + message( STATUS " - Libraries: ${OpenCV_LIBRARIES}") + add_definitions(-DOPEN_CV_INSTALLED) + set(OPEN_CV_3_3_0_INSTALLED 1) + + if(NOT OPENCV_CONTRIB_IMG_PROC STREQUAL OPENCV_CONTRIB_IMG_PROC-NOTFOUND) + message(STATUS "Found ximgproc module in OpenCV, will use it to filter disparity map in depth perception sample") + add_definitions(-DUSE_OPEN_CV_CONTRIB) + else() + message(STATUS "Did not find ximgproc in OpenCV") + endif() + + if(NOT OPENCV_VIZ STREQUAL OPENCV_VIZ-NOTFOUND) + message(STATUS "Found viz3d in OpenCV, will use it to visualize point cloud") + set(FOUND_OPENCV_VIZ TRUE) + else() + message(STATUS "Did not find viz3d in OpenCV") + endif() +else() + find_package( OpenCV QUIET ) + if (OpenCV_FOUND) + message(STATUS "Found OpenCV ${OpenCV_VERSION} installed in the system, this sample requires 3.3.0+ for better experience") + message(STATUS " - Includes: ${OpenCV_INCLUDE_DIRS}") + add_definitions(-DOPEN_CV_INSTALLED) + else() + message(STATUS "Did not find OpenCV in the system, image data is inside RecvContainer as raw data") + endif() +endif() + + +find_package(CUDA QUIET) +if(CUDA_FOUND) + message(STATUS "Found ${CUDA_VERSION} CUDA installed in the system, will use it for depth perception sample") + message(STATUS " - Includes: ${CUDA_INCLUDE_DIRS}") + add_definitions(-DUSE_GPU) +elseif() + message(STATUS "Did not find CUDA in the system") +endif() + +set(HELPER_FUNCTIONS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +set(MULTI_THREAD_SAMPLE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../stereo_vision_multi_thread_sample) + +include_directories(utility) +include_directories(${MULTI_THREAD_SAMPLE_DIR}) + +if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ) + message(STATUS "Found OpenCV ${OpenCV_VERSION} and Viz3d module, stereo vision depth perception sample will be compiled.") + + FILE(GLOB DEPTH_PERCEPTION_SAMPLE_SOURCE_FILES + ${HELPER_FUNCTIONS_DIR}/dji_linux_environment.cpp + ${HELPER_FUNCTIONS_DIR}/dji_linux_helpers.cpp + stereo_vision_depth_perception_sample.cpp + ${MULTI_THREAD_SAMPLE_DIR}/image_process_container.cpp + ${MULTI_THREAD_SAMPLE_DIR}/utility_thread.cpp + stereo_process_container.cpp + utility/*.cpp + ) + + add_executable(${PROJECT_NAME} + ${SOURCE_FILES} + ${DEPTH_PERCEPTION_SAMPLE_SOURCE_FILES} + ) + + target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS}) + + target_link_libraries(${PROJECT_NAME} + djiosdk-core + ) + + if (OpenCV_FOUND) + target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ) + endif () +else() + message(STATUS "Did not find required libraries, stereo vision depth perception sample will not be compiled.") +endif () diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m210_stereo_param.yaml b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m210_stereo_param.yaml new file mode 100755 index 000000000..392b80be5 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m210_stereo_param.yaml @@ -0,0 +1,41 @@ +%YAML:1.0 +leftCameraIntrinsicMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 459.882338, 0.000000, 328.380255, 0.000000, 458.855806, 243.749739, 0.000000, 0.000000, 1.000000 ] +rightCameraIntrinsicMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 455.744183, 0.000000, 317.783292, 0.000000, 454.333521, 226.203923, 0.000000, 0.000000, 1.000000 ] +leftDistCoeffs: !!opencv-matrix + rows: 5 + cols: 1 + dt: d + data: [0.035379, 0.003641, -0.004073, -0.002172, 0.000000 ] +rightDistCoeffs: !!opencv-matrix + rows: 5 + cols: 1 + dt: d + data: [0.030189, -0.033546, -0.000356, -0.000101, 0.000000 ] +leftRectificationMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [0.999019, -0.023417, 0.037579, 0.023545, 0.999718, -0.002975, -0.037498, 0.003857, 0.999289 ] +rightRectificationMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 0.999839, -0.003815, 0.017534, 0.003755, 0.999987, 0.003450, -0.017547, -0.003383, 0.999840 ] +leftProjectionMatrix: !!opencv-matrix + rows: 3 + cols: 4 + dt: d + data: [430.807701, 0.000000, 303.590256, 0.000000, 0.000000, 430.807701, 233.501654, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000 ] +rightProjectionMatrix: !!opencv-matrix + rows: 3 + cols: 4 + dt: d + data: [ 430.807701, 0.000000, 303.590256, -48.272953, 0.000000, 430.807701, 233.501654, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000 ] diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_process_container.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_process_container.cpp new file mode 100755 index 000000000..c0788e2b2 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_process_container.cpp @@ -0,0 +1,252 @@ +/* + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "stereo_process_container.hpp" + +using namespace M210_STEREO; + +typedef std::chrono::time_point timer; +typedef std::chrono::duration duration; + +StereoProcessContainer::StereoProcessContainer(DJI::OSDK::Vehicle *vehicle) + : ImageProcessContainer(vehicle) + , is_disp_filterd(false) +{ + if(!this->initStereoFrame()) + { + DERROR("Failed to init camera parameter and stereo frame\n"); + } +} + +StereoProcessContainer::~StereoProcessContainer() +{ + +} + +bool +StereoProcessContainer::initStereoFrame() +{ + camera_left_ptr = CameraParam::createCameraParam(CameraParam::FRONT_LEFT); + camera_right_ptr = CameraParam::createCameraParam(CameraParam::FRONT_RIGHT); + + stereo_frame_ptr = StereoFrame::createStereoFrame(camera_left_ptr, camera_right_ptr); + + return true; +} + +void +StereoProcessContainer::displayStereoRectImgCallback(DJI::OSDK::Vehicle *vehiclePtr, + DJI::OSDK::UserData userData) +{ + StereoProcessContainer* this_ptr = (StereoProcessContainer*)userData; + + //! Read raw images + this_ptr->stereo_frame_ptr->readStereoImgs(this_ptr->stereoVGAImg); + + //! Rectify images + timer rectify_start = std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->rectifyImgs(); + timer rectify_end = std::chrono::high_resolution_clock::now(); + + this_ptr->visualizeRectImgHelper(); + + cv::waitKey(1); + + duration rectify_time_diff = rectify_end - rectify_start; + DSTATUS("This stereo frame takes %.2f ms to rectify", rectify_time_diff.count()*1000.0); +} + +void +StereoProcessContainer::displayStereoDisparityCallback(DJI::OSDK::Vehicle *vehiclePtr, + DJI::OSDK::UserData userData) +{ + StereoProcessContainer* this_ptr = (StereoProcessContainer*)userData; + + //! Read raw images + this_ptr->stereo_frame_ptr->readStereoImgs(this_ptr->stereoVGAImg); + + //! Rectify images + timer rectify_start = std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->rectifyImgs(); + timer rectify_end = std::chrono::high_resolution_clock::now(); + + //! Compute disparity + timer disp_start = std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->computeDisparityMap(); + timer disp_end = std::chrono::high_resolution_clock::now(); + + this_ptr->visualizeRectImgHelper(); + + this_ptr->visualizeDisparityMapHelper(); + + cv::waitKey(1); + + duration rectify_time_diff = rectify_end - rectify_start; + duration disp_time_diff = disp_end - disp_start; + DSTATUS("This stereo frame takes %.2f ms to rectify, %.2f ms to compute disparity", + rectify_time_diff.count()*1000.0, + disp_time_diff.count()*1000.0); +} + +void +StereoProcessContainer::displayStereoFilteredDisparityCallback(DJI::OSDK::Vehicle *vehiclePtr, + DJI::OSDK::UserData userData) +{ +#ifdef USE_OPEN_CV_CONTRIB + + StereoProcessContainer* this_ptr = (StereoProcessContainer*)userData; + + //! Read raw images + this_ptr->stereo_frame_ptr->readStereoImgs(this_ptr->stereoVGAImg); + + //! Rectify images + timer rectify_start = std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->rectifyImgs(); + timer rectify_end = std::chrono::high_resolution_clock::now(); + + //! Compute disparity + timer disp_start = std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->computeDisparityMap(); + timer disp_end = std::chrono::high_resolution_clock::now(); + + //! Filter disparity map + timer filter_start = std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->filterDisparityMap(); + this_ptr->is_disp_filterd = true; + timer filter_end = std::chrono::high_resolution_clock::now(); + + this_ptr->visualizeRectImgHelper(); + + this_ptr->visualizeDisparityMapHelper(); + + cv::waitKey(1); + + duration rectify_time_diff = rectify_end - rectify_start; + duration disp_time_diff = disp_end - disp_start; + duration filter_diff = filter_end - filter_start; + DSTATUS("This stereo frame takes %.2f ms to rectify, %.2f ms to compute disparity, " + "%.2f ms to filter", + rectify_time_diff.count()*1000.0, + disp_time_diff.count()*1000.0, + filter_diff.count()*1000.0); +#else + DSTATUS("OpenCV ximgproc module is not found. It's required for disparity map filtering"); +#endif + +} + +void +StereoProcessContainer::displayStereoPtCloudCallback(DJI::OSDK::Vehicle *vehiclePtr, + DJI::OSDK::UserData userData) +{ + StereoProcessContainer* this_ptr = (StereoProcessContainer*)userData; + + //! Read raw images + this_ptr->stereo_frame_ptr->readStereoImgs(this_ptr->stereoVGAImg); + + //! Rectify images + timer rectify_start = std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->rectifyImgs(); + timer rectify_end = std::chrono::high_resolution_clock::now(); + + //! Compute disparity + timer disp_start = std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->computeDisparityMap(); + timer disp_end = std::chrono::high_resolution_clock::now(); + + //! Filter disparity map + timer filter_start= std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->filterDisparityMap(); + this_ptr->is_disp_filterd = true; + timer filter_end = std::chrono::high_resolution_clock::now(); + + //! Unproject image to 3D point cloud + timer pt_cloud_start= std::chrono::high_resolution_clock::now(); + this_ptr->stereo_frame_ptr->unprojectPtCloud(); + timer pt_cloud_end = std::chrono::high_resolution_clock::now(); + + cv::viz::WCloud pt_cloud = this_ptr->stereo_frame_ptr->getPtCloud(); + PointCloudViewer::showPointCloud(pt_cloud); + PointCloudViewer::spinOnce(); + + this_ptr->visualizeRectImgHelper(); + + this_ptr->visualizeDisparityMapHelper(); + + cv::waitKey(1); + + duration rectify_time_diff = rectify_end - rectify_start; + duration disp_time_diff = disp_end - disp_start; + duration filter_diff = filter_end - filter_start; + duration pt_cloud_diff = pt_cloud_end - pt_cloud_start; + DSTATUS("This stereo frame takes %.2f ms to rectify, %.2f ms to compute disparity, " + "%.2f ms to filter, %.2f ms to unproject point cloud", + rectify_time_diff.count()*1000.0, + disp_time_diff.count()*1000.0, + filter_diff.count()*1000.0, + pt_cloud_diff.count()*1000.0); +} + +void +StereoProcessContainer::visualizeRectImgHelper() +{ + cv::Mat img_to_show; + + cv::hconcat(stereo_frame_ptr->getRectLeftImg(), + stereo_frame_ptr->getRectRightImg(), + img_to_show); + + cv::resize(img_to_show, img_to_show, + cv::Size(VGA_WIDTH*2, VGA_HEIGHT), + (0, 0), (0, 0), cv::INTER_LINEAR); + + // draw epipolar lines to visualize rectification + for(int j = 0; j < img_to_show.rows; j += 24 ){ + line(img_to_show, cv::Point(0, j), + cv::Point(img_to_show.cols, j), + cv::Scalar(255, 0, 0, 255), 1, 8); + } + + cv::imshow("Rectified Stereo Imgs with epipolar lines", img_to_show); +} + +void +StereoProcessContainer::visualizeDisparityMapHelper() +{ + cv::Mat raw_disp_map; +#ifdef USE_OPEN_CV_CONTRIB + if(is_disp_filterd) { + raw_disp_map = stereo_frame_ptr->getFilteredDispMap().clone(); + } else { + raw_disp_map = stereo_frame_ptr->getDisparityMap().clone(); + } +#else + raw_disp_map = stereo_frame_ptr->getDisparityMap().clone(); +#endif + + double min_val, max_val; + cv::minMaxLoc(raw_disp_map, &min_val, &max_val, NULL, NULL); + + cv::Mat scaled_disp_map; + raw_disp_map.convertTo(scaled_disp_map, CV_8U, 255/(max_val-min_val), -min_val/(max_val-min_val)); + + cv::imshow("Scaled disparity map", scaled_disp_map); +} \ No newline at end of file diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_process_container.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_process_container.hpp new file mode 100755 index 000000000..092a8b14c --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_process_container.hpp @@ -0,0 +1,50 @@ +#ifndef ONBOARDSDK_STEREO_PROCESS_CONTAINER_H +#define ONBOARDSDK_STEREO_PROCESS_CONTAINER_H + +#include "image_process_container.hpp" +#include "stereo_frame.hpp" +#include "camera_param.hpp" +#include + + +class StereoProcessContainer : public ImageProcessContainer +{ +public: + StereoProcessContainer(DJI::OSDK::Vehicle *vehicle); + virtual ~StereoProcessContainer(); + +public: + bool initStereoFrame(); + + static void displayStereoRectImgCallback(DJI::OSDK::Vehicle *vehiclePtr, + DJI::OSDK::UserData userData); + + static void displayStereoDisparityCallback(DJI::OSDK::Vehicle *vehiclePtr, + DJI::OSDK::UserData userData); + + static void displayStereoFilteredDisparityCallback(DJI::OSDK::Vehicle *vehiclePtr, + DJI::OSDK::UserData userData); + + static void displayStereoPtCloudCallback(DJI::OSDK::Vehicle *vehiclePtr, + DJI::OSDK::UserData userData); + +protected: + void visualizeRectImgHelper(); + + // The disparity map return by stereoFrame is the actual pixel diff + // The image will be very dimmed if visualize directly + // this function scales it for visualization purpose + void visualizeDisparityMapHelper(); + +protected: + M210_STEREO::CameraParam::Ptr camera_left_ptr; + M210_STEREO::CameraParam::Ptr camera_right_ptr; + + M210_STEREO::StereoFrame::Ptr stereo_frame_ptr; + + // for visualization purpose + bool is_disp_filterd; +}; + + +#endif //ONBOARDSDK_STEREO_PROCESS_CONTAINER_H diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_vision_depth_perception_sample.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_vision_depth_perception_sample.cpp new file mode 100755 index 000000000..b450130e0 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_vision_depth_perception_sample.cpp @@ -0,0 +1,147 @@ +/* + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "stereo_vision_depth_perception_sample.hpp" + +using namespace DJI::OSDK; + +int +main(int argc, char** argv) +{ + if(argc >= 3){ + DSTATUS("Input yaml file: %s\n", argv[2]); + } else{ + DERROR("Please specify a yaml file with camera parameters\n"); + DERROR("Ex: ./stereo-vision-depth-perception-sample UserConfig.txt m210_stereo_param.yaml\n"); + return -1; + } + + // Setup OSDK. + bool enableAdvancedSensing = true; + LinuxSetup linuxEnvironment(argc, argv, enableAdvancedSensing); + Vehicle* vehicle = linuxEnvironment.getVehicle(); + if (vehicle == NULL) + { + std::cout << "Vehicle not initialized, exiting.\n"; + return -1; + } + + // Initialize variables + int functionTimeout = 1; + // Obtain Control Authority + vehicle->obtainCtrlAuthority(functionTimeout); + + // Display interactive prompt + std::cout + << std::endl + << " AdvancedSensing API works in subscription mechanism \n" + << " Please remember to unsubscribe when the program terminates \n" + << " If any error messages occur, please reboot the aircraft \n" + << std::endl + << "| Available commands: |\n" + << "| [a] Display rectified stereo images |\n" + << "| [b] Display disparity map |\n" + << "| [c] Display filtered disparity map |\n" + << "| [d] Display point cloud |\n" + << "| [e] Unsubscribe to VGA front stereo images |\n" + << std::endl; + char inputChar = ' '; + std::cin >> inputChar; + + std::string yaml_file_path = argv[2]; + M210_STEREO::Config::setParamFile(yaml_file_path); + + image_process_container_ptr = new StereoProcessContainer(vehicle); + + switch (inputChar) + { + case 'a': + { + // register a callback for "image processing" thread + image_process_container_ptr->setCallbackHandler(&StereoProcessContainer::displayStereoRectImgCallback, + dynamic_cast(image_process_container_ptr)); + // register a callback for "image reading" thread + vehicle->advancedSensing->subscribeFrontStereoVGA(AdvancedSensingProtocol::FREQ_20HZ, &storeStereoImgVGACallback, NULL); + } + break; + case 'b': + { + // register a callback for "image processing" thread + image_process_container_ptr->setCallbackHandler(&StereoProcessContainer::displayStereoDisparityCallback, + dynamic_cast(image_process_container_ptr)); + // register a callback for "image reading" thread + vehicle->advancedSensing->subscribeFrontStereoVGA(AdvancedSensingProtocol::FREQ_20HZ, &storeStereoImgVGACallback, NULL); + } + break; + case 'c': + { + // register a callback for "image processing" thread + image_process_container_ptr->setCallbackHandler(&StereoProcessContainer::displayStereoFilteredDisparityCallback, + dynamic_cast(image_process_container_ptr)); + // register a callback for "image reading" thread + vehicle->advancedSensing->subscribeFrontStereoVGA(AdvancedSensingProtocol::FREQ_20HZ, &storeStereoImgVGACallback, NULL); + } + break; + case 'd': + { + // register a callback for "image processing" thread + image_process_container_ptr->setCallbackHandler(&StereoProcessContainer::displayStereoPtCloudCallback, + dynamic_cast(image_process_container_ptr)); + // register a callback for "image reading" thread + vehicle->advancedSensing->subscribeFrontStereoVGA(AdvancedSensingProtocol::FREQ_20HZ, &storeStereoImgVGACallback, NULL); + } + break; + case 'e': + { + vehicle->advancedSensing->unsubscribeVGAImages(); + } + break; + default: + break; + } + + DSTATUS("AdvancedSensing sample sleeps 15 seconds here"); + sleep(15); + + DSTATUS("Unsubscribe VGA images"); + vehicle->advancedSensing->unsubscribeVGAImages(); + + sleep(1); + DSTATUS("waited 1 second for the image subscription to stop completely\n"); + + return 0; +} + +//! @note This callback is running on reading thread. +//! Another thread is created inside ImageProcessContainer. +//! Whenever the images are copied, another callback on +//! processing thread will perform the image computation +void storeStereoImgVGACallback(Vehicle *vehiclePtr, RecvContainer recvFrame, UserData userData) +{ + DSTATUS("sample VGACallback receive an image at frame: %d and time stamp: %d", + recvFrame.recvData.stereoVGAImgData->frame_index, + recvFrame.recvData.stereoVGAImgData->time_stamp); + + // copy the image data to image process container + // a boolean is flipped to inform the polling + // mechanism in image process thread + image_process_container_ptr->copyVGAImg(recvFrame.recvData.stereoVGAImgData); +} diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_vision_depth_perception_sample.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_vision_depth_perception_sample.hpp new file mode 100755 index 000000000..3407550be --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_vision_depth_perception_sample.hpp @@ -0,0 +1,27 @@ +#ifndef ONBOARDSDK_ADVANCED_SENSING_DEPTH_PERCEPTION_SAMPLE_HPP +#define ONBOARDSDK_ADVANCED_SENSING_DEPTH_PERCEPTION_SAMPLE_HPP + +// DJI OSDK includes +#include "dji_vehicle.hpp" + +// C++ +#include + +// Helpers +#include + +// Third party library +#ifdef OPEN_CV_INSTALLED +#include +#endif + +// Utility +#include "utility_thread.hpp" +#include "stereo_process_container.hpp" + +ImageProcessContainer* image_process_container_ptr; + +static void storeStereoImgVGACallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::RecvContainer recvFrame, DJI::OSDK::UserData userData); + + +#endif //ONBOARDSDK_ADVANCED_SENSING_DEPTH_PERCEPTION_SAMPLE_HPP diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/camera_param.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/camera_param.cpp new file mode 100755 index 000000000..614f642e9 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/camera_param.cpp @@ -0,0 +1,86 @@ +/* + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "camera_param.hpp" + +using namespace M210_STEREO; + +CameraParam::CameraParam(uint8_t location) + : location_(location) +{ + if(!initIntrinsicParam()) + { + std::cerr << "Failed to init intrinsic parameter"; + } + + if(!initDistortionParam()) + { + std::cerr << "Failed to init distortion parameter"; + } +} + +CameraParam::~CameraParam() +{ + +} + +CameraParam::Ptr CameraParam::createCameraParam(uint8_t position) +{ + return std::make_shared(position); +} + +bool +CameraParam::initDistortionParam() +{ + if(location_ == FRONT_LEFT) + { + param_distortion_ = Config::get("leftDistCoeffs"); + } + else if(location_ == FRONT_RIGHT) + { + param_distortion_ = Config::get("rightDistCoeffs"); + } + else + { + std::cerr << "Please specify the location of the camera\n"; + return false; + } + return true; +} + +bool +CameraParam::initIntrinsicParam() +{ + if(location_ == FRONT_LEFT) + { + param_intrinsic_ = Config::get("leftCameraIntrinsicMatrix"); + } + else if(location_ == FRONT_RIGHT) + { + param_intrinsic_ = Config::get("rightCameraIntrinsicMatrix"); + } + else + { + std::cerr << "Please specify the location of the camera\n"; + return false; + } + return true; +} diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/camera_param.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/camera_param.hpp new file mode 100755 index 000000000..8029e89f3 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/camera_param.hpp @@ -0,0 +1,58 @@ +#ifndef ONBOARDSDK_CAMERA_H +#define ONBOARDSDK_CAMERA_H + +#include +#include +#include "config.hpp" + +namespace M210_STEREO +{ + +class CameraParam +{ +public: + enum POSITION + { + FRONT_LEFT = 0, + FRONT_RIGHT = 1, + DOWN_LEFT = 2, + DOWN_RIGHT = 3 + }; + + typedef std::shared_ptr Ptr; + + CameraParam(uint8_t position); + ~CameraParam(); + +public: + static CameraParam::Ptr createCameraParam(uint8_t position); + + inline cv::Mat getIntrinsic() { return this->param_intrinsic_; } + + inline cv::Mat getDistortion() { return this->param_distortion_; } + + inline double getPrincipalX() { return this->param_intrinsic_.at(0, 2); } + + inline double getPrincipalY() { return this->param_intrinsic_.at(1, 2); } + + inline double getFocalX() { return this->param_intrinsic_.at(0, 0); } + + inline double getFocalY() { return this->param_intrinsic_.at(1, 1); } + +protected: + bool initIntrinsicParam(); + + bool initDistortionParam(); + +protected: + cv::Mat param_intrinsic_; + cv::Mat param_distortion_; + float depth_scale_; + uint8_t location_; +}; + +} // namespace M210_STEREO + + + +#endif //ONBOARDSDK_CAMERA_H diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/config.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/config.cpp new file mode 100755 index 000000000..3298274fb --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/config.cpp @@ -0,0 +1,67 @@ +/* + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "config.hpp" + +using namespace M210_STEREO; + +Config* Config::single_instance_ = NULL; + +Config::Config() +{ +} + +Config::~Config() +{ + if (file_.isOpened()) + { + file_.release(); + } +} + +Config& +Config::instance() +{ + return *Config::single_instance_; +} + +Config* +Config::instancePtr() +{ + return Config::single_instance_; +} + +void +Config::setParamFile(const std::string& file_name) +{ + if(!Config::single_instance_) + { + Config::single_instance_ = new Config(); + } + + Config::instancePtr()->file_ = cv::FileStorage( file_name, cv::FileStorage::READ ); + + if(!Config::instancePtr()->file_.isOpened()) + { + std::cerr << "Failed to open " << file_name << " file\n"; + Config::instancePtr()->file_.release(); + } +} diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/config.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/config.hpp new file mode 100755 index 000000000..75bf25f8b --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/config.hpp @@ -0,0 +1,42 @@ +#ifndef ONBOARDSDK_CONFIG_H +#define ONBOARDSDK_CONFIG_H + +#include +#include + +namespace M210_STEREO +{ + +class Config +{ +private: + Config(); + +public: + ~Config(); + + static Config& instance(); + + static Config* instancePtr(); + + static void setParamFile(const std::string& file_name); + + template + static T get(const std::string& key) + { + T t; + Config::instancePtr()->file_[key] >> t; + return t; + } + +private: + static Config* single_instance_; + + cv::FileStorage file_; + +}; + +} // namespace M210_STEREO + + +#endif //ONBOARDSDK_CONFIG_H diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/frame.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/frame.hpp new file mode 100755 index 000000000..24caa52c3 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/frame.hpp @@ -0,0 +1,36 @@ +#ifndef ONBOARDSDK_FRAME_H +#define ONBOARDSDK_FRAME_H + + +#include +#include + +namespace M210_STEREO +{ + static const int VGA_HEIGHT = 480; + static const int VGA_WIDTH = 640; + +struct Frame +{ + typedef std::shared_ptr Ptr; + + Frame(uint64_t id, uint32_t time_stamp, cv::Mat img) + : id(id) + , time_stamp(time_stamp) + , raw_image(img) {}; + + static Frame::Ptr createFrame(uint64_t id, uint32_t time_stamp, cv::Mat img) + { + return std::make_shared(id, time_stamp, img); + } + + inline cv::Mat getImg() { return this->raw_image; } + + uint64_t id; + uint32_t time_stamp; + cv::Mat raw_image; +}; + +} // namespace M210_STEREO + +#endif //ONBOARDSDK_FRAME_H diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/point_cloud_viewer.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/point_cloud_viewer.cpp new file mode 100755 index 000000000..e6a21d540 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/point_cloud_viewer.cpp @@ -0,0 +1,71 @@ +/* + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "point_cloud_viewer.hpp" + +using namespace M210_STEREO; + +PointCloudViewer* PointCloudViewer::single_instance_ = new PointCloudViewer("Point Cloud Viewer"); + +PointCloudViewer::PointCloudViewer(const std::string &name) + : viewer_(name) + , view_pos_(-1.0, -2.0, -3.0) + , view_point_(0, 0, 0) + , view_y_dir_(0, 1, 0) + , world_coordinate_(.3) +{ + cv::Affine3d view_pose = cv::viz::makeCameraPose(view_pos_, + view_point_, + view_y_dir_); + viewer_.setViewerPose(view_pose); + + world_coordinate_.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0); + + viewer_.showWidget("World", world_coordinate_); + cv::Affine3d M = cv::Affine3d::Identity(); + viewer_.setWidgetPose("World", M ); +} + +PointCloudViewer::~PointCloudViewer() +{ + +} + +PointCloudViewer& +PointCloudViewer::instance() +{ + return *PointCloudViewer::single_instance_; +} + +PointCloudViewer* +PointCloudViewer::instancePtr() +{ + return PointCloudViewer::single_instance_; +} + +void +PointCloudViewer::showPointCloud(cv::viz::WCloud &cloud) +{ + PointCloudViewer *this_ptr = PointCloudViewer::instancePtr(); + + cloud.setRenderingProperty( cv::viz::POINT_SIZE, 3); + this_ptr->viewer_.showWidget("Cloud", cloud); +} \ No newline at end of file diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/point_cloud_viewer.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/point_cloud_viewer.hpp new file mode 100755 index 000000000..828594d9d --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/point_cloud_viewer.hpp @@ -0,0 +1,41 @@ +#ifndef ONBOARDSDK_POINT_CLOUD_VIEWER_HPP +#define ONBOARDSDK_POINT_CLOUD_VIEWER_HPP + +#include +#include +#include + +namespace M210_STEREO +{ + +class PointCloudViewer +{ +public: + ~PointCloudViewer(); + + static void showPointCloud(cv::viz::WCloud &cloud); + + static inline void spinOnce() { PointCloudViewer::instancePtr()->viewer_.spinOnce(); }; + + static PointCloudViewer& instance(); + + static PointCloudViewer* instancePtr(); + +private: + PointCloudViewer(const std::string &name); + +private: + static PointCloudViewer* single_instance_; + + cv::viz::Viz3d viewer_; + + cv::Point3d view_pos_; + cv::Point3d view_point_; + cv::Point3d view_y_dir_; + + cv::viz::WCoordinateSystem world_coordinate_; +}; + +} // namespace M210_STEREO + +#endif //ONBOARDSDK_POINT_CLOUD_VIEWER_HPP diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/stereo_frame.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/stereo_frame.cpp new file mode 100755 index 000000000..99724cb0e --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/stereo_frame.cpp @@ -0,0 +1,238 @@ +/* + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "stereo_frame.hpp" + +using namespace M210_STEREO; +using namespace cv; + +StereoFrame::StereoFrame(CameraParam::Ptr left_cam, + CameraParam::Ptr right_cam, + int num_disp, int block_size) + : camera_left_ptr_(left_cam) + , camera_right_ptr_(right_cam) + , num_disp_(num_disp) + , block_size_(block_size) + , color_buffer_(VGA_WIDTH*VGA_HEIGHT) + , mat_vec3_pt_(VGA_HEIGHT, VGA_WIDTH, Vec3f(0, 0, 0)) + , color_mat_(VGA_HEIGHT, VGA_WIDTH, CV_8UC1, &color_buffer_[0]) + , pt_cloud_(mat_vec3_pt_, color_mat_) + , raw_disparity_map_(Mat(VGA_HEIGHT, VGA_WIDTH, CV_16SC1)) +{ + if(!this->initStereoParam()) + { + DERROR("Failed to init stereo parameters\n"); + } + + Mat m210_vga_stereo_left = Mat(VGA_HEIGHT,VGA_WIDTH, CV_8U); + Mat m210_vga_stereo_right = Mat(VGA_HEIGHT,VGA_WIDTH, CV_8U); + + frame_left_ptr_ = Frame::createFrame(0, 0, m210_vga_stereo_left); + frame_right_ptr_ = Frame::createFrame(0, 0, m210_vga_stereo_right); +} + +StereoFrame::~StereoFrame() +{ + +} + +bool +StereoFrame::initStereoParam() +{ + param_rect_left_ = Config::get("leftRectificationMatrix"); + param_rect_right_ = Config::get("rightRectificationMatrix"); + param_proj_left_ = Config::get("leftProjectionMatrix"); + param_proj_right_ = Config::get("rightProjectionMatrix"); + + principal_x_ = param_proj_left_.at(0, 2); + principal_y_ = param_proj_left_.at(1, 2); + fx_ = param_proj_left_.at(0, 0); + fy_ = param_proj_left_.at(1, 1); + baseline_x_fx_ = -param_proj_right_.at(0, 3); + + initUndistortRectifyMap(camera_left_ptr_->getIntrinsic(), + camera_left_ptr_->getDistortion(), + param_rect_left_, + param_proj_left_, + Size(VGA_WIDTH, VGA_HEIGHT), CV_32F, + rectified_mapping_[0][0], rectified_mapping_[0][1]); + initUndistortRectifyMap(camera_right_ptr_->getIntrinsic(), + camera_right_ptr_->getDistortion(), + param_rect_right_, + param_proj_right_, + Size(VGA_WIDTH, VGA_HEIGHT), CV_32F, + rectified_mapping_[1][0], rectified_mapping_[1][1]); + +#ifdef USE_GPU + for (int k = 0; k < 2; ++k) { + for (int i = 0; i < 2; ++i) { + cuda_rectified_mapping_[k][i].upload(rectified_mapping_[k][i]); + } + } + + block_matcher_ = cuda::createStereoBM(num_disp_, block_size_); +#else + block_matcher_ = StereoBM::create(num_disp_, block_size_); +#endif + +#ifdef USE_OPEN_CV_CONTRIB + wls_filter_ = ximgproc::createDisparityWLSFilter(block_matcher_); // left_matcher + wls_filter_->setLambda(8000.0); + wls_filter_->setSigmaColor(1.5); + + right_matcher_ = ximgproc::createRightMatcher(block_matcher_); +#endif + + return true; +} + +StereoFrame::Ptr +StereoFrame::createStereoFrame(CameraParam::Ptr left_cam, CameraParam::Ptr right_cam) +{ + return std::make_shared(left_cam, right_cam); +} + +void +StereoFrame::readStereoImgs(const DJI::OSDK::ACK::StereoVGAImgData &imgs) +{ + memcpy(frame_left_ptr_->raw_image.data, + imgs.img_vec[0], sizeof(char)*VGA_HEIGHT*VGA_WIDTH); + memcpy(frame_right_ptr_->raw_image.data, + imgs.img_vec[1], sizeof(char)*VGA_HEIGHT*VGA_WIDTH); + + frame_left_ptr_-> id = imgs.frame_index; + frame_right_ptr_->id = imgs.frame_index; + frame_left_ptr_-> time_stamp = imgs.time_stamp; + frame_right_ptr_->time_stamp = imgs.time_stamp; +} + +void +StereoFrame::rectifyImgs() +{ +#ifdef USE_GPU + cuda_rect_src.upload(frame_left_ptr_->getImg()); + cuda::remap(cuda_rect_src, cuda_rectified_img_left_, + cuda_rectified_mapping_[0][0], + cuda_rectified_mapping_[0][1], + INTER_LINEAR); + cuda_rectified_img_left_.download(rectified_img_left_); + + cuda_rect_src.upload(frame_right_ptr_->getImg()); + cuda::remap(cuda_rect_src, cuda_rectified_img_right_, + cuda_rectified_mapping_[1][0], + cuda_rectified_mapping_[1][1], + INTER_LINEAR); + cuda_rectified_img_right_.download(rectified_img_right_); +#else + remap(frame_left_ptr_->getImg(), rectified_img_left_, + rectified_mapping_[0][0], rectified_mapping_[0][1], INTER_LINEAR); + remap(frame_right_ptr_->getImg(), rectified_img_right_, + rectified_mapping_[1][0], rectified_mapping_[1][1], INTER_LINEAR); +#endif +} + +void +StereoFrame::computeDisparityMap() +{ + +#ifdef USE_GPU + cuda::GpuMat cuda_disp_left; + + // GPU implementation of stereoBM outputs uint8_t, i.e. CV_8U + block_matcher_->compute(cuda_rectified_img_left_.clone(), + cuda_rectified_img_right_.clone(), + cuda_disp_left); + + cuda_disp_left.download(raw_disparity_map_); + + raw_disparity_map_.convertTo(disparity_map_8u_, CV_8UC1, 1); + + // convert it from CV_8U to CV_16U for unified + // calculation in filterDisparityMap() & unprojectPtCloud() + raw_disparity_map_.convertTo(raw_disparity_map_, CV_16S, 16); +#else + // CPU implementation of stereoBM outputs short int, i.e. CV_16S + block_matcher_->compute(rectified_img_left_, rectified_img_right_, raw_disparity_map_); + + raw_disparity_map_.convertTo(disparity_map_8u_, CV_8UC1, 0.0625); +#endif + +} + +void +StereoFrame::filterDisparityMap() +{ +#ifdef USE_OPEN_CV_CONTRIB + right_matcher_->compute(rectified_img_right_, rectified_img_left_, raw_right_disparity_map_); + + // Only takes CV_16S type cv::Mat + wls_filter_->filter(raw_disparity_map_, + rectified_img_left_, + filtered_disparity_map_, + raw_right_disparity_map_); + + filtered_disparity_map_.convertTo(filtered_disparity_map_8u_, CV_8UC1, 0.0625); +#endif +} + +void +StereoFrame::unprojectPtCloud() +{ + // due to rectification, the image boarder are blank + // we cut them out + const int border_size = num_disp_; + const int trunc_img_width_end = VGA_WIDTH - border_size; + const int trunc_img_height_end = VGA_HEIGHT - border_size; + + mat_vec3_pt_ = Mat_(VGA_HEIGHT, VGA_WIDTH, Vec3f(0, 0, 0)); + + for(int v = border_size; v < trunc_img_height_end; ++v) + { + for(int u = border_size; u < trunc_img_width_end; ++u) + { + Vec3f &point = mat_vec3_pt_.at(v, u); + +#ifdef USE_OPEN_CV_CONTRIB + float disparity = (float)(filtered_disparity_map_.at(v, u)*0.0625); +#else + float disparity = (float)(raw_disparity_map_.at(v, u)*0.0625); +#endif + + // do not consider pts that are farther than 8.6m, i.e. disparity < 6 + if(disparity >= 6) + { + point[2] = baseline_x_fx_/disparity; + point[0] = (u-principal_x_)*point[2]/fx_; + point[1] = (v-principal_y_)*point[2]/fy_; + } + color_buffer_[v*VGA_WIDTH+u] = rectified_img_left_.at(v, u); + } + } + + color_mat_ = cv::Mat(VGA_HEIGHT, VGA_WIDTH, CV_8UC1, &color_buffer_[0]).clone(); + + // @note Unfortunately, calling this WCloud constructor costs about the same amount + // of time as we go through each pixel and unproject the pt cloud. Because there's + // another nested for-loop inside WCloud implementation + // Ideally these can be done in one shot but it involves changing openCV implementation + // TODO maybe opencv projectPoints() is a good alternative + pt_cloud_ = viz::WCloud(mat_vec3_pt_, color_mat_); +} \ No newline at end of file diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/stereo_frame.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/stereo_frame.hpp new file mode 100755 index 000000000..7fed93530 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/utility/stereo_frame.hpp @@ -0,0 +1,122 @@ +#ifndef ONBOARDSDK_STEREO_FRAME_H +#define ONBOARDSDK_STEREO_FRAME_H + +#include +#include +#include "frame.hpp" +#include "camera_param.hpp" +#include "dji_ack.hpp" +#include "dji_log.hpp" +#include "point_cloud_viewer.hpp" + +#ifdef USE_GPU + #include + #include +#endif + +#ifdef USE_OPEN_CV_CONTRIB + #include +#endif + +namespace M210_STEREO +{ + +class StereoFrame +{ +public: + typedef std::shared_ptr Ptr; + + StereoFrame(CameraParam::Ptr left_cam, CameraParam::Ptr right_cam, + int num_disp = 64, int block_size = 13); + ~StereoFrame(); + +public: + static StereoFrame::Ptr createStereoFrame(CameraParam::Ptr left_cam, + CameraParam::Ptr right_cam); + + void readStereoImgs(const DJI::OSDK::ACK::StereoVGAImgData &imgs); + + void rectifyImgs(); + + void computeDisparityMap(); + + void filterDisparityMap(); + + void unprojectPtCloud(); + + inline cv::Mat getRectLeftImg() { return this->rectified_img_left_; } + + inline cv::Mat getRectRightImg() { return this->rectified_img_right_; } + + inline cv::Mat getDisparityMap() { return this->disparity_map_8u_; } + + inline cv::viz::WCloud getPtCloud() { return this->pt_cloud_; } + +#ifdef USE_OPEN_CV_CONTRIB + inline cv::Mat getFilteredDispMap() { return this->filtered_disparity_map_8u_; } +#endif + +protected: + bool initStereoParam(); + +protected: + //! Image frames + Frame::Ptr frame_left_ptr_; + Frame::Ptr frame_right_ptr_; + + //! Camera related + CameraParam::Ptr camera_left_ptr_; + CameraParam::Ptr camera_right_ptr_; + + //! Stereo camera parameters + cv::Mat param_rect_left_; + cv::Mat param_rect_right_; + cv::Mat param_proj_left_; + cv::Mat param_proj_right_; + + cv::Mat rectified_mapping_[2][2]; + + //! Rectified images + cv::Mat rectified_img_left_; + cv::Mat rectified_img_right_; + + //! Block matching related + int num_disp_; + int block_size_; + cv::Ptr block_matcher_; + cv::Mat disparity_map_8u_; + cv::Mat raw_disparity_map_; + + //! Point Cloud related + double principal_x_; + double principal_y_; + double fx_; + double fy_; + double baseline_x_fx_; + std::vector color_buffer_; + cv::Mat color_mat_; + cv::Mat_ mat_vec3_pt_; + cv::viz::WCloud pt_cloud_; + +#ifdef USE_GPU + cv::cuda::GpuMat cuda_rectified_mapping_[2][2]; + + cv::cuda::GpuMat cuda_rect_src; + cv::cuda::GpuMat cuda_rectified_img_left_; + cv::cuda::GpuMat cuda_rectified_img_right_; + cv::cuda::GpuMat cuda_disparity_map_8u; +#endif + +#ifdef USE_OPEN_CV_CONTRIB + cv::Ptr wls_filter_; + cv::Ptr right_matcher_; + + cv::Mat raw_right_disparity_map_; + cv::Mat filtered_disparity_map_; + cv::Mat filtered_disparity_map_8u_; +#endif + +}; + +} // namespace M210_STEREO +#endif //ONBOARDSDK_STEREO_FRAME_H diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/CMakeLists.txt b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/CMakeLists.txt new file mode 100755 index 000000000..d7a11ef49 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 2.8) +project(stereo-vision-multi-thread-sample) + +# Try to see if user has OpenCV installed +# if yes, default callback will display the image +find_package( OpenCV QUIET ) +if (OpenCV_FOUND) + message( "\n${PROJECT_NAME}...") + message( STATUS "Found OpenCV installed in the system, will use it to display image in AdvancedSensing APIs") + message( STATUS " - Includes: ${OpenCV_INCLUDE_DIRS}") + message( STATUS " - Libraries: ${OpenCV_LIBRARIES}") + add_definitions(-DOPEN_CV_INSTALLED) +else() + message( STATUS "Did not find OpenCV in the system, image data is inside RecvContainer as raw data") +endif () + +set(HELPER_FUNCTIONS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +add_executable(${PROJECT_NAME} + ${SOURCE_FILES} + ${HELPER_FUNCTIONS_DIR}/dji_linux_environment.cpp + ${HELPER_FUNCTIONS_DIR}/dji_linux_helpers.cpp + stereo_vision_multi_thread_sample.cpp + utility_thread.cpp + image_process_container.cpp + ) + +target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS}) + +target_link_libraries(${PROJECT_NAME} + djiosdk-core + ) + +if (OpenCV_FOUND) + target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ) +endif () + diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/image_process_container.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/image_process_container.cpp new file mode 100755 index 000000000..399558914 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/image_process_container.cpp @@ -0,0 +1,170 @@ +/*! @file image_process_container.cpp + * @version 3.4 + * @date Oct 10 2017 + * + * @brief + * A simple class to allow user to copy images + * from reading thread and process them in + * a separate image_processing thread + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "image_process_container.hpp" + +using namespace DJI::OSDK; + +ImageProcessContainer::ImageProcessContainer(Vehicle *vehicle) + : vehicle_ptr(vehicle) + , gotNew240pImg(false) + , gotNewVGAImg(false) +{ + if(!this->initImgProcessThread()) + { + DERROR("Failed to initialize image process thread.\n"); + } +} + +ImageProcessContainer::~ImageProcessContainer() +{ + this->imgProcessThread->stopThread(); +} + +bool +ImageProcessContainer::initImgProcessThread() +{ + this->imgProcessThread = new UtilityThread(this, IMG_PROCESS_THREAD); + + if(this->imgProcessThread == 0) + { + DERROR("Error instantiating image process thread object\n"); + return false; + } + + bool imgProcessThreadStatus = false; + imgProcessThreadStatus = this->imgProcessThread->createThread(); + if (!imgProcessThreadStatus) + { + DERROR("Error creating image process thread!\n"); + } + + return imgProcessThreadStatus; +} + +void +ImageProcessContainer::copyVGAImg(const ACK::StereoVGAImgData *img) +{ + memcpy(&stereoVGAImg, img, sizeof(ACK::StereoVGAImgData)); + this->gotNewVGAImg = true; +} + +void +ImageProcessContainer::copy240pImg(const ACK::StereoImgData *img) +{ + memcpy(&stereo240pImg, img, sizeof(ACK::StereoImgData)); + this->gotNew240pImg = true; +} + + +void +ImageProcessContainer::displayStereoVGACallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::UserData userData) +{ + ImageProcessContainer* img_container_ptr = (ImageProcessContainer*)userData; + + DSTATUS("you got a set of VGA image frame in %s thread!", + img_container_ptr->imgProcessThread->getThreadName().c_str()); + +#ifdef OPEN_CV_INSTALLED + // VGA always comes in pair + cv::Mat cv_img_stereo[2]; + for (int i = 0; i < 2; ++i) { + cv_img_stereo[i] = cv::Mat(480, 640, CV_8U); + } + + memcpy(cv_img_stereo[0].data, img_container_ptr->stereoVGAImg.img_vec[0], sizeof(char)*480*640); + memcpy(cv_img_stereo[1].data, img_container_ptr->stereoVGAImg.img_vec[1], sizeof(char)*480*640); + + cv::Mat img_to_show; + cv::hconcat(cv_img_stereo[0], cv_img_stereo[1], img_to_show); + cv::resize(img_to_show, img_to_show, + cv::Size(640*2, 480), (0, 0), (0, 0), cv::INTER_LINEAR); + + cv::imshow("cv_img_stereo", img_to_show); + cv::waitKey(1); +#endif +} + +void +ImageProcessContainer::displayStereo240pCallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::UserData userData) +{ + ImageProcessContainer* img_container_ptr = (ImageProcessContainer*)userData; + + DSTATUS("you got 240p images in %s thread!", + img_container_ptr->imgProcessThread->getThreadName().c_str()); + +#ifdef OPEN_CV_INSTALLED + cv::Mat cv_img_stereo[img_container_ptr->stereo240pImg.num_imgs]; + + for (int i = 0; i < img_container_ptr->stereo240pImg.num_imgs; ++i) + { + cv_img_stereo[i] = cv::Mat(240, 320, CV_8U); + + memcpy(cv_img_stereo[i].data, img_container_ptr->stereo240pImg.img_vec[i].image, + sizeof(char)*ACK::IMG_240P_SIZE); + + cv::imshow(img_container_ptr->stereo240pImg.img_vec[i].name, + cv_img_stereo[i]); + cv::waitKey(1); + } +#endif +} + +UtilityThread* +ImageProcessContainer::getImgProcessThread() +{ + return this->imgProcessThread; +} + +void +ImageProcessContainer::setCallbackHandler(ImgCallBack callback, DJI::OSDK::UserData userData) +{ + this->imgCallBackHandler.callback = callback; + this->imgCallBackHandler.userData = userData; +} + +bool +ImageProcessContainer::newDataPoll() +{ + if(this->gotNew240pImg) + { + this->imgCallBackHandler.callback(vehicle_ptr, this->imgCallBackHandler.userData); + this->gotNew240pImg = false; + return true; + } + if(this->gotNewVGAImg) + { + this->imgCallBackHandler.callback(vehicle_ptr, this->imgCallBackHandler.userData); + this->gotNewVGAImg = false; + return true; + } + return false; +} \ No newline at end of file diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/image_process_container.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/image_process_container.hpp new file mode 100755 index 000000000..411ba7702 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/image_process_container.hpp @@ -0,0 +1,95 @@ +/*! @file image_process_container.hpp + * @version 3.4 + * @date Oct 10 2017 + * + * @brief + * A simple class to allow user to copy images + * from reading thread and process them in + * a separate image_processing thread + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#ifndef ONBOARDSDK_IMAGE_CONTAINER_H +#define ONBOARDSDK_IMAGE_CONTAINER_H + +#include "utility_thread.hpp" +#include "dji_vehicle.hpp" + +#ifdef OPEN_CV_INSTALLED +#include "opencv2/opencv.hpp" +#include "opencv2/highgui/highgui.hpp" +#endif + +class UtilityThread; + +class ImageProcessContainer +{ +public: + + typedef void (*ImgCallBack) (DJI::OSDK::Vehicle* vehicle, DJI::OSDK::UserData userData); + + typedef struct ImgCallBackHandler + { + ImgCallBack callback; + DJI::OSDK::UserData userData; + } ImgCallBackHandler; + + ImageProcessContainer(DJI::OSDK::Vehicle *vehicle); + virtual ~ImageProcessContainer(); + + static const int IMG_PROCESS_THREAD = 1; + + // copy images into this container + void copyVGAImg(const DJI::OSDK::ACK::StereoVGAImgData* img); + void copy240pImg(const DJI::OSDK::ACK::StereoImgData* img); + + // sample cb functions to process img + static void displayStereoVGACallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::UserData userData); + static void displayStereo240pCallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::UserData userData); + + // keep polling to see if there's any new data coming in + bool newDataPoll(); + + // use this function to register callback + void setCallbackHandler(ImgCallBack callback, DJI::OSDK::UserData userData); + + UtilityThread* getImgProcessThread(); + +protected: + bool initImgProcessThread(); + +protected: + DJI::OSDK::ACK::StereoVGAImgData stereoVGAImg; + DJI::OSDK::ACK::StereoImgData stereo240pImg; + + bool gotNew240pImg; + bool gotNewVGAImg; + + ImgCallBackHandler imgCallBackHandler; + + DJI::OSDK::Vehicle* vehicle_ptr; + UtilityThread* imgProcessThread; +}; + + +#endif //ONBOARDSDK_IMAGE_CONTAINER_H diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/stereo_vision_multi_thread_sample.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/stereo_vision_multi_thread_sample.cpp new file mode 100755 index 000000000..44ea6949d --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/stereo_vision_multi_thread_sample.cpp @@ -0,0 +1,166 @@ +/*! @file advanced_sensing_multi_thread_sample.cpp + * @version 3.4 + * @date Oct 10 2017 + * + * @brief + * AdvancedSensing API usage in a Linux environment. + * Provides an example callback function to subscribe to stereo images + * and an example class to process images in a separate thread + * (Optional) With OpenCV installed, user can visualize the images + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "stereo_vision_multi_thread_sample.hpp" + +using namespace DJI::OSDK; + +int +main(int argc, char** argv) +{ + // Setup OSDK. + bool enableAdvancedSensing = true; + LinuxSetup linuxEnvironment(argc, argv, enableAdvancedSensing); + Vehicle* vehicle = linuxEnvironment.getVehicle(); + if (vehicle == NULL) + { + std::cout << "Vehicle not initialized, exiting.\n"; + return -1; + } + + // Initialize variables + int functionTimeout = 1; + // Obtain Control Authority + vehicle->control->obtainCtrlAuthority(functionTimeout); + + // Display interactive prompt + std::cout + << std::endl + << " AdvancedSensing API works in subscription mechanism \n" + << " Please remember to unsubscribe when the program terminates \n" + << " If any error messages occur, please reboot the aircraft \n" + << std::endl + << "| Available commands: |\n" + << "| [a] Subscribe to 240p stereo images |\n" + << "| [b] Subscribe to VGA front stereo images |\n" + << "| [c] Unsubscribe to 240p stereo images |\n" + << "| [d] Unsubscribe to VGA front stereo images |\n" + << std::endl; + char inputChar = ' '; + std::cin >> inputChar; + + bool is240p = false, isVGA = false; + + image_process_container_ptr = new ImageProcessContainer(vehicle); + + AdvancedSensing::ImageSelection image_select; + switch (inputChar) + { + case 'a': + { + memset(&image_select, 0, sizeof(AdvancedSensing::ImageSelection)); + image_select.front_left = 1; + image_select.front_right = 1; + image_select.down_front = 1; + image_select.down_back = 1; + // register a callback for "image processing" thread + image_process_container_ptr->setCallbackHandler(&ImageProcessContainer::displayStereo240pCallback, + image_process_container_ptr); + // register a callback for "image reading" thread + vehicle->advancedSensing->subscribeStereoImages(&image_select, &storeStereoImg240pCallback, NULL); + is240p = true; + } + break; + case 'b': + { + // register a callback for "image processing" thread + image_process_container_ptr->setCallbackHandler(&ImageProcessContainer::displayStereoVGACallback, + image_process_container_ptr); + // register a callback for "image reading" thread + vehicle->advancedSensing->subscribeFrontStereoVGA(AdvancedSensingProtocol::FREQ_20HZ, &storeStereoImgVGACallback, NULL); + isVGA = true; + } + break; + case 'c': + { + vehicle->advancedSensing->unsubscribeStereoImages(); + } + break; + case 'd': + { + vehicle->advancedSensing->unsubscribeVGAImages(); + } + break; + default: + break; + } + + DSTATUS("AdvancedSensing sample sleeps 5 seconds here"); + sleep(5); + + if (is240p){ + vehicle->advancedSensing->unsubscribeStereoImages(); + DSTATUS("Unsubscribe 240p stereo images"); + vehicle->advancedSensing->unsubscribeStereoImages(); + } + if (isVGA){ + DSTATUS("Unsubscribe VGA images"); + vehicle->advancedSensing->unsubscribeVGAImages(); + } + + sleep(1); + DSTATUS("waited 1 second for the image subscription to stop completely\n"); + + return 0; +} + +//! @note This callback is running on reading thread. +//! Another thread is created inside ImageProcessContainer. +//! Whenever the images are copied, another callback on +//! processing thread will perform the image computation +void storeStereoImg240pCallback(Vehicle *vehiclePtr, RecvContainer recvFrame, UserData userData) +{ + DSTATUS("sample stereoCallback receive an image at frame: %d and time stamp: %d", + recvFrame.recvData.stereoImgData->frame_index, + recvFrame.recvData.stereoImgData->time_stamp); + + // copy the image data to image process container + // a boolean is flipped to inform the polling + // mechanism in image process thread + image_process_container_ptr->copy240pImg(recvFrame.recvData.stereoImgData); +} + +//! @note This callback is running on reading thread. +//! Another thread is created inside ImageProcessContainer. +//! Whenever the images are copied, another callback on +//! processing thread will perform the image computation +void storeStereoImgVGACallback(Vehicle *vehiclePtr, RecvContainer recvFrame, UserData userData) +{ + DSTATUS("sample VGACallback receive an image at frame: %d and time stamp: %d", + recvFrame.recvData.stereoVGAImgData->frame_index, + recvFrame.recvData.stereoVGAImgData->time_stamp); + + // copy the image data to image process container + // a boolean is flipped to inform the polling + // mechanism in image process thread + image_process_container_ptr->copyVGAImg(recvFrame.recvData.stereoVGAImgData); +} diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/stereo_vision_multi_thread_sample.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/stereo_vision_multi_thread_sample.hpp new file mode 100755 index 000000000..5b24b1cf3 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/stereo_vision_multi_thread_sample.hpp @@ -0,0 +1,61 @@ +/*! @file advanced_sensing_multi_thread_sample.cpp + * @version 3.4 + * @date Oct 10 2017 + * + * @brief + * AdvancedSensing API usage in a Linux environment. + * Provides an example callback function to subscribe to stereo images + * and an example class to process images in a separate thread + * (Optional) With OpenCV installed, user can visualize the images + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#ifndef ONBOARDSDK_ADVANCED_SENSING_SAMPLE_H +#define ONBOARDSDK_ADVANCED_SENSING_SAMPLE_H + +// DJI OSDK includes +#include "dji_vehicle.hpp" + +// C++ +#include + +// Helpers +#include "dji_linux_helpers.hpp" + +// Third party library +#ifdef OPEN_CV_INSTALLED +#include "opencv2/opencv.hpp" +#include "opencv2/highgui/highgui.hpp" +#endif + +// Utility +#include "utility_thread.hpp" + +ImageProcessContainer* image_process_container_ptr; + +static void storeStereoImg240pCallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::RecvContainer recvFrame, DJI::OSDK::UserData userData); + +static void storeStereoImgVGACallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::RecvContainer recvFrame, DJI::OSDK::UserData userData); + + +#endif //ONBOARDSDK_ADVANCED_SENSING_SAMPLE_H diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/utility_thread.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/utility_thread.cpp new file mode 100755 index 000000000..102695a6c --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/utility_thread.cpp @@ -0,0 +1,134 @@ +/*! @file utility_thread.cpp + * @version 3.4 + * @date Oct 10 2017 + * + * @brief + * Pthread-based threading for DJI Onboard SDK on linux platforms + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "utility_thread.hpp" + +UtilityThread::UtilityThread() +{ + this->imageContainer = 0; + this->type = 0; + this->stop_condition = false; +} + +UtilityThread::UtilityThread(ImageProcessContainer* imgContainer_ptr, int type) + : imageContainer(imgContainer_ptr) + , type(type) +{ + this->stop_condition = false; +} + +bool +UtilityThread::createThread() +{ + int ret = -1; + + /* Initialize and set thread detached attribute */ + pthread_attr_init(&attr); + pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE); + + if (type == 1) + { + ret = pthread_create(&threadID, NULL, img_process_call, (void *) this->imageContainer); + this->thread_name = "ImgProcessPoll"; + } + + + if (0 != ret) + { + DERROR("fail to create thread for %s!\n", this->thread_name.c_str()); + return false; + } + else + { + DSTATUS("a thread named %s is created.\n", this->thread_name.c_str()); + } + + ret = pthread_setname_np(threadID, this->thread_name.c_str()); + if (0 != ret) + { + DERROR("fail to set thread name for %s!\n", this->thread_name.c_str()); + return false; + } + return true; +} + +int +UtilityThread::stopThread() +{ + int ret = -1; + void* status; + this->stop_condition = true; + + /* Free attribute and wait for the other threads */ + if (int i = pthread_attr_destroy(&attr)) + { + DERROR("fail to destroy thread %d\n", i); + } + else + { + DDEBUG("success to distory thread\n"); + } + ret = pthread_join(threadID, &status); + + DDEBUG("Main: completed join with thread code: %d\n", ret); + if (ret) + { + // Return error code + return ret; + } + + return 0; +} + +bool +UtilityThread::getStopCondition() +{ + return this->stop_condition; +} + +std::string +UtilityThread::getThreadName() +{ + return this->thread_name; +} + +void* +UtilityThread::img_process_call(void *param) +{ + ImageProcessContainer* img_container_ptr = (ImageProcessContainer*)param; + + while (!img_container_ptr->getImgProcessThread()->getStopCondition()) + { + img_container_ptr->newDataPoll(); + + usleep(100); + } + + DDEBUG("Quit image process function\n"); +} diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/utility_thread.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/utility_thread.hpp new file mode 100755 index 000000000..866003c23 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_multi_thread_sample/utility_thread.hpp @@ -0,0 +1,67 @@ +/*! @file utility_thread.cpp + * @version 3.4 + * @date Oct 10 2017 + * + * @brief + * Pthread-based threading for DJI Onboard SDK on linux platforms + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#ifndef POSIX_THREAD_HPP +#define POSIX_THREAD_HPP + +#include +#include +#include "image_process_container.hpp" + +class ImageProcessContainer; + +class UtilityThread +{ +public: + + UtilityThread(); + UtilityThread(ImageProcessContainer* imgContainer_ptr, int type); + ~UtilityThread(){} + + bool createThread(); + int stopThread(); + + std::string getThreadName(); + +private: + pthread_t threadID; + pthread_attr_t attr; + int type; + bool stop_condition; + std::string thread_name; + + ImageProcessContainer*imageContainer; + +private: + bool getStopCondition(); + static void* img_process_call(void* param); + +}; + +#endif // POSIX_THREAD_HPP \ No newline at end of file diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/CMakeLists.txt b/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/CMakeLists.txt new file mode 100755 index 000000000..87e1134da --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 2.8) +project(stereo-vision-single-thread-sample) + +# Try to see if user has OpenCV installed +# if yes, default callback will display the image +find_package( OpenCV QUIET ) +if (OpenCV_FOUND) + message( "\n${PROJECT_NAME}...") + message( STATUS "Found OpenCV installed in the system, will use it to display image in AdvancedSensing APIs") + message( STATUS " - Includes: ${OpenCV_INCLUDE_DIRS}") + message( STATUS " - Libraries: ${OpenCV_LIBRARIES}") + add_definitions(-DOPEN_CV_INSTALLED) +else() + message( STATUS "Did not find OpenCV in the system, image data is inside RecvContainer as raw data") +endif () + +set(HELPER_FUNCTIONS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +add_executable(${PROJECT_NAME} + ${SOURCE_FILES} + ${HELPER_FUNCTIONS_DIR}/dji_linux_environment.cpp + ${HELPER_FUNCTIONS_DIR}/dji_linux_helpers.cpp + stereo_vision_single_thread_sample.cpp + ) + +target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS}) + +target_link_libraries(${PROJECT_NAME} + djiosdk-core + ) + +if (OpenCV_FOUND) + target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ) +endif () diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/stereo_vision_single_thread_sample.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/stereo_vision_single_thread_sample.cpp new file mode 100755 index 000000000..a07f3d22a --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/stereo_vision_single_thread_sample.cpp @@ -0,0 +1,188 @@ +/*! @file advanced_sensing_sample.cpp + * @version 3.3.2 + * @date Aug 25 2017 + * + * @brief + * AdvancedSensing API usage in a Linux environment. + * Provides an example callback function to subscribe to stereo images + * (Optional) With OpenCV installed, user can visualize the images + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "stereo_vision_single_thread_sample.hpp" + +using namespace DJI::OSDK; + +int +main(int argc, char** argv) +{ + // Setup OSDK. + bool enableAdvancedSensing = true; + LinuxSetup linuxEnvironment(argc, argv, enableAdvancedSensing); + Vehicle* vehicle = linuxEnvironment.getVehicle(); + if (vehicle == NULL) + { + std::cout << "Vehicle not initialized, exiting.\n"; + return -1; + } + + // Initialize variables + int functionTimeout = 1; + // Obtain Control Authority + vehicle->control->obtainCtrlAuthority(functionTimeout); + + // Display interactive prompt + std::cout + << std::endl + << " AdvancedSensing API works in subscription mechanism \n" + << " Please remember to unsubscribe when the program terminates \n" + << " If any error messages occur, please reboot the aircraft \n" + << std::endl + << "| Available commands: |\n" + << "| [a] Subscribe to 240p stereo images |\n" + << "| [b] Subscribe to VGA front stereo images |\n" + << "| [c] Unsubscribe to 240p stereo images |\n" + << "| [d] Unsubscribe to VGA front stereo images |\n" + << std::endl; + char inputChar = ' '; + std::cin >> inputChar; + + bool is240p = false, isVGA = false; + + AdvancedSensing::ImageSelection image_select; + switch (inputChar) + { + case 'a': + { + memset(&image_select, 0, sizeof(AdvancedSensing::ImageSelection)); + image_select.front_left = 1; + image_select.front_right = 1; + image_select.down_front = 1; + image_select.down_back = 1; + vehicle->advancedSensing->subscribeStereoImages(&image_select, &stereoImg240pCallback, NULL); + is240p = true; + } + break; + case 'b': + { + vehicle->advancedSensing->subscribeFrontStereoVGA(AdvancedSensingProtocol::FREQ_20HZ, &stereoImgVGACallback, NULL); + isVGA = true; + } + break; + case 'c': + { + vehicle->advancedSensing->unsubscribeStereoImages(); + } + break; + case 'd': + { + vehicle->advancedSensing->unsubscribeVGAImages(); + } + break; + default: + break; + } + + DSTATUS("AdvancedSensing sample sleeps 5 seconds here"); + sleep(5); + + if (is240p){ + vehicle->advancedSensing->unsubscribeStereoImages(); + DSTATUS("Unsubscribe 240p stereo images"); + vehicle->advancedSensing->unsubscribeStereoImages(); + } + if (isVGA){ + DSTATUS("Unsubscribe VGA images"); + vehicle->advancedSensing->unsubscribeVGAImages(); + } + + sleep(1); + DSTATUS("waited 1 second for the image subscription to stop completely\n"); + + return 0; +} + +//! @note This callback is running on reading thread. +//! If you would like to run computation inside this +//! callback, note that images are coming at 20 fps, +//! there's only 50ms window for reading + processing. +//! Please make sure your computation does not exceed +//! this limit to prevent dropping frame. +//! Check out multi_thread sample to perform computation on +//! another thread +void stereoImg240pCallback(Vehicle *vehiclePtr, RecvContainer recvFrame, UserData userData) +{ + DSTATUS("sample stereoCallback receive an image at frame: %d and time stamp: %d", + recvFrame.recvData.stereoImgData->frame_index, + recvFrame.recvData.stereoImgData->time_stamp); + +#ifdef OPEN_CV_INSTALLED + cv::Mat cv_img_stereo[recvFrame.recvData.stereoImgData->num_imgs]; + + for (int i = 0; i < recvFrame.recvData.stereoImgData->num_imgs; ++i) + { + cv_img_stereo[i] = cv::Mat(240, 320, CV_8U); + + memcpy(cv_img_stereo[i].data, recvFrame.recvData.stereoImgData->img_vec[i].image, + sizeof(char)*ACK::IMG_240P_SIZE); + + cv::imshow(recvFrame.recvData.stereoImgData->img_vec[i].name, + cv_img_stereo[i]); + cv::waitKey(1); + } +#endif +} + +//! @note This callback is running on reading thread. +//! If you would like to run computation inside this +//! callback, note that images are coming at 10/20 fps, +//! there's only 100/50 ms window for reading + processing. +//! Please make sure your computation does not exceed +//! this limit to prevent dropping frame. +//! Check out multi_thread sample to perform computation on +//! another thread +void stereoImgVGACallback(Vehicle *vehiclePtr, RecvContainer recvFrame, UserData userData) +{ + DSTATUS("sample VGACallback receive an image at frame: %d and time stamp: %d", + recvFrame.recvData.stereoVGAImgData->frame_index, + recvFrame.recvData.stereoVGAImgData->time_stamp); + +#ifdef OPEN_CV_INSTALLED + // VGA always comes in pair + cv::Mat cv_img_stereo[2]; + for (int i = 0; i < 2; ++i) { + cv_img_stereo[i] = cv::Mat(480, 640, CV_8U); + } + + memcpy(cv_img_stereo[0].data, recvFrame.recvData.stereoVGAImgData->img_vec[0], sizeof(char)*480*640); + memcpy(cv_img_stereo[1].data, recvFrame.recvData.stereoVGAImgData->img_vec[1], sizeof(char)*480*640); + + cv::Mat img_to_show; + cv::hconcat(cv_img_stereo[0], cv_img_stereo[1], img_to_show); + cv::resize(img_to_show, img_to_show, + cv::Size(640*2, 480), (0, 0), (0, 0), cv::INTER_LINEAR); + + cv::imshow("cv_img_stereo", img_to_show); + cv::waitKey(1); +#endif +} diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/stereo_vision_single_thread_sample.hpp b/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/stereo_vision_single_thread_sample.hpp new file mode 100755 index 000000000..2856d375b --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_single_thread_sample/stereo_vision_single_thread_sample.hpp @@ -0,0 +1,54 @@ +/*! @file advanced-sensing_sample.cpp + * @version 3.3.2 + * @date Aug 25 2017 + * + * @brief + * AdvancedSensing API usage in a Linux environment. + * Provides an example callback function to subscribe to stereo images + * (Optional) With OpenCV installed, user can visualize the images + * + * @Copyright (c) 2017 DJI + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#ifndef ONBOARDSDK_ADVANCED_SENSING_SAMPLE_H +#define ONBOARDSDK_ADVANCED_SENSING_SAMPLE_H + +// DJI OSDK includes +#include "dji_vehicle.hpp" + +// C++ +#include + +// Helpers +#include "dji_linux_helpers.hpp" + +#ifdef OPEN_CV_INSTALLED +#include "opencv2/opencv.hpp" +#include "opencv2/highgui/highgui.hpp" +#endif + +static void stereoImg240pCallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::RecvContainer recvFrame, DJI::OSDK::UserData userData); + +static void stereoImgVGACallback(DJI::OSDK::Vehicle *vehiclePtr, DJI::OSDK::RecvContainer recvFrame, DJI::OSDK::UserData userData); + + +#endif //ONBOARDSDK_ADVANCED_SENSING_SAMPLE_H