diff --git a/samples/get_depth_with_region.cc b/samples/get_depth_with_region.cc index 9dee888..e2d57c8 100644 --- a/samples/get_depth_with_region.cc +++ b/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 elem2string, - int elem_space = 40, + int elem_space = 60, std::function - getinfo = nullptr) { + const cv::Mat &depth, const cv::Point &point, const std::uint32_t &n, + 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(0, 0) = static_cast(point_.x); + mouse_img_cor.at(0, 1) = static_cast(point_.y); + mouse_img_cor.at(0, 2) = 1.0; + double Z = depth.at(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(0, 0); + y = mouse_left_cor.at(1, 0); + z = mouse_left_cor.at(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 in; + in = std::static_pointer_cast(in_left); + in -> ResizeIntrinsics(); + cv_in_left.at(0, 0) = in->fx; + cv_in_left.at(1, 1) = in->fy; + cv_in_left.at(0, 2) = in->cx; + cv_in_left.at(1, 2) = in->cy; + } else if (in_left->calib_model() == CalibrationModel::KANNALA_BRANDT) { + in_left = std::dynamic_pointer_cast(in_left); + std::shared_ptr in; + in = std::static_pointer_cast(in_left); + in -> ResizeIntrinsics(); + cv_in_left.at(0, 0) = in->coeffs[4]; + cv_in_left.at(1, 1) = in->coeffs[5]; + cv_in_left.at(0, 2) = in->coeffs[6]; + cv_in_left.at(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(cv::waitKey(1));