Skip to content
Snippets Groups Projects
Commit 94770c8a authored by Nicolas Pope's avatar Nicolas Pope
Browse files

Implements #82 registration app

parent 23bff5a2
No related branches found
No related tags found
No related merge requests found
......@@ -186,6 +186,10 @@ if (BUILD_VISION)
add_subdirectory(applications/vision)
endif()
if (HAVE_PCL)
add_subdirectory(applications/registration)
endif()
if (BUILD_RECONSTRUCT AND HAVE_PCL)
add_subdirectory(applications/reconstruct)
endif()
......
......@@ -233,7 +233,7 @@ Eigen::Matrix4f findTransformation(vector<PointCloud<PointXYZ>::Ptr> clouds_sour
// score new transformation
double score = 0.0;
for (size_t j = 0; j < n_clouds; ++j) {
score += validate.validateTransformation(clouds_source[j], clouds_target[j], T);
score += validate.validateTransformation(clouds_source[j], clouds_target[j], T); // CHECK Is use of T here a mistake??
}
score /= n_clouds;
......
......@@ -19,8 +19,8 @@ VirtualSource::VirtualSource(ftl::rgbd::Source *host)
params_.fy = params_.fx;
params_.width = rays_->value("width", 640);
params_.height = rays_->value("height", 480);
params_.cx = params_.width / 2;
params_.cy = params_.height / 2;
params_.cx = -((double)params_.width / 2);
params_.cy = -((double)params_.height / 2);
params_.maxDepth = rays_->value("max_depth", 10.0f);
params_.minDepth = rays_->value("min_depth", 0.1f);
......@@ -43,8 +43,8 @@ bool VirtualSource::grab() {
DepthCameraParams params;
params.fx = params_.fx;
params.fy = params_.fy;
params.mx = params_.cx;
params.my = params_.cy;
params.mx = -params_.cx;
params.my = -params_.cy;
params.m_imageWidth = params_.width;
params.m_imageHeight = params_.height;
params.m_sensorDepthWorldMin = params_.minDepth;
......
set(REGSRC
src/main.cpp
src/manual.cpp
src/correspondances.cpp
)
add_executable(ftl-register ${REGSRC})
target_include_directories(ftl-register PRIVATE src)
target_link_libraries(ftl-register ftlcommon ftlnet ftlrgbd Threads::Threads ${OpenCV_LIBS})
#include "correspondances.hpp"
#include <nlohmann/json.hpp>
using ftl::registration::Correspondances;
using std::string;
using std::map;
using std::vector;
using ftl::rgbd::Source;
using pcl::PointCloud;
using pcl::PointXYZ;
using pcl::PointXYZRGB;
using nlohmann::json;
using std::tuple;
using std::make_tuple;
Correspondances::Correspondances(Source *src, Source *targ)
: parent_(nullptr), targ_(targ), src_(src),
targ_cloud_(new PointCloud<PointXYZ>),
src_cloud_(new PointCloud<PointXYZ>), uptodate_(true) {
transform_.setIdentity();
}
Correspondances::Correspondances(Correspondances *parent, Source *src)
: parent_(parent), targ_(parent_->source()), src_(src),
targ_cloud_(new PointCloud<PointXYZ>),
src_cloud_(new PointCloud<PointXYZ>), uptodate_(true) {
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);
PointXYZ pcl_p1;
pcl_p1.x = p1[0];
pcl_p1.y = p1[1];
pcl_p1.z = p1[2];
PointXYZ pcl_p2;
pcl_p2.x = p2[0];
pcl_p2.y = p2[1];
pcl_p2.z = p2[2];
if (pcl_p2.z >= 40.0f || pcl_p1.z >= 40.0f) {
LOG(WARNING) << "Bad point choosen";
return false;
}
targ_cloud_->push_back(pcl_p1);
src_cloud_->push_back(pcl_p2);
log_.push_back(make_tuple(tx,ty,sx,sy));
uptodate_ = false;
return true;
}
void Correspondances::removeLast() {
uptodate_ = false;
targ_cloud_->erase(targ_cloud_->end()-1);
src_cloud_->erase(src_cloud_->end()-1);
log_.pop_back();
}
double Correspondances::estimateTransform() {
uptodate_ = true;
int n_points = targ_cloud_->size();
vector<int> idx(n_points);
for (int i = 0; i < n_points; i++) { idx[i] = i; }
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
svd.estimateRigidTransformation(*src_cloud_, idx, *targ_cloud_, idx, transform_);
return validate.validateTransformation(src_cloud_, targ_cloud_, transform_);
}
Eigen::Matrix4f Correspondances::transform() {
if (!uptodate_) estimateTransform();
return (parent_) ? parent_->transform() * transform_ : transform_;
}
#ifndef _FTL_REG_CORRESPONDANCES_HPP_
#define _FTL_REG_CORRESPONDANCES_HPP_
#include <ftl/rgbd/source.hpp>
#include <nlohmann/json.hpp>
#include <pcl/common/transforms.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/transformation_estimation_svd_scale.h>
#include <pcl/registration/transformation_validation.h>
#include <pcl/registration/transformation_validation_euclidean.h>
#include <vector>
#include <string>
#include <map>
#include <tuple>
namespace ftl {
namespace registration {
/**
* Manage the identified set of correspondances between two sources. There may
* also be a parent correspondances object to support the chaining of
* transforms.
*/
class Correspondances {
public:
Correspondances(ftl::rgbd::Source *src, ftl::rgbd::Source *targ);
Correspondances(Correspondances *parent, ftl::rgbd::Source *src);
ftl::rgbd::Source *source() { return src_; }
ftl::rgbd::Source *target() { return targ_; }
/**
* Add a new correspondance point. The point will only be added if there is
* a valid depth value at that location.
*
* @param tx X-Coordinate in target image
* @param ty Y-Coordinate in target image
* @param sx X-Coordinate in source image
* @param sy Y-Coordinate in source image
* @return False if point was invalid and not added.
*/
bool add(int tx, int ty, int sx, int sy);
void removeLast();
const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
/**
* Calculate a transform using current set of correspondances.
*
* @return Validation score of the transform.
*/
double estimateTransform();
/**
* Get the estimated transform. This includes any parent transforms to make
* it relative to root camera.
*/
Eigen::Matrix4f transform();
private:
Correspondances *parent_;
ftl::rgbd::Source *targ_;
ftl::rgbd::Source *src_;
pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud_;
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud_;
Eigen::Matrix4f transform_;
bool uptodate_;
std::vector<std::tuple<int,int,int,int>> log_;
};
}
}
#endif // _FTL_REG_CORRESPONDANCES_HPP_
#include <loguru.hpp>
#include <ftl/configuration.hpp>
#include <string>
#include "manual.hpp"
using std::string;
int main(int argc, char **argv) {
auto root = ftl::configure(argc, argv, "registration_default");
// TODO
ftl::registration::manual(root);
if (root->value("mode", string("manual") == "manual")) {
//ftl::registration::manual(root);
}
return 0;
}
#include "manual.hpp"
#include "correspondances.hpp"
#include <loguru.hpp>
#include <ftl/net/universe.hpp>
#include <ftl/rgbd/source.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include <map>
using std::string;
using std::vector;
using std::pair;
using std::map;
using ftl::net::Universe;
using ftl::rgbd::Source;
using cv::Mat;
using cv::Point;
using cv::Scalar;
using namespace ftl::registration;
using MouseAction = std::function<void(int, int, int, int)>;
static void setMouseAction(const std::string& winName, const MouseAction &action)
{
cv::setMouseCallback(winName,
[] (int event, int x, int y, int flags, void* userdata) {
(*(MouseAction*)userdata)(event, x, y, flags);
}, (void*)&action);
}
static MouseAction tmouse;
static MouseAction smouse;
static void to_json(nlohmann::json &json, map<string, Eigen::Matrix4f> &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]); }
json[item.first] = val;
}
}
static bool saveTransformations(const string &path, map<string, Eigen::Matrix4f> &data) {
nlohmann::json data_json;
to_json(data_json, data);
std::ofstream file(path);
if (!file.is_open()) {
LOG(ERROR) << "Error writing transformations to file " << path;
return false;
}
file << std::setw(4) << data_json;
return true;
}
static void build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin) {
Correspondances *last = nullptr;
cs[sources[origin]->getURI()] = nullptr;
for (int i=origin-1; i>=0; i--) {
if (last == nullptr) {
auto *c = new Correspondances(sources[i], sources[origin]);
last = c;
cs[sources[i]->getURI()] = c;
} else {
auto *c = new Correspondances(last, sources[i]);
last = c;
cs[sources[i]->getURI()] = c;
}
}
last = nullptr;
for (int i=origin+1; i<sources.size(); i++) {
if (last == nullptr) {
auto *c = new Correspondances(sources[i], sources[origin]);
last = c;
cs[sources[i]->getURI()] = c;
} else {
auto *c = new Correspondances(last, sources[i]);
last = c;
cs[sources[i]->getURI()] = c;
}
}
}
void ftl::registration::manual(ftl::Configurable *root) {
using namespace cv;
Universe *net = ftl::create<Universe>(root, "net");
net->start();
net->waitConnections();
auto sources = ftl::createArray<Source>(root, "sources", net);
int curtarget = 0;
bool freeze = false;
vector<Mat> rgb;
vector<Mat> depth;
if (sources.size() == 0) return;
rgb.resize(sources.size());
depth.resize(sources.size());
cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
//Correspondances c(sources[targsrc], sources[cursrc]);
map<string, Correspondances*> corrs;
build_correspondances(sources, corrs, root->value("origin", 0));
int lastTX = 0;
int lastTY = 0;
int lastSX = 0;
int lastSY = 0;
tmouse = [&]( int event, int ux, int uy, int) {
if (event == 1) { // click
lastTX = ux;
lastTY = uy;
}
};
setMouseAction("Target", tmouse);
smouse = [&]( int event, int ux, int uy, int) {
if (event == 1) { // click
lastSX = ux;
lastSY = uy;
}
};
setMouseAction("Source", smouse);
bool depthtoggle = false;
double lastScore = 0.0;
Correspondances *current = corrs[sources[curtarget]->getURI()];
while (ftl::running) {
if (!freeze) {
// Grab the latest frames from sources
for (int i=0; i<sources.size(); i++) {
if (sources[i]->isReady()) {
sources[i]->grab();
}
}
}
// Get the raw rgb and depth frames from sources
Mat ttrgb, trgb, tdepth, ttdepth;
Mat tsrgb, srgb, sdepth, tsdepth;
if (current == nullptr) {
srgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
trgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
putText(srgb, string("No correspondance for ") + sources[curtarget]->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
if (!trgb.empty()) imshow("Target", trgb);
if (!srgb.empty()) imshow("Source", srgb);
} else if (current->source()->isReady() && current->target()->isReady()) {
current->source()->getFrames(tsrgb, tsdepth);
current->target()->getFrames(ttrgb, ttdepth);
tsrgb.copyTo(srgb);
ttrgb.copyTo(trgb);
ttdepth.convertTo(tdepth, CV_8U, 255.0f / 10.0f);
applyColorMap(tdepth, tdepth, cv::COLORMAP_JET);
tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
// Apply points and labels...
auto &points = current->screenPoints();
for (auto &p : points) {
auto [tx,ty,sx,sy] = p;
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);
}
// 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);
drawMarker(sdepth, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
drawMarker(tdepth, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
putText(srgb, string("Source: ") + current->source()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
putText(trgb, string("Target: ") + current->target()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
putText(srgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
putText(trgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
if (freeze) putText(srgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
if (freeze) putText(trgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
if (!trgb.empty()) imshow("Target", (depthtoggle) ? tdepth : trgb);
if (!srgb.empty()) imshow("Source", (depthtoggle) ? sdepth : srgb);
}
int key = cv::waitKey(20);
if (key == 27) break;
else if (key >= 48 && key <= 57) {
curtarget = key-48;
if (curtarget >= sources.size()) curtarget = 0;
current = corrs[sources[curtarget]->getURI()];
} else if (key == 'd') depthtoggle = !depthtoggle;
else if (key == 'a') {
if (current->add(lastTX,lastTY,lastSX,lastSY)) {
lastTX = lastTY = lastSX = lastSY = 0;
lastScore = current->estimateTransform();
}
} else if (key == 'u') {
current->removeLast();
lastScore = current->estimateTransform();
} else if (key == 's') {
// Save
map<string, Eigen::Matrix4f> 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();
} else {
transforms[x.first] = x.second->transform();
}
}
saveTransformations(root->value("output", string("./test.json")), transforms);
LOG(INFO) << "Saved!";
}
else if (key == 32) freeze = !freeze;
}
// store transformations in map<string Matrix4f>
// TODO: (later) extract features and correspondencies from complete cloud
// and do ICP to find best possible transformation
// saveTransformations(const string &path, map<string, Matrix4f> &data)
}
#ifndef _FTL_REGISTRATION_MANUAL_HPP_
#define _FTL_REGISTRATION_MANUAL_HPP_
#include <ftl/configurable.hpp>
namespace ftl {
namespace registration {
void manual(ftl::Configurable *root);
}
}
#endif // _FTL_REGISTRATION_MANUAL_HPP_
......@@ -149,8 +149,8 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) {
Eigen::Vector4f Source::point(uint ux, uint uy) {
const auto &params = parameters();
const float x = ((float)ux-(float)params.cx) / (float)params.fx;
const float y = ((float)uy-(float)params.cy) / (float)params.fy;
const float x = ((float)ux+(float)params.cx) / (float)params.fx;
const float y = ((float)uy+(float)params.cy) / (float)params.fy;
SHARED_LOCK(mutex_,lk);
const float depth = depth_.at<float>(uy,ux);
......
......@@ -266,7 +266,11 @@ void Streamer::_schedule() {
// Grab job
pool_.push([this,src](int id) {
//StreamSource *src = sources_[uri];
src->src->grab();
try {
src->src->grab();
} catch (...) {
LOG(ERROR) << "Exception when grabbing frame";
}
// CHECK (Nick) Can state be an atomic instead?
//UNIQUE_LOCK(src->mutex, lk);
......
......@@ -171,11 +171,13 @@
"registration_default": {
"net": {
"peers": ["tcp://ftl-node-2:9001"]
"peers": []
},
"sources": [
{"uri":"ftl://utu.fi/node2#vision_default/source"}
]
{"uri":"file:///home/nick/Pictures/FTL/snap.tar.gz#0", "index": 0},
{"uri":"file:///home/nick/Pictures/FTL/snap.tar.gz#1", "index": 1}
],
"origin": 0
},
"reconstruction_rs": {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment