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

Made some optimization about pointscloud

Browse Source
This commit is contained in:
Osenberg-Y 2018-08-10 15:55:22 +08:00
parent f92ecd4371
commit 5677aa56b2
4 changed files with 43 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
samples/tutorials/util/pc_viewer.cc
View File

@ -84,6 +84,7 @@ void PCViewer::ConvertMatToPointCloud(
for (int i = 0; i < xyz.rows; i++) {
for (int j = 0; j < xyz.cols; j++) {
auto &&p = xyz.at<cv::Point3f>(i, j);
if (std::abs(p.z) > 9999) continue;
if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) {
// LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y
// << ", z: " << p.z;

3
src/api/processor/disparity_processor.cc
View File

@ -26,6 +26,7 @@ const char DisparityProcessor::NAME[] = "DisparityProcessor";
DisparityProcessor::DisparityProcessor(std::int32_t proc_period)
: Processor(std::move(proc_period)) {
VLOG(2) << __func__ << ": proc_period=" << proc_period;
int blockSize_ = 15; // 15
int minDisparity_ = 0; // 0
int numDisparities_ = 64; // 64
@ -68,7 +69,7 @@ bool DisparityProcessor::OnProcess(
cv::Mat disparity;
bm_->compute(input->first, input->second, disparity);
output->value = disparity;
disparity.convertTo(output->value, CV_32F, 1./16);
return true;
}

37
src/api/processor/points_processor.cc
View File

@ -45,7 +45,42 @@ bool PointsProcessor::OnProcess(
UNUSED(parent)
const ObjMat *input = Object::Cast<ObjMat>(in);
ObjMat *output = Object::Cast<ObjMat>(out);
cv::reprojectImageTo3D(input->value, output->value, Q_, true);
// cv::reprojectImageTo3D(input->value, output->value, Q_, true, -1);
bool handleMissingValues = true;
cv::Mat disparity = input->value;
output->value.create(disparity.size(), CV_MAKETYPE(CV_32FC3, 3));
cv::Mat _3dImage = output->value;
const float bigZ = 10000.f;
cv::Matx44d Q;
Q_.convertTo(Q, CV_64F);
int x, cols = disparity.cols;
CV_Assert(cols >= 0);
double minDisparity = FLT_MAX;
if (handleMissingValues) {
cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0);
}
for (int y = 0; y < disparity.rows; y++) {
float *sptr = disparity.ptr<float>(y);
cv::Vec3f *dptr = _3dImage.ptr<cv::Vec3f>(y);
for( x = 0; x < cols; x++)
{
double d = sptr[x];
cv::Vec4d homg_pt = Q*cv::Vec4d(x, y, d, 1.0);
dptr[x] = cv::Vec3d(homg_pt.val);
dptr[x] /= homg_pt[3];
if( fabs(d-minDisparity) <= FLT_EPSILON ) {
dptr[x][2] = bigZ;
}
}
}
return true;
}

4
src/api/processor/rectify_processor.cc
View File

@ -69,12 +69,16 @@ void RectifyProcessor::InitParams(
in_right.cy, 0, 0, 1);
cv::Mat D1(1, 5, CV_64F, in_left.coeffs);
cv::Mat D2(1, 5, CV_64F, in_right.coeffs);
/*
cv::Mat R =
(cv::Mat_<double>(3, 3) << ex_right_to_left.rotation[0][0],
ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2],
ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1],
ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0],
ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]);
*/
cv::Mat R =
(cv::Mat_<double>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation);
VLOG(2) << "InitParams size: " << size;