Merge pull request #121 in OS/onboard-sdk from stereo_m300_point_clou…

…d to develop

* commit '01ec45034e64f817d9d4ffe50145cdb4fcc26251':
  feat:adapting stereo camera sample to M300

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
             )
 

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:"<<endl;
+  std::cout<<"leftCameraIntrinsicMatrix : \n"<<leftCameraIntrinsicMatrix<<endl;
+  std::cout<<"rightCameraIntrinsicMatrix : \n"<<rightCameraIntrinsicMatrix<<endl;
+  std::cout<<"leftDistCoeffs : \n"<<leftDistCoeffs<<endl;
+  std::cout<<"rightDistCoeffs : \n"<<rightDistCoeffs<<endl;
+  std::cout<<"leftRectificationMatrix : \n"<<leftRectificationMatrix<<endl;
+  std::cout<<"rightRectificationMatrix : \n"<<rightRectificationMatrix<<endl;
+  std::cout<<"leftProjectionMatrix : \n"<<leftProjectionMatrix<<endl;
+  std::cout<<"rightProjectionMatrix : \n"<<rightProjectionMatrix<<endl;
+  cv::FileStorage paramFile(fileName, cv::FileStorage::WRITE);
+  paramFile<<"leftCameraIntrinsicMatrix"<<leftCameraIntrinsicMatrix;
+  paramFile<<"rightCameraIntrinsicMatrix"<<rightCameraIntrinsicMatrix;
+  paramFile<<"leftDistCoeffs"<<leftDistCoeffs;
+  paramFile<<"rightDistCoeffs"<<rightDistCoeffs;
+  paramFile<<"leftRectificationMatrix"<<leftRectificationMatrix;
+  paramFile<<"rightRectificationMatrix"<<rightRectificationMatrix;
+  paramFile<<"leftProjectionMatrix"<<leftProjectionMatrix;
+  paramFile<<"rightProjectionMatrix"<<rightProjectionMatrix;
+  paramFile.release();
+  std::cout<<fileName<<" is created."<<endl;
+  return true;
+}
+
+void M300StereoParamTool::setParamFileForM300(std::string fileName) {
+  M210_STEREO::Config::setParamFile(fileName);
+}

sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/m300_stereo_param_tool.hpp

@@ -0,0 +1,42 @@
+#ifndef ONBOARDSDK_M300_STEREO_PARAM_TOOL_H
+#define ONBOARDSDK_M300_STEREO_PARAM_TOOL_H
+
+#include "dji_vehicle.hpp"
+#include "dji_perception.hpp"
+#include "config.hpp"
+
+using namespace M210_STEREO;
+using namespace DJI;
+using namespace DJI::OSDK;
+
+class M300StereoParamTool
+{
+public:
+  M300StereoParamTool(Vehicle *vehicle);
+  ~M300StereoParamTool();
+
+public:
+  typedef struct DoubleCamParamType {
+    Perception::DirectionType direction;
+    double leftIntrinsics[9];
+    double rightIntrinsics[9];
+    double rotaionLeftInRight[9];
+    double translationLeftInRight[3];
+  } DoubleCamParamType;
+
+  /*!*/
+  Perception::CamParamType getM300stereoParams(Perception::DirectionType direction);
+
+  bool createStereoParamsYamlFile(std::string fileName,
+                                  Perception::CamParamType param);
+
+  void setParamFileForM300(std::string fileName);
+
+ private:
+  static void PerceptionCamParamCB(Perception::CamParamPacketType pack,
+                                   void *userData);
+  Vehicle *vehicle;
+};
+
+
+#endif //ONBOARDSDK_M300_STEREO_PARAM_TOOL_H

sample/platform/linux/advanced-sensing/stereo_vision_depth_perception_sample/stereo_vision_depth_perception_sample.cpp

@@ -20,20 +20,14 @@
  */
 
 #include "stereo_vision_depth_perception_sample.hpp"
+#include "m300_stereo_param_tool.hpp"
+#define M300_FRONT_STEREO_PARAM_YAML_NAME "m300_front_stereo_param.yaml"
 
 using namespace DJI::OSDK;
 
 int
 main(int argc, char** argv)
 {
-  if(argc >= 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);