tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feature(get_depth_with_region) : add camera coordinate calc

Browse Source
This commit is contained in:
Messier 2019-09-09 17:20:16 +08:00
parent 9eb52c1f44
commit d9615a95d3
1 changed files with 55 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
samples/get_depth_with_region.cc
View File

@ -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)>
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<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));