From a7f6a00524e162ef0430b5583958d4353df030d9 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Wed, 22 May 2019 13:18:02 +0300
Subject: [PATCH] Check for bad corners in reg
---
applications/reconstruct/src/main.cpp | 10 +++++-----
applications/reconstruct/src/registration.cpp | 2 ++
2 files changed, 7 insertions(+), 5 deletions(-)
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index 4238136d8..2e483b93a 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 4017d666d..85e077efa 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)
--
GitLab