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 index 5234d6392..63a3ac4f5 100755 --- 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 @@ -87,6 +87,7 @@ if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ) ${MULTI_THREAD_SAMPLE_DIR}/image_process_container.cpp ${MULTI_THREAD_SAMPLE_DIR}/utility_thread.cpp stereo_process_container.cpp + m300_stereo_param_tool.cpp utility/*.cpp ) diff --git a/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m300_stereo_param_tool.cpp b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m300_stereo_param_tool.cpp new file mode 100755 index 000000000..3997ef587 --- /dev/null +++ b/sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m300_stereo_param_tool.cpp @@ -0,0 +1,111 @@ +/* + * 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 "m300_stereo_param_tool.hpp" + +M300StereoParamTool::M300StereoParamTool(Vehicle *vehicle) { + this->vehicle = vehicle; +} + +M300StereoParamTool::~M300StereoParamTool() {} + +Perception::CamParamType M300StereoParamTool::getM300stereoParams( + Perception::DirectionType direction) { + Perception::CamParamType stereoParam; + stereoParam.direction = (Perception::DirectionType) 0xFF; + vehicle->advancedSensing->setStereoCamParamsObserver(PerceptionCamParamCB, + &stereoParam); + while (1) { + DSTATUS("Getting M300 front stereo camera parameters ..."); + vehicle->advancedSensing->triggerStereoCamParamsPushing(); + OsdkOsal_TaskSleepMs(1000); + if (stereoParam.direction == direction) break; + } + + return stereoParam; +} + +void M300StereoParamTool::PerceptionCamParamCB( + Perception::CamParamPacketType pack, void *userData) { + DSTATUS("stereo cam parameters : timestamp(%d) dirNum(%d)", pack.timeStamp, + pack.directionNum); + if ((pack.directionNum > 0) && (pack.directionNum <= IMAGE_MAX_DIRECTION_NUM)) + for (int i = 0; i < pack.directionNum; i++) { + if ((userData) + && (pack.cameraParam[i].direction == Perception::RECTIFY_FRONT)) { + auto camParam = (Perception::CamParamType *) userData; + *camParam = pack.cameraParam[i]; + } + } +} + +bool M300StereoParamTool::createStereoParamsYamlFile(std::string fileName, + Perception::CamParamType param) { + DoubleCamParamType doubleCameraParams; + doubleCameraParams.direction = param.direction; + /*! Convert float camera parameters matrix to be double matrix */ + for (int i = 0; i < sizeof(param.leftIntrinsics); i++) { + doubleCameraParams.leftIntrinsics[i] = (double)param.leftIntrinsics[i]; + doubleCameraParams.rightIntrinsics[i] = (double)param.rightIntrinsics[i]; + doubleCameraParams.rotaionLeftInRight[i] = (double)param.rotaionLeftInRight[i]; + } + for (int i = 0; i < sizeof(param.translationLeftInRight); i++) + doubleCameraParams.translationLeftInRight[i] = (double)param.translationLeftInRight[i]; + + DSTATUS("Write stereo camera parameters to yaml"); + cv::Mat leftCameraIntrinsicMatrix(3, 3, CV_64F, doubleCameraParams.leftIntrinsics); + cv::Mat rightCameraIntrinsicMatrix(3, 3, CV_64F, doubleCameraParams.rightIntrinsics); + cv::Mat leftDistCoeffs = cv::Mat::zeros(5, 1, CV_64F); + cv::Mat rightDistCoeffs = cv::Mat::zeros(5, 1, CV_64F); + cv::Mat leftRectificationMatrix = cv::Mat::eye(3, 3, CV_64F); + cv::Mat rightRectificationMatrix = cv::Mat::eye(3, 3, CV_64F); + cv::Mat translationLeftInRight(3, 1, CV_64F, doubleCameraParams.translationLeftInRight); + cv::Mat leftProjectionMatrix(leftCameraIntrinsicMatrix.rows, leftCameraIntrinsicMatrix.cols + translationLeftInRight.cols, CV_64F); + cv::hconcat(leftCameraIntrinsicMatrix, translationLeftInRight, leftProjectionMatrix); + cv::Mat rightProjectionMatrix(rightCameraIntrinsicMatrix.rows, rightCameraIntrinsicMatrix.cols + translationLeftInRight.cols, CV_64F); + cv::hconcat(rightCameraIntrinsicMatrix, translationLeftInRight, rightProjectionMatrix); + + std::cout<<"Logging yaml file for stereo camera parameters. Here are the detials:"<= 3){ - printf("Input yaml file: %s\n", argv[2]); - } else{ - printf("Please specify a yaml file with camera parameters\n"); - printf("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); @@ -44,11 +38,38 @@ main(int argc, char** argv) return -1; } + if (vehicle->isM210V2()) { + if (argc >= 3) { + DSTATUS("Input yaml file: %s\n", argv[2]); + } else { + DERROR("Please specify a yaml file with camera parameters\n"); + DERROR("Example: ./stereo-vision-depth-perception-sample UserConfig.txt m210_stereo_param.yaml\n"); + return -1; + } + } else if (vehicle->isM300()) { + DSTATUS("M300 stereo parameters can be got from the drone. So yaml file is" + "not need here for M300 stereo camera."); + } + // Initialize variables int functionTimeout = 1; // Obtain Control Authority vehicle->control->obtainCtrlAuthority(functionTimeout); + /*! get stereo camera parameters */ + if (vehicle->isM210V2()) { + std::string yaml_file_path = argv[2]; + M210_STEREO::Config::setParamFile(yaml_file_path); + } else if (vehicle->isM300()) { + M300StereoParamTool *tool = new M300StereoParamTool(vehicle); + Perception::CamParamType stereoParam = + tool->getM300stereoParams(Perception::DirectionType::RECTIFY_FRONT); + if (tool->createStereoParamsYamlFile(M300_FRONT_STEREO_PARAM_YAML_NAME, stereoParam)) + tool->setParamFileForM300(M300_FRONT_STEREO_PARAM_YAML_NAME); + else + return -1; + } + // Display interactive prompt std::cout << std::endl @@ -66,9 +87,6 @@ main(int argc, char** argv) 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) @@ -136,7 +154,7 @@ main(int argc, char** argv) //! 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", + DSTATUS("sample VGACallback receive an image at frame: %d and time stamp: %u", recvFrame.recvData.stereoVGAImgData->frame_index, recvFrame.recvData.stereoVGAImgData->time_stamp);