From 3d79c67f783f9fc8f99c5d465cfd8f0489c9b7dd Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Wed, 22 May 2019 11:32:34 +0300
Subject: [PATCH] Add fy to camera params
---
CMakeLists.txt | 2 +-
applications/reconstruct/src/main.cpp | 15 ++++++++++-----
applications/reconstruct/src/registration.cpp | 7 ++++---
components/renderers/cpp/src/display.cpp | 7 ++++---
.../rgbd-sources/include/ftl/rgbd_source.hpp | 3 ++-
components/rgbd-sources/src/net_source.cpp | 2 ++
components/rgbd-sources/src/rgbd_source.cpp | 10 +++++++---
.../rgbd-sources/src/stereovideo_source.cpp | 3 ++-
8 files changed, 32 insertions(+), 17 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7984483ed..512d8b801 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -123,7 +123,7 @@ if (CPPCHECK_FOUND)
set(CMAKE_CXX_CPPCHECK "cppcheck" "--enable=style" "--suppress=*:*catch.hpp" "--suppress=*:*elas*" "--suppress=*:*json.hpp")
endif()
-include_directories(${PROJECT_SOURCE_DIR}/common/cpp/include)
+# include_directories(${PROJECT_SOURCE_DIR}/common/cpp/include)
include(git_version)
include(ftl_paths)
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index af23dcdf2..4238136d8 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -73,7 +73,8 @@ PointCloud<PointXYZRGB>::Ptr createPointCloud(RGBDSource *src);
PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src) {
const double CX = src->getParameters().cx;
const double CY = src->getParameters().cy;
- const double F = src->getParameters().f;
+ const double FX = src->getParameters().fx;
+ const double FY = src->getParameters().fy;
cv::Mat rgb;
cv::Mat depth;
@@ -89,8 +90,8 @@ PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src) {
float d = sptr[j] * 1000.0f;
pcl::PointXYZRGB point;
- point.x = (((double)j + CX) / F) * d;
- point.y = (((double)i + CY) / F) * d;
+ point.x = (((double)j + CX) / FX) * d;
+ point.y = (((double)i + CY) / FY) * d;
point.z = d;
if (point.x == INFINITY || point.y == INFINITY || point.z == INFINITY || point.z < 6.0f) {
@@ -322,15 +323,19 @@ 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);
//if (!display.active()) continue;
active += 1;
clouds[i] = ftl::rgbd::createPointCloud(input);
- //display.render(clouds[i]);
+ //display.render(rgb, depth,input->getParameters());
+ 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 900a5a4aa..4017d666d 100644
--- a/applications/reconstruct/src/registration.cpp
+++ b/applications/reconstruct/src/registration.cpp
@@ -68,7 +68,8 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
const double CX = p.cx;
const double CY = p.cy;
- const double F = p.f;
+ const double FX = p.fx;
+ const double FY = p.fy;
// Output point cloud
PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
@@ -88,8 +89,8 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
//cv::Vec3d p = cv::Vec3d(homg_pt.val) / homg_pt[3];
PointXYZ point;
- point.x = (((double)x + CX) / F) * d; // / 1000.0f;
- point.y = (((double)y + CY) / F) * d; // / 1000.0f;
+ point.x = (((double)x + CX) / FX) * d; // / 1000.0f;
+ point.y = (((double)y + CY) / FY) * d; // / 1000.0f;
point.z = d;
// no check since disparities assumed to be good in the calibration pattern
diff --git a/components/renderers/cpp/src/display.cpp b/components/renderers/cpp/src/display.cpp
index 32f997192..8841c1ad0 100644
--- a/components/renderers/cpp/src/display.cpp
+++ b/components/renderers/cpp/src/display.cpp
@@ -92,7 +92,8 @@ Display::~Display() {
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgbdToPointXYZ(const cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::CameraParameters &p) {
const double CX = p.cx;
const double CY = p.cy;
- const double F = p.f;
+ const double FX = p.fx;
+ const double FY = p.fy;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
point_cloud_ptr->width = rgb.cols * rgb.rows;
@@ -104,8 +105,8 @@ static pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgbdToPointXYZ(const cv::Mat &rgb,
float d = sptr[j] * 1000.0f;
pcl::PointXYZRGB point;
- point.x = (((double)j + CX) / F) * d;
- point.y = (((double)i + CY) / F) * d;
+ point.x = (((double)j + CX) / FX) * d;
+ point.y = (((double)i + CY) / FY) * d;
point.z = d;
if (point.x == INFINITY || point.y == INFINITY || point.z > 20000.0f || point.z < 0.04f) {
diff --git a/components/rgbd-sources/include/ftl/rgbd_source.hpp b/components/rgbd-sources/include/ftl/rgbd_source.hpp
index 43cce5090..a00a03549 100644
--- a/components/rgbd-sources/include/ftl/rgbd_source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd_source.hpp
@@ -12,7 +12,8 @@ namespace ftl {
namespace rgbd {
struct CameraParameters {
- double f;
+ double fx;
+ double fy;
double cx;
double cy;
unsigned int width;
diff --git a/components/rgbd-sources/src/net_source.cpp b/components/rgbd-sources/src/net_source.cpp
index 1a8b85177..2f4ed1995 100644
--- a/components/rgbd-sources/src/net_source.cpp
+++ b/components/rgbd-sources/src/net_source.cpp
@@ -23,6 +23,8 @@ static bool getCalibration(Universe &net, string src, ftl::rgbd::CameraParameter
LOG(ERROR) << "Corrupted calibration";
return false;
}
+
+ LOG(INFO) << "Calibration received: " << p.cx << ", " << p.cy << ", " << p.fx << ", " << p.fy;
return true;
} else {
diff --git a/components/rgbd-sources/src/rgbd_source.cpp b/components/rgbd-sources/src/rgbd_source.cpp
index 9738c313e..f4f3e4aed 100644
--- a/components/rgbd-sources/src/rgbd_source.cpp
+++ b/components/rgbd-sources/src/rgbd_source.cpp
@@ -2,6 +2,7 @@
using ftl::rgbd::RGBDSource;
using ftl::Configurable;
+using std::string;
std::map<std::string, std::function<RGBDSource*(nlohmann::json&,ftl::net::Universe*)>> *RGBDSource::sources__ = nullptr;
@@ -34,9 +35,12 @@ bool RGBDSource::snapshot(const std::string &fileprefix) {
}
RGBDSource *RGBDSource::create(nlohmann::json &config, ftl::net::Universe *net) {
- if (config["type"].type_name() != "string") return nullptr;
- if (sources__->count(config["type"]) != 1) return nullptr;
- return (*sources__)[config["type"]](config, net);
+ if (config["type"].type_name() != "string") {
+ LOG(ERROR) << "Missing RGB-D source type: " << config["type"].type_name();
+ //return nullptr;
+ }
+ if (sources__->count(config["type"].get<string>()) != 1) return nullptr;
+ return (*sources__)[config["type"].get<string>()](config, net);
}
void RGBDSource::_register(const std::string &n,
diff --git a/components/rgbd-sources/src/stereovideo_source.cpp b/components/rgbd-sources/src/stereovideo_source.cpp
index 457b7727c..4b14dcabf 100644
--- a/components/rgbd-sources/src/stereovideo_source.cpp
+++ b/components/rgbd-sources/src/stereovideo_source.cpp
@@ -48,7 +48,8 @@ StereoVideoSource::StereoVideoSource(nlohmann::json &config, const string &file)
// Generate camera parameters from Q matrix
cv::Mat q = calib_->getQ();
params_ = {
- q.at<double>(2,3), // F
+ q.at<double>(2,3), // FX
+ q.at<double>(2,3), // FY
q.at<double>(0,3), // Cx
q.at<double>(1,3), // Cy
(unsigned int)left_.cols, // TODO (Nick)
--
GitLab