diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index 4238136d866253e8acf9914f4d0dda1070361bee..2e483b93a97d612b2e8b5f9958f5851a70a213df 100644 --- a/applications/reconstruct/src/main.cpp +++ b/applications/reconstruct/src/main.cpp @@ -94,8 +94,8 @@ PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src) { point.y = (((double)i + CY) / FY) * d; point.z = d; - if (point.x == INFINITY || point.y == INFINITY || point.z == INFINITY || point.z < 6.0f) { - point.x = 0; point.y = 0; point.z = 0; + if (point.x == INFINITY || point.y == INFINITY || point.z > 20000.0f || point.z < 0.04f) { + point.x = 0.0f; point.y = 0.0f; point.z = 0.0f; } cv::Point3_<uchar> prgb = rgb.at<cv::Point3_<uchar>>(i, j); @@ -323,7 +323,7 @@ static void run() { PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>); for (size_t i = 0; i < inputs.size(); i++) { - Display &display = displays[i]; + //Display &display = displays[i]; RGBDSource *input = inputs[i]; Mat rgb, depth; input->getRGBD(rgb,depth); @@ -334,8 +334,8 @@ static void run() { clouds[i] = ftl::rgbd::createPointCloud(input); //display.render(rgb, depth,input->getParameters()); - display.render(clouds[i]); - display.wait(5); + //display.render(clouds[i]); + //display.wait(5); } for (size_t i = 0; i < clouds.size(); i++) { diff --git a/applications/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp index 4017d666d71df438d421b7a01f685b8af2ccdd0d..85e077efabd3ad24ebb911b6f917e0b9f8ca26a9 100644 --- a/applications/reconstruct/src/registration.cpp +++ b/applications/reconstruct/src/registration.cpp @@ -92,6 +92,8 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners point.x = (((double)x + CX) / FX) * d; // / 1000.0f; point.y = (((double)y + CY) / FY) * d; // / 1000.0f; point.z = d; + + if (d > 6000.0f) LOG(ERROR) << "Bad corner : " << i; // no check since disparities assumed to be good in the calibration pattern // if (fabs(d-minDisparity) <= FLT_EPSILON)