From 2b93e251cbdd9a5b36e087e78486f525dd1bf5d7 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Thu, 20 Jun 2019 07:39:16 +0300
Subject: [PATCH] WIP switch to double
---
applications/gui/src/main.cpp | 6 ++---
.../registration/src/correspondances.cpp | 12 +++++-----
applications/registration/src/manual.cpp | 24 +++++++++----------
3 files changed, 21 insertions(+), 21 deletions(-)
diff --git a/applications/gui/src/main.cpp b/applications/gui/src/main.cpp
index d070e3941..979fc3359 100644
--- a/applications/gui/src/main.cpp
+++ b/applications/gui/src/main.cpp
@@ -305,13 +305,13 @@ class FTLApplication : public nanogui::Screen {
Eigen::Vector4f camPos;
try {
- camPos = src_->point(sx,sy);
+ camPos = src_->point(sx,sy).cast<float>();
} catch(...) {
return true;
}
camPos *= -1.0f;
- Eigen::Vector4f worldPos = src_->getPose() * camPos;
+ Eigen::Vector4f worldPos = src_->getPose().cast<float>() * camPos;
//lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
LOG(INFO) << "Depth at click = " << -camPos[2];
return true;
@@ -374,7 +374,7 @@ class FTLApplication : public nanogui::Screen {
Eigen::Affine3f t(trans);
Eigen::Matrix4f viewPose = (t * r).matrix();
- src_->setPose(viewPose);
+ src_->setPose(viewPose.cast<double>());
src_->grab();
src_->getFrames(rgb, depth);
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index a5a37b54f..b385800a2 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -213,8 +213,8 @@ void Correspondances::removeLast() {
}
double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
- pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
- pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+ pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
+ pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
validate.setMaxRange(0.1);
@@ -223,8 +223,8 @@ double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
}
double Correspondances::estimateTransform(Eigen::Matrix4d &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
- pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
- pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+ pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
+ pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -276,7 +276,7 @@ double Correspondances::icp() {
return icp.getFitnessScore();
}
-double Correspondances::icp(const Eigen::Matrix4f &T_in, Eigen::Matrix4f &T_out, const vector<int> &idx) {
+double Correspondances::icp(const Eigen::Matrix4d &T_in, Eigen::Matrix4d &T_out, const vector<int> &idx) {
//pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -291,7 +291,7 @@ double Correspondances::icp(const Eigen::Matrix4f &T_in, Eigen::Matrix4f &T_out,
pcl::transformPointCloud(*src_cloud, *tsrc_cloud, T_in);
- 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);
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index 23dc75d32..be729155c 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -45,16 +45,16 @@ static void setMouseAction(const std::string& winName, const MouseAction &action
static MouseAction tmouse;
static MouseAction smouse;
-void from_json(nlohmann::json &json, map<string, Eigen::Matrix4f> &transformations) {
+void from_json(nlohmann::json &json, map<string, Eigen::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;
}
}
-static void to_json(nlohmann::json &json, map<string, Eigen::Matrix4f> &transformations) {
+static void to_json(nlohmann::json &json, map<string, Eigen::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]); }
@@ -62,7 +62,7 @@ static void to_json(nlohmann::json &json, map<string, Eigen::Matrix4f> &transfor
}
}
-static bool saveTransformations(const string &path, map<string, Eigen::Matrix4f> &data) {
+static bool saveTransformations(const string &path, map<string, Eigen::Matrix4d> &data) {
nlohmann::json data_json;
to_json(data_json, data);
std::ofstream file(path);
@@ -76,7 +76,7 @@ static bool saveTransformations(const string &path, map<string, Eigen::Matrix4f>
return true;
}
-bool loadTransformations(const string &path, map<string, Eigen::Matrix4f> &data) {
+bool loadTransformations(const string &path, map<string, Eigen::Matrix4d> &data) {
std::ifstream file(path);
if (!file.is_open()) {
LOG(ERROR) << "Error loading transformations from file " << path;
@@ -89,7 +89,7 @@ bool loadTransformations(const string &path, map<string, Eigen::Matrix4f> &data)
return true;
}
-static void build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin, map<string, Eigen::Matrix4f> &old) {
+static void build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin, map<string, Eigen::Matrix4d> &old) {
Correspondances *last = nullptr;
cs[sources[origin]->getURI()] = nullptr;
@@ -151,7 +151,7 @@ void ftl::registration::manual(ftl::Configurable *root) {
cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
- map<string, Eigen::Matrix4f> oldTransforms;
+ map<string, Eigen::Matrix4d> oldTransforms;
loadTransformations(root->value("output", string("./test.json")), oldTransforms);
//Correspondances c(sources[targsrc], sources[cursrc]);
@@ -288,7 +288,7 @@ void ftl::registration::manual(ftl::Configurable *root) {
Mat f1,f2;
current->capture(f1,f2);
}else if (key == 'a') {
- Eigen::Matrix4f t;
+ Eigen::Matrix4d t;
if (current->add(lastTX,lastTY,lastSX,lastSY)) {
lastTX = lastTY = lastSX = lastSY = 0;
lastScore = current->estimateTransform(t);
@@ -296,20 +296,20 @@ void ftl::registration::manual(ftl::Configurable *root) {
}
} else if (key == 'u') {
current->removeLast();
- Eigen::Matrix4f t;
+ Eigen::Matrix4d t;
lastScore = current->estimateTransform(t);
current->setTransform(t);
} else if (key == 'i') {
current->icp();
} else if (key == 's') {
// Save
- map<string, Eigen::Matrix4f> transforms;
+ map<string, Eigen::Matrix4d> transforms;
//transforms[sources[targsrc]->getURI()] = Eigen::Matrix4f().setIdentity();
//transforms[sources[cursrc]->getURI()] = c.transform();
for (auto x : corrs) {
if (x.second == nullptr) {
- transforms[x.first] = Eigen::Matrix4f().setIdentity();
+ transforms[x.first] = Eigen::Matrix4d().setIdentity();
} else {
transforms[x.first] = x.second->transform();
}
@@ -327,7 +327,7 @@ void ftl::registration::manual(ftl::Configurable *root) {
LOG(INFO) << "START";
lastScore = 1000.0;
do {
- Eigen::Matrix4f tr;
+ Eigen::Matrix4d tr;
float s = current->findBestSubset(tr, (int)(current->screenPoints().size() * 0.4f), 100);
LOG(INFO) << "SCORE = " << s;
if (s < lastScore) {
--
GitLab