feature(get_depth_with_region) : add camera coordinate calc

samples/get_depth_with_region.cc

@@ -18,6 +18,8 @@
 
 namespace {
 
+static cv::Mat cv_in_left, cv_in_left_inv;
+
 class DepthRegion {
  public:
   explicit DepthRegion(std::uint32_t n)
@@ -60,10 +62,10 @@ class DepthRegion {
   void ShowElems(
       const cv::Mat &depth,
       std::function<std::string(const T &elem)> elem2string,
-      int elem_space = 40,
+      int elem_space = 60,
       std::function<std::string(
-          const cv::Mat &depth, const cv::Point &point, const std::uint32_t &n)>
+          const cv::Mat &depth, const cv::Point &point, const std::uint32_t &n,
-          getinfo = nullptr) {
+          double X, double Y, double Z)>getinfo = nullptr) {
     if (!show_)
       return;
 
@@ -74,6 +76,17 @@ class DepthRegion {
     int x, y;
     std::string str;
     int baseline = 0;
+
+    // calculate (X, Y, Z) in left camera coordinate
+    cv::Mat mouse_left_cor(3, 1, CV_64FC1), mouse_img_cor(3, 1, CV_64FC1);
+    mouse_img_cor.at<double>(0, 0) = static_cast<double>(point_.x);
+    mouse_img_cor.at<double>(0, 1) = static_cast<double>(point_.y);
+    mouse_img_cor.at<double>(0, 2) = 1.0;
+    double Z = depth.at<T>(point_.y, point_.x);
+    mouse_left_cor = cv_in_left_inv *Z * mouse_img_cor;
+    // std::cout << std::endl << "Mouse Left Cor:" << std::endl
+    //   << mouse_left_cor << std::endl;
+
     for (int i = -n_; i <= n; ++i) {
       x = point_.x + i;
       if (x < 0 || x >= depth.cols)
@@ -101,7 +114,11 @@ class DepthRegion {
     }
 
     if (getinfo) {
-      std::string info = getinfo(depth, point_, n_);
+      double x, y, z;
+      x = mouse_left_cor.at<double>(0, 0);
+      y = mouse_left_cor.at<double>(1, 0);
+      z = mouse_left_cor.at<double>(2, 0);
+      std::string info = getinfo(depth, point_, n_,  x, y, z);
       if (!info.empty()) {
         cv::Size sz =
             cv::getTextSize(info, cv::FONT_HERSHEY_PLAIN, 1, 1, &baseline);
@@ -158,17 +175,48 @@ int main(int argc, char *argv[]) {
 
   api->Start(Source::VIDEO_STREAMING);
 
+  // get left camera Intrinsics
+  auto in_left = api->GetIntrinsicsBase(Stream::LEFT);
+  cv_in_left = cv::Mat::eye(3, 3, CV_64F);
+  if (in_left->calib_model() == CalibrationModel::PINHOLE) {
+    std::shared_ptr<IntrinsicsPinhole> in;
+    in = std::static_pointer_cast<IntrinsicsPinhole>(in_left);
+    in -> ResizeIntrinsics();
+    cv_in_left.at<double>(0, 0) = in->fx;
+    cv_in_left.at<double>(1, 1) = in->fy;
+    cv_in_left.at<double>(0, 2) = in->cx;
+    cv_in_left.at<double>(1, 2) = in->cy;
+  } else if (in_left->calib_model() == CalibrationModel::KANNALA_BRANDT) {
+    in_left = std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_left);
+    std::shared_ptr<IntrinsicsEquidistant> in;
+    in = std::static_pointer_cast<IntrinsicsEquidistant>(in_left);
+    in -> ResizeIntrinsics();
+    cv_in_left.at<double>(0, 0) = in->coeffs[4];
+    cv_in_left.at<double>(1, 1) = in->coeffs[5];
+    cv_in_left.at<double>(0, 2) = in->coeffs[6];
+    cv_in_left.at<double>(1, 2) = in->coeffs[7];
+  } else {
+    std::cout << "UNKNOW CALIB MODEL.";
+    return 0;
+  }
+
+  std::cout << std::endl << "Left Camera Intrinsics:" << std::endl
+      << cv_in_left << std::endl;
+  cv_in_left_inv = cv_in_left.inv();
   cv::namedWindow("frame");
   cv::namedWindow("depth");
   cv::namedWindow("region");
 
   DepthRegion depth_region(3);
   auto depth_info = [](
-      const cv::Mat &depth, const cv::Point &point, const std::uint32_t &n) {
+      const cv::Mat &depth, const cv::Point &point, const std::uint32_t &n,
+          double X, double Y, double Z) {
     MYNTEYE_UNUSED(depth)
     std::ostringstream os;
     os << "depth pos: [" << point.y << ", " << point.x << "]"
-       << "±" << n << ", unit: mm";
+       << " camera pos: [" << X << ", " << Y
+       << ", " << Z << "]" << ", unit: mm";
+
     return os.str();
   };
 
@@ -215,7 +263,7 @@ int main(int argc, char *argv[]) {
             }
             return std::to_string(elem);
           },
-          80, depth_info);
+          90, depth_info);
     }
 
     char key = static_cast<char>(cv::waitKey(1));