From b7080d01ba932b44c584bcdfc5eef97d4d5acc6b Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Thu, 20 Jun 2019 08:02:49 +0300
Subject: [PATCH] Partial completion of #99 double precision
---
.../reconstruct/include/ftl/registration.hpp | 8 ++++----
applications/reconstruct/src/main.cpp | 15 ++++++++-------
applications/reconstruct/src/registration.cpp | 11 ++++++-----
applications/registration/src/correspondances.cpp | 4 ++--
4 files changed, 20 insertions(+), 18 deletions(-)
diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp
index be5154f30..20f8eac6a 100644
--- a/applications/reconstruct/include/ftl/registration.hpp
+++ b/applications/reconstruct/include/ftl/registration.hpp
@@ -14,11 +14,11 @@
namespace ftl {
namespace registration {
-void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4f> &transformations);
-void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4f> &transformations);
+void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
+void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
-bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4f> &data);
-bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4f> &data);
+bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
+bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
/** @brief Find transformation matrix for transforming clouds_source to clouds_target.
* Assumes that corresponding points in clouds_source[i] and clouds_target[i] have same indices.
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index d1c2c095a..f3a29a9fd 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -102,13 +102,13 @@ static void run(ftl::Configurable *root) {
if (!merge) {
LOG(WARNING) << "Input merging not configured, using only first input in configuration";
inputs = { inputs[0] };
- inputs[0].source->setPose(Eigen::Matrix4f::Identity());
+ inputs[0].source->setPose(Eigen::Matrix4d::Identity());
}
if (inputs.size() > 1) {
- std::map<std::string, Eigen::Matrix4f> transformations;
+ std::map<std::string, Eigen::Matrix4d> transformations;
- if ((*merge)["register"]) {
+ /*if ((*merge)["register"]) {
LOG(INFO) << "Registration requested";
ftl::registration::Registration *reg = ftl::registration::ChessboardRegistration::create(*merge);
@@ -129,14 +129,14 @@ static void run(ftl::Configurable *root) {
free(reg);
}
- else {
+ else {*/
if (loadTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
LOG(INFO) << "Loaded camera trasformations from file";
}
else {
LOG(ERROR) << "Error loading camera transformations from file";
}
- }
+ //}
for (auto &input : inputs) {
string uri = input.source->getURI();
@@ -146,7 +146,7 @@ static void run(ftl::Configurable *root) {
LOG(WARNING) << "Using only first configured source";
// TODO: use target source if configured and found
inputs = { inputs[0] };
- inputs[0].source->setPose(Eigen::Matrix4f::Identity());
+ inputs[0].source->setPose(Eigen::Matrix4d::Identity());
break;
}
input.source->setPose(T->second);
@@ -244,7 +244,8 @@ static void run(ftl::Configurable *root) {
// Send to GPU and merge view into scene
inputs[i].gpu.updateParams(inputs[i].params);
inputs[i].gpu.updateData(depth, rgba);
- scene->integrate(inputs[i].source->getPose(), inputs[i].gpu, inputs[i].params, nullptr);
+ // TODO(Nick): Use double pose
+ scene->integrate(inputs[i].source->getPose().cast<float>(), inputs[i].gpu, inputs[i].params, nullptr);
}
} else {
active = 1;
diff --git a/applications/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp
index ec27f43ba..ad8c1e8cd 100644
--- a/applications/reconstruct/src/registration.cpp
+++ b/applications/reconstruct/src/registration.cpp
@@ -40,17 +40,18 @@ using pcl::PointXYZRGB;
using cv::Mat;
using Eigen::Matrix4f;
+using Eigen::Matrix4d;
-void from_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
+void from_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
for (auto it = json.begin(); it != json.end(); ++it) {
- Eigen::Matrix4f m;
+ Eigen::Matrix4d m;
auto data = m.data();
for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
transformations[it.key()] = m;
}
}
-void to_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
+void to_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
for (auto &item : transformations) {
auto val = nlohmann::json::array();
for(size_t i = 0; i < 16; i++) { val.push_back((float) item.second.data()[i]); }
@@ -58,7 +59,7 @@ void to_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
}
}
-bool loadTransformations(const string &path, map<string, Matrix4f> &data) {
+bool loadTransformations(const string &path, map<string, Matrix4d> &data) {
std::ifstream file(path);
if (!file.is_open()) {
LOG(ERROR) << "Error loading transformations from file " << path;
@@ -71,7 +72,7 @@ bool loadTransformations(const string &path, map<string, Matrix4f> &data) {
return true;
}
-bool saveTransformations(const string &path, map<string, Matrix4f> &data) {
+bool saveTransformations(const string &path, map<string, Matrix4d> &data) {
nlohmann::json data_json;
to_json(data_json, data);
std::ofstream file(path);
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index b385800a2..5292905e9 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -267,7 +267,7 @@ double Correspondances::icp() {
pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
- pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
+ pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
icp.setInputSource(tsrc_cloud);
icp.setInputTarget(targ_cloud);
icp.align(*final_cloud);
@@ -399,7 +399,7 @@ double Correspondances::findBestSubset(Eigen::Matrix4d &tr, int K, int N) {
//return score;
}
-Eigen::Matrix4f Correspondances::transform() {
+Eigen::Matrix4d Correspondances::transform() {
if (!uptodate_) estimateTransform(transform_);
return (parent_) ? parent_->transform() * transform_ : transform_;
}
--
GitLab