diff --git a/applications/gui/src/main.cpp b/applications/gui/src/main.cpp
index d070e3941a2622fdbf5dd43840647a0edf29668f..979fc3359f850f2102fe29a61f018759dc6bc4d9 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/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp
index 1aa1bd353c177a511392ff08cde101e9834a8f34..ad61ccccb14555418106b9e408dc48e3daeccee2 100644
--- a/applications/gui/src/src_window.cpp
+++ b/applications/gui/src/src_window.cpp
@@ -74,87 +74,6 @@ Eigen::Matrix<T,4,4> lookAt
return res;
}
-class VirtualCameraView : public nanogui::ImageView {
- public:
- VirtualCameraView(nanogui::Widget *parent) : nanogui::ImageView(parent, 0) {
- src_ = nullptr;
- eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
- centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
- up_ = Eigen::Vector3f(0,1.0f,0);
- lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
- lerpSpeed_ = 0.4f;
- depth_ = false;
- }
-
- void setSource(Source *src) { src_ = src; }
-
- bool mouseButtonEvent(const nanogui::Vector2i &p, int button, bool down, int modifiers) {
- //LOG(INFO) << "Mouse move: " << p[0];
- if (src_ && down) {
- Eigen::Vector4f camPos = src_->point(p[0],p[1]);
- camPos *= -1.0f;
- Eigen::Vector4f worldPos = src_->getPose() * camPos;
- lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
- LOG(INFO) << "Depth at click = " << -camPos[2];
- }
- }
-
- bool keyboardEvent(int key, int scancode, int action, int modifiers) {
- LOG(INFO) << "Key press" << key << " - " << action;
- if (key == 81 || key == 83) {
- // TODO Should rotate around lookAt object, but requires correct depth
- Eigen::Quaternion<float> q; q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, up_);
- eye_ = (q * (eye_ - centre_)) + centre_;
- } else if (key == 84 || key == 82) {
- float scalar = (key == 84) ? 0.99f : 1.01f;
- eye_ = ((eye_ - centre_) * scalar) + centre_;
- }
- }
-
- void draw(NVGcontext *ctx) {
- //net_->broadcast("grab");
- if (src_) {
- cv::Mat rgb, depth;
- centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
- Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
-
- src_->setPose(viewPose);
- src_->grab();
- src_->getFrames(rgb, depth);
-
- if (depth_) {
- if (depth.rows > 0) {
- cv::Mat idepth;
- depth.convertTo(idepth, CV_8U, 255.0f / 10.0f); // TODO(nick)
- applyColorMap(idepth, idepth, cv::COLORMAP_JET);
- texture_.update(idepth);
- bindImage(texture_.texture());
- }
- } else {
- if (rgb.rows > 0) {
- texture_.update(rgb);
- bindImage(texture_.texture());
- }
- }
-
- screen()->performLayout(ctx);
- }
- ImageView::draw(ctx);
- }
-
- void setDepth(bool d) { depth_ = d; }
-
- private:
- Source *src_;
- GLTexture texture_;
- Eigen::Vector3f eye_;
- Eigen::Vector3f centre_;
- Eigen::Vector3f up_;
- Eigen::Vector3f lookPoint_;
- float lerpSpeed_;
- bool depth_;
-};
-
SourceWindow::SourceWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
: nanogui::Window(parent, "Source View"), ctrl_(ctrl) {
setLayout(new nanogui::GroupLayout());
diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp
index be5154f30f67e56c556410b3e3df3766e64595e1..20f8eac6ab9ad751bc4021d6f5469d6480affb2d 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 d1c2c095a6c9ae928416b5216694fb07be8726f3..f3a29a9fd329275d822c8a547190c4762b7a855f 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 ec27f43ba89cb1bdf71f57c5ba74c29ac4e25999..ad8c1e8cd71c442f10c18f0611d6b9fcce5ecc2c 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/reconstruct/src/virtual_source.cpp b/applications/reconstruct/src/virtual_source.cpp
index 578b360907a04a8198395a778ca1073cd7ae94be..0bc4abf4a72b089b50b786bd2d64daa4d1e94c02 100644
--- a/applications/reconstruct/src/virtual_source.cpp
+++ b/applications/reconstruct/src/virtual_source.cpp
@@ -50,7 +50,8 @@ bool VirtualSource::grab() {
params.m_sensorDepthWorldMin = params_.minDepth;
params.m_sensorDepthWorldMax = params_.maxDepth;
- rays_->render(scene_->getHashData(), scene_->getHashParams(), params, host_->getPose());
+ // TODO(Nick) Use double precision pose here
+ rays_->render(scene_->getHashData(), scene_->getHashParams(), params, host_->getPose().cast<float>());
//unique_lock<mutex> lk(mutex_);
if (rays_->isIntegerDepth()) {
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 134b9088a06ac505d72d77ab6bbc7aafdf90ed44..5292905e9e7e44dee329e1d9b7ee60d557b2d4c7 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -2,6 +2,9 @@
#include <nlohmann/json.hpp>
#include <random>
#include <chrono>
+#include <thread>
+#include <opencv2/xphoto.hpp>
+#include <pcl/registration/icp.h>
using ftl::registration::Correspondances;
using std::string;
@@ -14,6 +17,7 @@ using pcl::PointXYZRGB;
using nlohmann::json;
using std::tuple;
using std::make_tuple;
+using cv::Mat;
Correspondances::Correspondances(Source *src, Source *targ)
@@ -30,61 +34,270 @@ Correspondances::Correspondances(Correspondances *parent, Source *src)
transform_.setIdentity();
}
-bool Correspondances::add(int tx, int ty, int sx, int sy) {
- Eigen::Vector4f p1 = targ_->point(tx,ty);
- Eigen::Vector4f p2 = src_->point(sx,sy);
+static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
+ const int rows = in[0].rows;
+ const int cols = in[0].cols;
+
+ // todo: create new only if out is empty (or wrong shape/type)
+ out = Mat(rows, cols, CV_32FC1);
+
+ for (int i = 0; i < rows * cols; ++i) {
+ double sum = 0;
+ int good_values = 0;
+
+ // Calculate mean
+ for (int i_in = 0; i_in < in.size(); ++i_in) {
+ double d = in[i_in].at<float>(i);
+ if (d < 40.0) {
+ good_values++;
+ sum += d;
+ }
+ }
+
+ if (good_values > 0) {
+ sum /= (double)good_values;
+
+ // Calculate variance
+ double var = 0.0;
+ for (int i_in = 0; i_in < in.size(); ++i_in) {
+ double d = in[i_in].at<float>(i);
+ if (d < 40.0) {
+ double delta = (d*1000.0 - sum*1000.0);
+ var += delta*delta;
+ }
+ }
+ if (good_values > 1) var /= (double)(good_values-1);
+ else var = 0.0;
+
+ //LOG(INFO) << "VAR " << var;
+
+ if (var < varThresh) {
+ out.at<float>(i) = (float)sum;
+ } else {
+ out.at<float>(i) = 41.0f;
+ }
+ } else {
+ out.at<float>(i) = 41.0f;
+ }
+ }
+}
+
+static PointXYZ makePCL(Source *s, int x, int y) {
+ Eigen::Vector4d p1 = s->point(x,y);
PointXYZ pcl_p1;
pcl_p1.x = p1[0];
pcl_p1.y = p1[1];
pcl_p1.z = p1[2];
+ return pcl_p1;
+}
+
+void Correspondances::clear() {
+ targ_cloud_->clear();
+ src_cloud_->clear();
+ src_index_ = cv::Mat(cv::Size(src_->parameters().width, src_->parameters().height), CV_32SC1, cv::Scalar(-1));
+ targ_index_ = cv::Mat(cv::Size(targ_->parameters().width, targ_->parameters().height), CV_32SC1, cv::Scalar(-1));
+ src_feat_.clear();
+ targ_feat_.clear();
+ log_.clear();
+}
+
+void Correspondances::clearCorrespondances() {
+ src_feat_.clear();
+ targ_feat_.clear();
+ log_.clear();
+ uptodate_ = false;
+}
- PointXYZ pcl_p2;
- pcl_p2.x = p2[0];
- pcl_p2.y = p2[1];
- pcl_p2.z = p2[2];
+bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
+ Mat d1, d2;
+ size_t buffer_size = 10;
+ size_t buffer_i = 0;
+ vector<vector<Mat>> buffer(2, vector<Mat>(buffer_size));
- if (pcl_p2.z >= 40.0f || pcl_p1.z >= 40.0f) {
- LOG(WARNING) << "Bad point choosen";
+ for (size_t i = 0; i < buffer_size; ++i) {
+ src_->grab();
+ targ_->grab();
+ src_->getFrames(rgb1, d1);
+ targ_->getFrames(rgb2, d2);
+ buffer[0][i] = d1;
+ buffer[1][i] = d2;
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ }
+ averageDepth(buffer[0], d1, 0.00000005f);
+ averageDepth(buffer[1], d2, 0.00000005f);
+ Mat d1_v, d2_v;
+ d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
+ d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);
+ applyColorMap(d1_v, d1_v, cv::COLORMAP_JET);
+ applyColorMap(d2_v, d2_v, cv::COLORMAP_JET);
+
+ cv::imshow("smooth d1", d1_v);
+ cv::imshow("smooth d2", d2_v);
+
+ // Should be done in RGB-Depth source class
+ cv::Ptr<cv::xphoto::WhiteBalancer> wb;
+ wb = cv::xphoto::createSimpleWB();
+ wb->balanceWhite(rgb1, rgb1);
+ wb->balanceWhite(rgb2, rgb2);
+
+ // Build point clouds
+ clear();
+ int six=0;
+ int tix=0;
+
+ for (int y=0; y<rgb1.rows; y++) {
+ //unsigned char *rgb1ptr = rgb1.ptr(y);
+ //unsigned char *rgb2ptr = rgb2.ptr(y);
+ float *d1ptr = (float*)d1.ptr(y);
+ float *d2ptr = (float*)d2.ptr(y);
+
+ for (int x=0; x<rgb1.cols; x++) {
+ float d1_value = d1ptr[x];
+ float d2_value = d2ptr[x];
+
+ if (d1_value < 39.0f) {
+ // Make PCL points with specific depth value
+ pcl::PointXYZ p1;
+ Eigen::Vector4d p1e = src_->point(x,y,d1_value);
+ p1.x = p1e[0];
+ p1.y = p1e[1];
+ p1.z = p1e[2];
+ src_cloud_->push_back(p1);
+ src_index_.at<int>(y,x) = six;
+ six++;
+ }
+
+ if (d2_value < 39.0f) {
+ // Make PCL points with specific depth value
+ pcl::PointXYZ p2;
+ Eigen::Vector4d p2e = targ_->point(x,y,d2_value);
+ p2.x = p2e[0];
+ p2.y = p2e[1];
+ p2.z = p2e[2];
+ targ_cloud_->push_back(p2);
+ targ_index_.at<int>(y,x) = tix;
+ tix++;
+ }
+ }
+ }
+}
+
+bool Correspondances::add(int tx, int ty, int sx, int sy) {
+ LOG(INFO) << "Adding...";
+ int tix = targ_index_.at<int>(ty,tx);
+ int six = src_index_.at<int>(sy,sx);
+
+ // Validate feature
+ if (tix == -1 || six == -1) {
+ LOG(WARNING) << "Bad point";
return false;
}
- targ_cloud_->push_back(pcl_p1);
- src_cloud_->push_back(pcl_p2);
log_.push_back(make_tuple(tx,ty,sx,sy));
+ // Record correspondance point cloud point indexes
+ src_feat_.push_back(six);
+ targ_feat_.push_back(tix);
+
uptodate_ = false;
+ LOG(INFO) << "Point added: " << tx << "," << ty << " and " << sx << "," << sy;
return true;
}
-PointXYZ makePCL(Source *s, int x, int y) {
- Eigen::Vector4f p1 = s->point(x,y);
- PointXYZ pcl_p1;
- pcl_p1.x = p1[0];
- pcl_p1.y = p1[1];
- pcl_p1.z = p1[2];
- return pcl_p1;
-}
-
void Correspondances::removeLast() {
uptodate_ = false;
- targ_cloud_->erase(targ_cloud_->end()-1);
- src_cloud_->erase(src_cloud_->end()-1);
+ targ_feat_.erase(targ_feat_.end()-1);
+ src_feat_.erase(src_feat_.end()-1);
log_.pop_back();
}
-double Correspondances::estimateTransform() {
+double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
+ pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
+ pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
+
+ validate.setMaxRange(0.1);
+
+ svd.estimateRigidTransformation(*src_cloud_, src_feat_, *targ_cloud_, targ_feat_, T);
+ return validate.validateTransformation(src_cloud_, targ_cloud_, T);
+}
+
+double Correspondances::estimateTransform(Eigen::Matrix4d &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
+ 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>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ vector<int> idx;
+
+ for (int i=0; i<src_feat.size(); i++) {
+ idx.push_back(i);
+ src_cloud->push_back(src_cloud_->at(src_feat[i]));
+ targ_cloud->push_back(targ_cloud_->at(targ_feat[i]));
+ }
+
+ pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
+
+ validate.setMaxRange(0.1);
+
+ svd.estimateRigidTransformation(*tsrc_cloud, idx, *targ_cloud, idx, T);
+ T = T * transform_;
uptodate_ = true;
- int n_points = targ_cloud_->size();
+ float score = validate.validateTransformation(src_cloud, targ_cloud, T);
+ return score;
+}
+
+double Correspondances::icp() {
+ //pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- vector<int> idx(n_points);
- for (int i = 0; i < n_points; i++) { idx[i] = i; }
+ pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
- pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+ vector<int> idx;
- svd.estimateRigidTransformation(*src_cloud_, idx, *targ_cloud_, idx, transform_);
+ for (int i=0; i<src_feat_.size(); i++) {
+ idx.push_back(i);
+ src_cloud->push_back(src_cloud_->at(src_feat_[i]));
+ targ_cloud->push_back(targ_cloud_->at(targ_feat_[i]));
+ }
- return validate.validateTransformation(src_cloud_, targ_cloud_, transform_);
+ pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
+
+ pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
+ icp.setInputSource(tsrc_cloud);
+ icp.setInputTarget(targ_cloud);
+ icp.align(*final_cloud);
+ LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
+ transform_ *= icp.getFinalTransformation();
+ return icp.getFitnessScore();
+}
+
+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>);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ for (int i=0; i<idx.size(); i++) {
+ src_cloud->push_back(src_cloud_->at(src_feat_[idx[i]]));
+ targ_cloud->push_back(targ_cloud_->at(targ_feat_[idx[i]]));
+ }
+
+ pcl::transformPointCloud(*src_cloud, *tsrc_cloud, T_in);
+
+ pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
+ icp.setInputSource(tsrc_cloud);
+ icp.setInputTarget(targ_cloud);
+ icp.align(*final_cloud);
+ LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
+ T_out = T_in * icp.getFinalTransformation();
+ return icp.getFitnessScore();
}
static std::default_random_engine generator;
@@ -99,64 +312,94 @@ void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>
}
}
-double Correspondances::findBest(Eigen::Matrix4f &tr, const std::vector<std::tuple<int,int,int,int>> &p, const std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &pp, int K, int N) {
- double score = 10000.0f;
- vector<tuple<int,int,int,int>> best;
- Eigen::Matrix4f bestT;
+void Correspondances::getFeatures3D(std::vector<Eigen::Vector4d> &f) {
+ for (int i=0; i<src_feat_.size(); i++) {
+ Eigen::Vector4d p;
+ const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+ p[0] = pp.x;
+ p[1] = pp.y;
+ p[2] = pp.z;
+ p[3] = 1.0;
+ f.push_back(p);
+ }
+}
- pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
- pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+void Correspondances::getTransformedFeatures(std::vector<Eigen::Vector4d> &f) {
+ for (int i=0; i<src_feat_.size(); i++) {
+ Eigen::Vector4d p;
+ const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+ p[0] = pp.x;
+ p[1] = pp.y;
+ p[2] = pp.z;
+ p[3] = 1.0;
+ f.push_back(transform_ * p);
+ }
+}
+
+void Correspondances::getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f) {
+ for (int i=0; i<src_feat_.size(); i++) {
+ Eigen::Vector4d p;
+ const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+ p[0] = pp.x;
+ p[1] = pp.y;
+ p[2] = pp.z;
+ p[3] = 1.0;
+ f.push_back(src_->point(transform_ * p));
+ }
+}
+
+double Correspondances::findBestSubset(Eigen::Matrix4d &tr, int K, int N) {
+ double score = 10000.0f;
+ vector<int> best;
+ Eigen::Matrix4d bestT;
std::mt19937 rng(std::chrono::steady_clock::now().time_since_epoch().count());
- std::uniform_int_distribution<int> distribution(0,p.size()-1);
+ std::uniform_int_distribution<int> distribution(0,src_feat_.size()-1);
//int dice_roll = distribution(generator);
auto dice = std::bind ( distribution, rng );
- vector<int> idx(K);
- for (int i = 0; i < K; i++) { idx[i] = i; }
+ vector<int> sidx(K);
+ vector<int> tidx(K);
+ //for (int i = 0; i < K; i++) { idx[i] = i; }
+
+ // 1. Build complete point clouds using filtered and smoothed depth maps
+ // 2. Build a full index map of x,y to point cloud index.
+ // 3. Generate random subsets of features using index map
+ // 4. Find minimum
for (int n=0; n<N; n++) {
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_t(new PointCloud<PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_s(new PointCloud<PointXYZ>);
- vector<tuple<int,int,int,int>> ps;
+ sidx.clear();
+ tidx.clear();
+ vector<int> idx;
for (int k=0; k<K; k++) {
int r = dice();
- //LOG(INFO) << "DICE " << r << " - " << K;
- //ps.push_back(p[r]);
- auto [sx,sy,tx,ty] = p[r];
- //LOG(INFO) << "POINT " << sx << "," << sy;
- //auto p1 = makePCL(src_, sx, sy);
- //auto p2 = makePCL(targ_, tx, ty);
-
- auto [p1,p2] = pp[r];
-
- if (p1.z >= 30.0f || p2.z >= 30.0f) { k--; continue; }
- ps.push_back(std::make_tuple(tx,ty,sx,sy));
- cloud_s->push_back(p1);
- cloud_t->push_back(p2);
+ idx.push_back(r);
+ sidx.push_back(src_feat_[r]);
+ tidx.push_back(targ_feat_[r]);
}
- Eigen::Matrix4f T;
- svd.estimateRigidTransformation(*cloud_s, idx, *cloud_t, idx, T);
- float scoreT = validate.validateTransformation(cloud_s, cloud_t, T);
+ Eigen::Matrix4d T;
+ float scoreT = estimateTransform(T, sidx, tidx);
if (scoreT < score) {
score = scoreT;
- best = ps;
bestT = T;
+ best = idx;
}
}
+ return icp(bestT, tr, best);
+
// TODO Add these best points to actual clouds.
- log_ = best;
- tr = bestT;
+ //log_ = best;
+ //tr = bestT;
//uptodate_ = true;
- return score;
+ //return score;
}
-Eigen::Matrix4f Correspondances::transform() {
- if (!uptodate_) estimateTransform();
+Eigen::Matrix4d Correspondances::transform() {
+ if (!uptodate_) estimateTransform(transform_);
return (parent_) ? parent_->transform() * transform_ : transform_;
}
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index bd08dd6dd9a6fbea3678a8667f35a92391714b41..57f4442f6c893863fb2df146ec2ae13894691f9f 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -32,6 +32,11 @@ class Correspondances {
ftl::rgbd::Source *source() { return src_; }
ftl::rgbd::Source *target() { return targ_; }
+ void clear();
+ void clearCorrespondances();
+
+ bool capture(cv::Mat &rgb1, cv::Mat &rgb2);
+
/**
* Add a new correspondance point. The point will only be added if there is
* a valid depth value at that location.
@@ -47,6 +52,9 @@ class Correspondances {
void removeLast();
const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
+ void getFeatures3D(std::vector<Eigen::Vector4d> &f);
+ void getTransformedFeatures(std::vector<Eigen::Vector4d> &f);
+ void getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f);
void setPoints(const std::vector<std::tuple<int,int,int,int>> &p) { log_ = p; }
@@ -55,9 +63,13 @@ class Correspondances {
*
* @return Validation score of the transform.
*/
- double estimateTransform();
+ double estimateTransform(Eigen::Matrix4d &);
+ double estimateTransform(Eigen::Matrix4d &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
+
+ double findBestSubset(Eigen::Matrix4d &tr, int K, int N);
- double findBest(Eigen::Matrix4f &tr, const std::vector<std::tuple<int,int,int,int>> &p, const std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2, int K, int N);
+ double icp();
+ double icp(const Eigen::Matrix4d &T_in, Eigen::Matrix4d &T_out, const std::vector<int> &idx);
void convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2);
@@ -65,9 +77,9 @@ class Correspondances {
* Get the estimated transform. This includes any parent transforms to make
* it relative to root camera.
*/
- Eigen::Matrix4f transform();
+ Eigen::Matrix4d transform();
- void setTransform(Eigen::Matrix4f &t) { uptodate_ = true; transform_ = t; }
+ void setTransform(Eigen::Matrix4d &t) { uptodate_ = true; transform_ = t; }
private:
Correspondances *parent_;
@@ -75,9 +87,13 @@ class Correspondances {
ftl::rgbd::Source *src_;
pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud_;
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud_;
- Eigen::Matrix4f transform_;
+ Eigen::Matrix4d transform_;
bool uptodate_;
std::vector<std::tuple<int,int,int,int>> log_;
+ cv::Mat src_index_;
+ cv::Mat targ_index_;
+ std::vector<int> targ_feat_;
+ std::vector<int> src_feat_;
};
}
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index ca7f1cbf5361305c8fa30042eceb82dfb08db03f..be729155c66cd9de3d155c2c476e3c0b4cd68722 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -14,7 +14,6 @@
#include <opencv2/imgcodecs.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
-#include <opencv2/xphoto.hpp>
#include <map>
@@ -46,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]); }
@@ -63,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);
@@ -77,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;
@@ -90,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;
@@ -128,32 +127,6 @@ static void build_correspondances(const vector<Source*> &sources, map<string, Co
}
}
-void averageDepth(vector<Mat> &in, Mat &out) {
- const int rows = in[0].rows;
- const int cols = in[0].cols;
-
- // todo: create new only if out is empty (or wrong shape/type)
- out = Mat(rows, cols, CV_32FC1);
-
- for (int i = 0; i < rows * cols; ++i) {
- float sum = 0;
- int good_values = 0;
- for (int i_in = 0; i_in < in.size(); ++i_in) {
- float d = in[i_in].at<float>(i);
- if (d < 40) {
- good_values++;
- sum += d;
- }
- }
-
- if (good_values > 0) {
- out.at<float>(i) = sum / (float) good_values;
- } else {
- out.at<float>(i) = 41.0f;
- }
- }
-}
-
void ftl::registration::manual(ftl::Configurable *root) {
using namespace cv;
@@ -178,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]);
@@ -209,12 +182,9 @@ void ftl::registration::manual(ftl::Configurable *root) {
bool depthtoggle = false;
double lastScore = 0.0;
bool insearch = false;
+ int point_n = -1;
Correspondances *current = corrs[sources[curtarget]->getURI()];
-
- // Features for current frame...
- vector<tuple<int,int,int,int>> feat;
- vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> pclfeat;
while (ftl::running) {
if (!freeze) {
@@ -249,21 +219,38 @@ void ftl::registration::manual(ftl::Configurable *root) {
tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
- Ptr<xphoto::WhiteBalancer> wb;
+ /*Ptr<xphoto::WhiteBalancer> wb;
wb = xphoto::createSimpleWB();
wb->balanceWhite(tsrgb, srgb);
- wb->balanceWhite(ttrgb, trgb);
+ wb->balanceWhite(ttrgb, trgb);*/
// Apply points and labels...
auto &points = current->screenPoints();
- for (auto &p : points) {
- auto [tx,ty,sx,sy] = p;
+ if (point_n == -1) {
+ for (int i=0; i<points.size(); i++) {
+ //for (auto &p : points) {
+ auto [tx,ty,sx,sy] = points[i];
+ drawMarker(srgb, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
+ drawMarker(sdepth, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
+ drawMarker(trgb, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
+ drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
+ }
+ } else if(point_n < points.size()) {
+ auto [tx,ty,sx,sy] = points[point_n];
drawMarker(srgb, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
drawMarker(sdepth, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
drawMarker(trgb, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
}
+ vector<Eigen::Vector2i> tpoints;
+ current->getTransformedFeatures2D(tpoints);
+ for (int i=0; i<tpoints.size(); i++) {
+ Eigen::Vector2i p = tpoints[i];
+ drawMarker(trgb, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+ drawMarker(tdepth, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+ }
+
// Add most recent click position
drawMarker(srgb, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
drawMarker(trgb, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
@@ -287,32 +274,42 @@ void ftl::registration::manual(ftl::Configurable *root) {
curtarget = key-48;
if (curtarget >= sources.size()) curtarget = 0;
current = corrs[sources[curtarget]->getURI()];
- feat.clear();
- pclfeat.clear();
lastScore = 1000.0;
} else if (key == 'd') {
depthtoggle = !depthtoggle;
} else if (key == 'c') {
- feat.clear();
- pclfeat.clear();
+ current->clear();
+ lastScore = 1000.0;
+ } else if (key == 'n') {
+ point_n++;
+ } else if (key == 'p') {
+ LOG(INFO) << "Captured..";
lastScore = 1000.0;
+ Mat f1,f2;
+ current->capture(f1,f2);
}else if (key == 'a') {
+ Eigen::Matrix4d t;
if (current->add(lastTX,lastTY,lastSX,lastSY)) {
lastTX = lastTY = lastSX = lastSY = 0;
- lastScore = current->estimateTransform();
+ lastScore = current->estimateTransform(t);
+ current->setTransform(t);
}
} else if (key == 'u') {
current->removeLast();
- lastScore = current->estimateTransform();
+ 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();
}
@@ -325,19 +322,18 @@ void ftl::registration::manual(ftl::Configurable *root) {
} else if (key == 'g') {
if (!insearch) {
insearch = true;
- LOG(INFO) << "Features matched = " << feat.size();
- ftl::pool.push([&lastScore,&insearch,current,&feat,&pclfeat](int id) {
+ ftl::pool.push([&lastScore,&insearch,current](int id) {
LOG(INFO) << "START";
lastScore = 1000.0;
do {
- Eigen::Matrix4f tr;
- float s = current->findBest(tr, feat, pclfeat, 20, 100);
- //LOG(INFO) << "SCORE = " << s;
+ Eigen::Matrix4d tr;
+ float s = current->findBestSubset(tr, (int)(current->screenPoints().size() * 0.4f), 100);
+ LOG(INFO) << "SCORE = " << s;
if (s < lastScore) {
lastScore = s;
current->setTransform(tr);
- current->source()->setPose(current->transform());
+ //current->source()->setPose(current->transform());
}
} while (ftl::running && insearch && lastScore > 0.000001);
insearch = false;
@@ -347,55 +343,24 @@ void ftl::registration::manual(ftl::Configurable *root) {
auto [sx,sy,tx,ty] = feat[i];
current->add(tx,ty,sx,sy);
}
- lastScore = current->estimateTransform();*/
+ lastsScore = current->estimateTransform();*/
} else {
insearch = false;
}
} else if (key == 'f') {
- Mat f1, d1, f2, d2;
-
- size_t buffer_size = 10;
- size_t buffer_i = 0;
- vector<vector<Mat>> buffer(2, vector<Mat>(buffer_size));
-
- for (size_t i = 0; i < buffer_size; ++i) {
- current->source()->grab();
- current->target()->grab();
- current->source()->getFrames(f1, d1);
- current->target()->getFrames(f2, d2);
- buffer[0][i] = d1;
- buffer[1][i] = d2;
-
- std::this_thread::sleep_for(std::chrono::milliseconds(20));
- }
- averageDepth(buffer[0], d1);
- averageDepth(buffer[1], d2);
- Mat d1_v, d2_v;
- d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
- d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);
- applyColorMap(d1_v, d1_v, cv::COLORMAP_JET);
- applyColorMap(d2_v, d2_v, cv::COLORMAP_JET);
-
- cv::imshow("smooth d1", d1_v);
- cv::imshow("smooth d2", d2_v);
-
- Ptr<xphoto::WhiteBalancer> wb;
- wb = xphoto::createSimpleWB();
- wb->balanceWhite(f1, f1);
- wb->balanceWhite(f2, f2);
-
- vector<tuple<int,int,int,int>> tfeat;
- if (!insearch && ftl::registration::featuresSIFT(f1, f2, tfeat, 10)) {
- vector<tuple<int,int,int,int>> tfeat2;
- for (int j=0; j<tfeat.size(); j++) {
- auto [sx,sy,tx,ty] = tfeat[j];
- tfeat2.push_back(std::make_tuple(tx,ty,sx,sy));
+ if (!insearch) {
+ Mat f1,f2;
+ current->capture(f1,f2);
+
+ vector<tuple<int,int,int,int>> feat;
+ if (ftl::registration::featuresSIFT(f1, f2, feat, 2)) {
+ for (int j=0; j<feat.size(); j++) {
+ auto [sx,sy,tx,ty] = feat[j];
+ current->add(tx, ty, sx, sy);
+ }
}
- current->setPoints(tfeat2);
- feat.insert(feat.end(), tfeat.begin(), tfeat.end());
- //vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> tpclfeat;
- current->convertToPCL(tfeat,pclfeat);
+ LOG(INFO) << "Found " << current->screenPoints().size() << " features";
} else {
LOG(ERROR) << "Can't add features whilst searching...";
}
diff --git a/components/renderers/cpp/include/ftl/rgbd_display.hpp b/components/renderers/cpp/include/ftl/rgbd_display.hpp
index 2d0c26a365e7bd0f9b3fe7e71bf9d24481334f9f..9c1be76fb5f38a584d3032d50b6ef046f0d0b605 100644
--- a/components/renderers/cpp/include/ftl/rgbd_display.hpp
+++ b/components/renderers/cpp/include/ftl/rgbd_display.hpp
@@ -28,10 +28,10 @@ class Display : public ftl::Configurable {
Source *source_;
std::string name_;
std::vector<std::function<void(int)>> key_handlers_;
- Eigen::Vector3f eye_;
- Eigen::Vector3f centre_;
- Eigen::Vector3f up_;
- Eigen::Vector3f lookPoint_;
+ Eigen::Vector3d eye_;
+ Eigen::Vector3d centre_;
+ Eigen::Vector3d up_;
+ Eigen::Vector3d lookPoint_;
float lerpSpeed_;
bool active_;
MouseAction mouseaction_;
diff --git a/components/renderers/cpp/src/rgbd_display.cpp b/components/renderers/cpp/src/rgbd_display.cpp
index 0623bf38c91116b116b85678e732ce98741f0a23..a0d79f8aeeb42b0a0a1fd281c5f3e9065b43780c 100644
--- a/components/renderers/cpp/src/rgbd_display.cpp
+++ b/components/renderers/cpp/src/rgbd_display.cpp
@@ -64,20 +64,20 @@ void Display::init() {
source_ = nullptr;
cv::namedWindow(name_, cv::WINDOW_KEEPRATIO);
- eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
- centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
- up_ = Eigen::Vector3f(0,1.0f,0);
- lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
+ eye_ = Eigen::Vector3d(0.0, 0.0, 0.0);
+ centre_ = Eigen::Vector3d(0.0, 0.0, -4.0);
+ up_ = Eigen::Vector3d(0,1.0,0);
+ lookPoint_ = Eigen::Vector3d(0.0,0.0,-4.0);
lerpSpeed_ = 0.4f;
// Keyboard camera controls
onKey([this](int key) {
//LOG(INFO) << "Key = " << key;
if (key == 81 || key == 83) {
- Eigen::Quaternion<float> q; q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, up_);
+ Eigen::Quaternion<double> q; q = Eigen::AngleAxis<double>((key == 81) ? 0.01 : -0.01, up_);
eye_ = (q * (eye_ - centre_)) + centre_;
} else if (key == 84 || key == 82) {
- float scalar = (key == 84) ? 0.99f : 1.01f;
+ double scalar = (key == 84) ? 0.99 : 1.01;
eye_ = ((eye_ - centre_) * scalar) + centre_;
}
});
@@ -86,10 +86,10 @@ void Display::init() {
mouseaction_ = [this]( int event, int ux, int uy, int) {
//LOG(INFO) << "Mouse " << ux << "," << uy;
if (event == 1 && source_) { // click
- Eigen::Vector4f camPos = source_->point(ux,uy);
+ Eigen::Vector4d camPos = source_->point(ux,uy);
camPos *= -1.0f;
- Eigen::Vector4f worldPos = source_->getPose() * camPos;
- lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
+ Eigen::Vector4d worldPos = source_->getPose() * camPos;
+ lookPoint_ = Eigen::Vector3d(worldPos[0],worldPos[1],worldPos[2]);
LOG(INFO) << "Depth at click = " << -camPos[2];
}
};
@@ -118,7 +118,7 @@ void Display::update() {
if (!source_) return;
centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
- Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
+ Eigen::Matrix4d viewPose = lookAt<double>(eye_,centre_,up_).inverse();
source_->setPose(viewPose);
Mat rgb, depth;
diff --git a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
index 65d4b5d3f710973871103faab84cd51ffb33d321..cee6e61794db6a7d9cead54d9ffa2d035baee7f7 100644
--- a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
@@ -22,7 +22,7 @@ class Source {
virtual bool grab()=0;
virtual bool isReady() { return false; };
- virtual void setPose(const Eigen::Matrix4f &pose) { };
+ virtual void setPose(const Eigen::Matrix4d &pose) { };
protected:
ftl::rgbd::Source *host_;
diff --git a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp b/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
index 7e055b7cc3e044450a6d4d7bae29d4a1a6a2ca40..9643b1312b59ba068e300bc304a01c3f62d5d955 100644
--- a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
@@ -24,9 +24,9 @@ public:
explicit SnapshotWriter(const std::string &filename);
~SnapshotWriter();
- bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth, const Eigen::Matrix4f &pose, const ftl::rgbd::Camera ¶ms);
+ bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth, const Eigen::Matrix4d &pose, const ftl::rgbd::Camera ¶ms);
bool addMat(const std::string &name, const cv::Mat &mat, const std::string &format="tiff");
- bool addEigenMatrix4f(const std::string &name, const Eigen::Matrix4f &m, const std::string &format="pfm");
+ bool addEigenMatrix4d(const std::string &name, const Eigen::Matrix4d &m, const std::string &format="pfm");
bool addFile(const std::string &name, const std::vector<uchar> &buf);
bool addFile(const std::string &name, const uchar *buf, const size_t len);
@@ -38,7 +38,7 @@ private:
struct SnapshotEntry {
cv::Mat rgb;
cv::Mat depth;
- Eigen::Matrix4f pose;
+ Eigen::Matrix4d pose;
ftl::rgbd::Camera params;
uint status;
SnapshotEntry() : status(1+2+4+8) {};
@@ -49,7 +49,7 @@ public:
explicit SnapshotReader(const std::string &filename);
~SnapshotReader();
- bool getCameraRGBD(const std::string &id, cv::Mat &rgb, cv::Mat &depth, Eigen::Matrix4f &pose, ftl::rgbd::Camera ¶ms);
+ bool getCameraRGBD(const std::string &id, cv::Mat &rgb, cv::Mat &depth, Eigen::Matrix4d &pose, ftl::rgbd::Camera ¶ms);
std::vector<std::string> getIds();
private:
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 885046c3a449f9e441ed0cb71e53c1cd0ae4fb85..a810fb6f947bf8a80aab7f8205ec8e73d39900b7 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -116,12 +116,12 @@ class Source : public ftl::Configurable {
* cameras this will move the camera, for physical cameras it is set by the
* registration process as it attempts to work out a cameras relative pose.
*/
- virtual void setPose(const Eigen::Matrix4f &pose);
+ virtual void setPose(const Eigen::Matrix4d &pose);
/**
* Get the camera position as a pose matrix.
*/
- const Eigen::Matrix4f &getPose() const;
+ const Eigen::Matrix4d &getPose() const;
/**
* Check what features this source has available.
@@ -131,7 +131,15 @@ class Source : public ftl::Configurable {
/**
* Get a point in camera coordinates at specified pixel location.
*/
- Eigen::Vector4f point(uint x, uint y);
+ Eigen::Vector4d point(uint x, uint y);
+
+ /**
+ * Get a point in camera coordinates at specified pixel location, with a
+ * given depth value.
+ */
+ Eigen::Vector4d point(uint x, uint y, double d);
+
+ Eigen::Vector2i point(const Eigen::Vector4d &p);
/**
* Force the internal implementation to be reconstructed.
@@ -151,7 +159,7 @@ class Source : public ftl::Configurable {
cv::Mat rgb_;
cv::Mat depth_;
Camera params_; // TODO Find better solution
- Eigen::Matrix4f pose_;
+ Eigen::Matrix4d pose_;
ftl::net::Universe *net_;
std::shared_mutex mutex_;
diff --git a/components/rgbd-sources/src/local.cpp b/components/rgbd-sources/src/local.cpp
index a0d6334b357e8c51243fa2922123333092640e23..02bf0d36ef9c1635633d8596c4bc48cf398a3540 100644
--- a/components/rgbd-sources/src/local.cpp
+++ b/components/rgbd-sources/src/local.cpp
@@ -270,10 +270,11 @@ bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
0, 0, cv::INTER_LINEAR);
}
- cv::Ptr<cv::xphoto::WhiteBalancer> wb;
+ // Note: this seems to be too slow on CPU...
+ /*cv::Ptr<cv::xphoto::WhiteBalancer> wb;
wb = cv::xphoto::createSimpleWB();
wb->balanceWhite(l, l);
- wb->balanceWhite(r, r);
+ wb->balanceWhite(r, r);*/
if (flip_v_) {
Mat tl, tr;
diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp
index bd4d5127fad7bd6375226d580f5f01b2441c98ca..674bc24719233954f6a1d32f10bf6a506a9220db 100644
--- a/components/rgbd-sources/src/net.cpp
+++ b/components/rgbd-sources/src/net.cpp
@@ -115,7 +115,7 @@ void NetSource::_recvChunk(int frame, int chunk, bool delta, const vector<unsign
//}
}
-void NetSource::setPose(const Eigen::Matrix4f &pose) {
+void NetSource::setPose(const Eigen::Matrix4d &pose) {
if (!active_) return;
vector<unsigned char> vec((unsigned char*)pose.data(), (unsigned char*)(pose.data()+(pose.size())));
diff --git a/components/rgbd-sources/src/net.hpp b/components/rgbd-sources/src/net.hpp
index 3a986d0647ae465e355dbf887e283f61a43b53b3..ce88d10c399badc35c18cfeb74883e4aaa8afce8 100644
--- a/components/rgbd-sources/src/net.hpp
+++ b/components/rgbd-sources/src/net.hpp
@@ -25,7 +25,7 @@ class NetSource : public detail::Source {
bool grab();
bool isReady();
- void setPose(const Eigen::Matrix4f &pose);
+ void setPose(const Eigen::Matrix4d &pose);
void reset();
diff --git a/components/rgbd-sources/src/snapshot.cpp b/components/rgbd-sources/src/snapshot.cpp
index 51b8a635f1ac906773ccb038f1dfa50e692765da..5599364c6508184c57651d7c46ca54951ac0d4ab 100644
--- a/components/rgbd-sources/src/snapshot.cpp
+++ b/components/rgbd-sources/src/snapshot.cpp
@@ -5,7 +5,7 @@
using namespace ftl::rgbd;
using cv::Mat;
-using Eigen::Matrix4f;
+using Eigen::Matrix4d;
using cv::imencode;
using cv::imdecode;
@@ -13,8 +13,6 @@ using cv::imdecode;
using std::string;
using std::vector;
-using Eigen::Matrix4f;
-
// TODO: move to camera_params
using ftl::rgbd::Camera;
@@ -120,18 +118,18 @@ bool SnapshotWriter::addMat(const string &name, const Mat &mat, const std::strin
return retval;
}
-bool SnapshotWriter::addEigenMatrix4f(const string &name, const Matrix4f &m, const string &format) {
+bool SnapshotWriter::addEigenMatrix4d(const string &name, const Matrix4d &m, const string &format) {
Mat tmp;
cv::eigen2cv(m, tmp);
return addMat(name, tmp, format);
}
bool SnapshotWriter::addCameraRGBD(const string &name, const Mat &rgb, const Mat &depth,
- const Matrix4f &pose, const Camera ¶ms) {
+ const Matrix4d &pose, const Camera ¶ms) {
bool retval = true;
retval &= addMat(name + "-RGB", rgb);
retval &= addMat(name + "-D", depth);
- retval &= addEigenMatrix4f(name + "-POSE", pose);
+ retval &= addEigenMatrix4d(name + "-POSE", pose);
nlohmann::json j;
to_json(j, params);
@@ -223,7 +221,7 @@ bool SnapshotReader::readArchive() {
if (!readEntry(data)) continue;
Mat m_ = cv::imdecode(Mat(data), 0);
if ((m_.rows != 4) || (m_.cols != 4)) continue;
- cv::Matx44f pose_(m_);
+ cv::Matx44d pose_(m_);
cv::cv2eigen(pose_, snapshot.pose);
snapshot.status &= ~(1 << 2);
}
@@ -256,7 +254,7 @@ vector<string> SnapshotReader::getIds() {
}
bool SnapshotReader::getCameraRGBD(const string &id, Mat &rgb, Mat &depth,
- Matrix4f &pose, Camera ¶ms) {
+ Matrix4d &pose, Camera ¶ms) {
if (data_.find(id) == data_.end()) {
LOG(ERROR) << "entry not found: " << id;
return false;
diff --git a/components/rgbd-sources/src/snapshot_source.cpp b/components/rgbd-sources/src/snapshot_source.cpp
index 96c16e4d086df4bf39e7e8d884cc238d929b9167..367fc2861ef84647d6f600a68124e83eb04edf22 100644
--- a/components/rgbd-sources/src/snapshot_source.cpp
+++ b/components/rgbd-sources/src/snapshot_source.cpp
@@ -10,7 +10,7 @@ using ftl::rgbd::detail::SnapshotSource;
using std::string;
SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, SnapshotReader &reader, const string &id) : detail::Source(host) {
- Eigen::Matrix4f pose;
+ Eigen::Matrix4d pose;
reader.getCameraRGBD(id, rgb_, depth_, pose, params_);
setPose(pose);
}
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 787dbdadaa51ef8efb1d9f3d1938c4bc302d2c81..b3b9e82b27a54f56bd2f0242d72ad60be6f338e4 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -27,7 +27,7 @@ using ftl::rgbd::detail::NetSource;
using ftl::rgbd::detail::ImageSource;
using ftl::rgbd::capability_t;
-Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4f::Identity()), net_(nullptr) {
+Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(nullptr) {
impl_ = nullptr;
params_ = {0};
reset();
@@ -38,7 +38,7 @@ Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matri
});
}
-Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), pose_(Eigen::Matrix4f::Identity()), net_(net) {
+Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(net) {
impl_ = nullptr;
params_ = {0};
reset();
@@ -147,22 +147,38 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) {
depth = depth_;
}
-Eigen::Vector4f Source::point(uint ux, uint uy) {
+Eigen::Vector4d Source::point(uint ux, uint uy) {
const auto ¶ms = parameters();
- const float x = ((float)ux+(float)params.cx) / (float)params.fx;
- const float y = ((float)uy+(float)params.cy) / (float)params.fy;
+ const double x = ((double)ux+params.cx) / params.fx;
+ const double y = ((double)uy+params.cy) / params.fy;
SHARED_LOCK(mutex_,lk);
- const float depth = depth_.at<float>(uy,ux);
- return Eigen::Vector4f(x*depth,y*depth,depth,1.0);
+ const double depth = depth_.at<float>(uy,ux);
+ return Eigen::Vector4d(x*depth,y*depth,depth,1.0);
}
-void Source::setPose(const Eigen::Matrix4f &pose) {
+Eigen::Vector4d Source::point(uint ux, uint uy, double d) {
+ const auto ¶ms = parameters();
+ const double x = ((double)ux+params.cx) / params.fx;
+ const double y = ((double)uy+params.cy) / params.fy;
+ return Eigen::Vector4d(x*d,y*d,d,1.0);
+}
+
+Eigen::Vector2i Source::point(const Eigen::Vector4d &p) {
+ const auto ¶ms = parameters();
+ double x = p[0] / p[2];
+ double y = p[1] / p[2];
+ x *= params.fx;
+ y *= params.fy;
+ return Eigen::Vector2i((int)(x - params.cx), (int)(y - params.cy));
+}
+
+void Source::setPose(const Eigen::Matrix4d &pose) {
pose_ = pose;
if (impl_) impl_->setPose(pose);
}
-const Eigen::Matrix4f &Source::getPose() const {
+const Eigen::Matrix4d &Source::getPose() const {
return pose_;
}
diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp
index 34ee61f0afc8ecb8fd87d62b1a0f07b04d0e85cd..45ac6d4962631d96eb14fcaf88f8cfc733799757 100644
--- a/components/rgbd-sources/src/streamer.cpp
+++ b/components/rgbd-sources/src/streamer.cpp
@@ -51,7 +51,7 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
SHARED_LOCK(mutex_,slk);
if (sources_.find(uri) != sources_.end()) {
- Eigen::Matrix4f pose;
+ Eigen::Matrix4d pose;
memcpy(pose.data(), buf.data(), buf.size());
sources_[uri]->src->setPose(pose);
}