Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision

Target

Select target project
  • nicolaspope/ftl
1 result
Select Git revision
Show changes
Commits on Source (8)
Hide whitespace changes
Inline Side-by-side
Showing
with 652 additions and 353 deletions
applications/reconstruct/include/ftl/registration.hpp
View file @ 903917f1
......@@ -2,6 +2,7 @@
#define _FTL_RECONSTRUCT_REGISTRATION_HPP_
#include <ftl/config.h>
#include <ftl/configurable.hpp>
#include <ftl/rgbd.hpp>
#include <opencv2/opencv.hpp>
......@@ -13,23 +14,193 @@
namespace ftl {
namespace registration {
/* 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
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);
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);
/** @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.
*/
Eigen::Matrix4f findTransformation( std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds_source,
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds_target);
/* Convert chessboard corners found with OpenCV's findChessboardCorners to PCL point cloud.
*/
/** @brief Convert chessboard corners found with OpenCV's findChessboardCorners to PCL point cloud. */
pcl::PointCloud<pcl::PointXYZ>::Ptr cornersToPointCloud(const std::vector<cv::Point2f> &corners, const cv::Mat &disp, const ftl::rgbd::CameraParameters &p);
/* Find chessboard corners from image.
/** @brief Find chessboard corners from image and store them in PCL PointCloud.
* @note Corners will be drawn in rgb.
*/
bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::CameraParameters &p, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out, float error_threshold);
/**
* @brief Abstract class for registration
*
* Registration procedure
*
* @todo Support for multiple features (possibly necessary for other algorithms).
*/
class Registration : public ftl::Configurable {
public:
Registration(nlohmann::json &config);
void addSource(ftl::rgbd::RGBDSource* source);
size_t getSourcesCount() { return sources_.size(); }
/**
* @brief Run registration loop.
*
* Loop terminates when processData() returns false.
*/
virtual void run();
/**
* @brief Find registration transformations. run() must be called before
* findTransformations(). Transformations are in same order as
* sources were added with addSource().
*
* Transformations are calculated to targetsource if configured. If
* targetsource is not configured or is not found in inputs, which target
* coordinate system is used depends on sub-classes' implementation.
*
* @param Output parameter for transformations.
* @return True if transformations found, otherwise false.
*/
virtual bool findTransformations(std::vector<Eigen::Matrix4f> &data)=0;
/**
* @brief Overload of findTransformations(). Map keys are source URIs.
*/
virtual bool findTransformations(std::map<std::string, Eigen::Matrix4f> &data);
protected:
ftl::rgbd::RGBDSource* getSource(size_t idx);
bool isTargetSourceSet();
bool isTargetSourceFound();
bool isTargetSource(ftl::rgbd::RGBDSource* source);
bool isTargetSource(size_t idx);
/**
* @brief Get index for target source. If target source not defined
* returns 0. If target source is not found, returns 0.
*/
size_t getTargetSourceIdx();
/**
* @brief Resets visibility matrix.
*/
void resetVisibility();
/**
* @brief Check if there is enough data to cover all the cameras;
* that is visibility is a connected graph. Implemented with BFS.
* Implementations of processData() in sub-classes may use this
* method (but it is not required).
*
* @todo Add support for checking visibility for each different features
* found in all images. Also see findFeatures().
*/
bool connectedVisibility();
/**
* @brief Check if there are enough data for all the sources to calculate
* transformations. Method may also implement additional processing
* for the data. Called once after iteration round. run() stops
* when processData() returns false.
*/
virtual bool processData()=0;
/**
* @brief Find features in source, return
*
* Called iteratively n times for every input (n * inputs.size()). Exact
* details depend on implementation of processData(). Implementations may
* choose not to set/use visibility information.
*
* @param Input source
* @param Unique index for source provided by run(). Same source will
* always have same idx. Implementations may choose to ignore it.
* @return True/false if feature was found. Used to build adjacency matrix.
*/
virtual bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx)=0;
std::vector<std::vector<bool>> visibility_; /*< Adjacency matrix for sources (feature visibility). */
private:
std::optional<std::string> target_source_; /*< Reference coordinate system for transformations. */
std::vector<ftl::rgbd::RGBDSource*> sources_;
};
/**
* @brief Registration using chessboard calibration pattern
*
* Parameters from configuration:
*
* patternsize: required
* Chessboard pattern size, inner corners.
*
* maxerror: default +inf
* Maximum allowed error value for pattern detection. MSE error between
* estimated plane and points captured from input.
*
* delay: default 500
* Milliseconds between captured images.
*
* chain: default false
* Enabling allows camera chaining. In chain mode, pattern is not
* required to be visible in every source. In default (chain: false)
* mode, pattern visibility is required for every source.
*
* iter: default 10
* Number of iterations for capturing calibration samples. In
* non-chaining mode, each iteration consists of images where patterns
* were detected on every input. In chaining mode each iteration only
* requires camera visibility to be connected.
*/
bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &disp, const ftl::rgbd::CameraParameters &p, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out, float error_threshold);
class ChessboardRegistration : public Registration {
public:
ChessboardRegistration(nlohmann::json &config);
/**
* @brief Creates new ChessboardRegistration or ChessboardRegistrationChain
* object depending on chain option in config. User of the method
* needs to free the memory.
*/
static ChessboardRegistration* create(nlohmann::json &config);
void run() override;
bool findTransformations(std::vector<Eigen::Matrix4f> &data) override;
protected:
bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx) override;
bool processData() override;
cv::Size pattern_size_;
std::vector<std::vector<std::optional<pcl::PointCloud<pcl::PointXYZ>::Ptr>>> data_;
std::vector<Eigen::Matrix4f> T_;
float error_threshold_;
uint delay_;
uint iter_;
uint iter_remaining_;
};
/**
* @brief Chain registration. Finds visibility and then runs registration.
*/
class ChessboardRegistrationChain : public ChessboardRegistration {
public:
ChessboardRegistrationChain(nlohmann::json &config);
bool findTransformations(std::vector<Eigen::Matrix4f> &data) override;
protected:
bool processData() override;
std::vector<Eigen::Matrix4f> T_;
std::vector<std::vector<std::pair<size_t, size_t>>> edges_;
};
}
}
#endif // HAVE_PCL
#endif // _FTL_RECONSTRUCT_REGISTRATION_HPP_
#endif // _FTL_RECONSTRUCT_REGISTRATION_HPP_
\ No newline at end of file
applications/reconstruct/src/main.cpp
View file @ 903917f1
......@@ -60,168 +60,10 @@ using std::unique_lock;
using std::vector;
#ifdef HAVE_PCL
using pcl::PointCloud;
using pcl::PointXYZ;
using pcl::PointXYZRGB;
#endif
using cv::Mat;
namespace ftl {
namespace rgbd {
PointCloud<PointXYZRGB>::Ptr createPointCloud(RGBDSource *src);
PointCloud<PointXYZRGB>::Ptr createPointCloud(RGBDSource *src, const cv::Point3_<uchar> &col);
}
}
PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src) {
const double CX = src->getParameters().cx;
const double CY = src->getParameters().cy;
const double FX = src->getParameters().fx;
const double FY = src->getParameters().fy;
cv::Mat rgb;
cv::Mat depth;
src->getRGBD(rgb, depth);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
point_cloud_ptr->width = rgb.cols * rgb.rows;
point_cloud_ptr->height = 1;
for(int i=0;i<rgb.rows;i++) {
const float *sptr = depth.ptr<float>(i);
for(int j=0;j<rgb.cols;j++) {
float d = sptr[j]; // * 1000.0f;
pcl::PointXYZRGB point;
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 > 20.0f || point.z < 0.04f) {
point.x = 0.0f; point.y = 0.0f; point.z = 0.0f;
}
cv::Point3_<uchar> prgb = rgb.at<cv::Point3_<uchar>>(i, j);
uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
point.rgb = *reinterpret_cast<float*>(&rgb);
point_cloud_ptr -> points.push_back(point);
}
}
return point_cloud_ptr;
}
PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src, const cv::Point3_<uchar> &prgb) {
const double CX = src->getParameters().cx;
const double CY = src->getParameters().cy;
const double FX = src->getParameters().fx;
const double FY = src->getParameters().fy;
cv::Mat rgb;
cv::Mat depth;
src->getRGBD(rgb, depth);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
point_cloud_ptr->width = rgb.cols * rgb.rows;
point_cloud_ptr->height = 1;
for(int i=0;i<rgb.rows;i++) {
const float *sptr = depth.ptr<float>(i);
for(int j=0;j<rgb.cols;j++) {
float d = sptr[j]; // * 1000.0f;
pcl::PointXYZRGB point;
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 > 20.0f || point.z < 0.04f) {
point.x = 0.0f; point.y = 0.0f; point.z = 0.0f;
}
uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
point.rgb = *reinterpret_cast<float*>(&rgb);
point_cloud_ptr -> points.push_back(point);
}
}
return point_cloud_ptr;
}
#ifdef HAVE_PCL
#include <pcl/filters/uniform_sampling.h>
/*
class InputMerged {
private:
// todo: Abstract input systems, can also use other 3D inputs (like InputMerged)
vector<InputStereo> inputs_;
vector<Eigen::Matrix4f> T_;
public:
PointCloud<PointXYZRGB>::Ptr getPointCloud() {
PointCloud<PointXYZRGB>::Ptr result(new PointCloud<PointXYZRGB>);
for (size_t i = 0; i < T_.size(); i++) {
inputs_[i].lock();
PointCloud<PointXYZRGB>::Ptr cloud = inputs_[i].getPointCloud();
// Documentation: Can be used with cloud_in equal to cloud_out
pcl::transformPointCloud(*cloud, *cloud, T_[i]);
*result += *cloud;
}
PointCloud<PointXYZRGB>::Ptr result_sampled(new PointCloud<PointXYZRGB>);
pcl::UniformSampling<PointXYZRGB> uniform_sampling;
uniform_sampling.setInputCloud(result);
uniform_sampling.setRadiusSearch(0.1f); // todo parametrize
uniform_sampling.filter(*result_sampled);
return result_sampled;
}
};
*/
std::map<string, Eigen::Matrix4f> loadRegistration() {
std::map<string, Eigen::Matrix4f> registration;
std::ifstream file(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json");
// Use identity transform if no registration
if (!file.is_open()) {
LOG(ERROR) << "Error loading registration from file!";
return registration;
}
nlohmann::json load;
file >> load;
for (auto it = load.begin(); it != load.end(); ++it) {
Eigen::Matrix4f m;
auto data = m.data();
for(size_t i = 0; i < 16; i++) {;
data[i] = it.value()[i];
}
registration[it.key()] = m;
}
return registration;
}
void saveRegistration(std::map<string, Eigen::Matrix4f> registration) {
nlohmann::json save;
for (auto &item : registration) {
auto val = nlohmann::json::array();
for(size_t j = 0; j < 16; j++) { val.push_back((float) item.second.data()[j]); }
save[item.first] = val;
}
std::ofstream file(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json");
file << save;
}
using ftl::registration::loadTransformations;
using ftl::registration::saveTransformations;
struct Cameras {
RGBDSource *source;
......@@ -229,96 +71,12 @@ struct Cameras {
DepthCameraParams params;
};
template <template<class> class Container>
std::map<string, Eigen::Matrix4f> runRegistration(ftl::Configurable *root, ftl::net::Universe &net, Container<Cameras> &inputs) {
std::map<string, Eigen::Matrix4f> registration;
// NOTE: uses config["registration"]
if (!root->getConfig()["registration"].is_object()) {
LOG(FATAL) << "Configuration missing \"registration\" entry!";
return registration;
}
int iter = (int) root->getConfig()["registration"]["calibration"]["iterations"];
int delay = (int) root->getConfig()["registration"]["calibration"]["delay"];
float max_error = (int) root->getConfig()["registration"]["calibration"]["max_error"];
string ref_uri = (string) root->getConfig()["registration"]["reference-source"];
cv::Size pattern_size( (int) root->getConfig()["registration"]["calibration"]["patternsize"][0],
(int) root->getConfig()["registration"]["calibration"]["patternsize"][1]);
// config["registration"] done
size_t ref_i;
bool found = false;
for (size_t i = 0; i < inputs.size(); i++) {
if (inputs[i].source->getConfig()["uri"] == ref_uri) {
ref_i = i;
found = true;
break;
}
}
if (!found) { LOG(ERROR) << "Reference input not found!"; return registration; }
for (auto &input : inputs) {
cv::namedWindow("Registration: " + (string)input.source->getConfig()["uri"], cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
}
// vector for every input: vector of point clouds of patterns
vector<vector<PointCloud<PointXYZ>::Ptr>> patterns(inputs.size());
while (iter > 0) {
net.broadcast("grab");
bool retval = true; // set to false if pattern not found in one of the sources
vector<PointCloud<PointXYZ>::Ptr> patterns_iter(inputs.size());
for (size_t i = 0; i < inputs.size(); i++) {
RGBDSource *input = inputs[i].source;
Mat rgb, depth;
input->getRGBD(rgb, depth);
if ((rgb.cols == 0) || (depth.cols == 0)) { retval = false; break; }
retval &= ftl::registration::findChessboardCorners(rgb, depth, input->getParameters(), pattern_size, patterns_iter[i], max_error);
cv::imshow("Registration: " + (string)input->getConfig()["uri"], rgb);
}
cv::waitKey(delay);
// every camera found the pattern
if (retval) {
for (size_t i = 0; i < patterns_iter.size(); i++) {
patterns[i].push_back(patterns_iter[i]);
}
iter--;
}
else { LOG(WARNING) << "Pattern not detected on all inputs";}
}
for (auto &input : inputs) { cv::destroyWindow("Registration: " + (string)input.source->getConfig()["uri"]); }
for (size_t i = 0; i < inputs.size(); i++) {
Eigen::Matrix4f T;
if (i == ref_i) {
T.setIdentity();
}
else {
T = ftl::registration::findTransformation(patterns[i], patterns[ref_i]);
}
registration[(string)inputs[i].source->getConfig()["uri"]] = T;
}
saveRegistration(registration);
return registration;
}
#endif
static void run(ftl::Configurable *root) {
Universe *net = ftl::create<Universe>(root, "net");
// Make sure connections are complete
//sleep_for(milliseconds(500));
// sleep_for(milliseconds(500));
// TODO: possible to do more reliably?
net->waitConnections();
std::vector<Cameras> inputs;
......@@ -328,13 +86,12 @@ static void run(ftl::Configurable *root) {
LOG(ERROR) << "No sources configured!";
return;
}
//std::vector<Display> displays;
// TODO Allow for non-net source types
for (auto &src : *sources) {
RGBDSource *in = ftl::rgbd::RGBDSource::create(src, net); //new ftl::rgbd::NetSource(src, &net);
for (auto &src : *sources) {
RGBDSource *in = ftl::rgbd::RGBDSource::create(src, net);
if (!in) {
LOG(ERROR) << "Unrecognised source: " << src;
LOG(ERROR) << "Unrecognized source: " << src;
} else {
auto &cam = inputs.emplace_back();
cam.source = in;
......@@ -348,85 +105,78 @@ static void run(ftl::Configurable *root) {
cam.params.m_sensorDepthWorldMin = in->getParameters().minDepth;
cam.gpu.alloc(cam.params);
//displays.emplace_back(config["display"], src["uri"]);
LOG(INFO) << (string)src["uri"] << " loaded " << cam.params.fx;
}
}
// Displays and Inputs configured
// load point cloud transformations
std::map<string, Eigen::Matrix4f> registration;
if (root->getConfig()["registration"]["calibration"]["run"]) {
registration = runRegistration(root, *net, inputs);
}
else {
LOG(INFO) << "LOAD REG";
registration = loadRegistration();
}
// verify that registration and configuration is valid
// (registration includes every camera)
bool valid_registration = true;
string ref_input = root->getConfig()["registration"]["reference-source"];
// check every camera is included in registration
for (auto &input : inputs) {
string uri = input.source->getConfig()["uri"];
if (registration.find(uri) == registration.end()) {
valid_registration = false;
LOG(ERROR) << "Camera pose for " + uri + " not found in registration!";
LOG(INFO) << (string) src["uri"] << " loaded " << cam.params.fx;
}
}
// TODO move this block in its own method and add automated tests
// for error cases
if (registration.find(ref_input) == registration.end()) {
LOG(WARNING) << "reference input " + ref_input + " not found in registration";
std::optional<json_t> merge = root->get<json_t>("merge");
if (!merge) {
LOG(WARNING) << "Input merging not configured, using only first input in configuration";
inputs = { inputs[0] };
}
// if registration not valid, use reference input or first input
if (!valid_registration) {
vector<Cameras> inputs_;
for (auto &input : inputs) {
if ((string) input.source->getConfig()["uri"] == ref_input) {
inputs_.push_back(input);
break;
if (inputs.size() > 1) {
std::map<std::string, Eigen::Matrix4f> transformations;
if ((*merge)["register"]) {
LOG(INFO) << "Registration requested";
ftl::registration::Registration *reg = ftl::registration::ChessboardRegistration::create(*merge);
for (auto &input : inputs) {
while(!input.source->isReady()) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); }
reg->addSource(input.source);
}
reg->run();
if (reg->findTransformations(transformations)) {
if (!saveTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
LOG(ERROR) << "Error saving new registration";
};
}
else {
LOG(ERROR) << "Registration failed";
}
free(reg);
}
if (inputs_.size() == 0) {
LOG(ERROR) << "Reference input not found in configured inputs, using first input: " + (string) inputs[0].source->getConfig()["uri"];
inputs_.push_back(inputs[0]);
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";
}
}
inputs = inputs_;
inputs[0].source->setPose(Eigen::Matrix4f::Identity());
}
else {
LOG(INFO) << "Registration valid, assigning poses";
vector<Eigen::Matrix4f> T;
for (auto &input : inputs) {
LOG(INFO) << (unsigned long long)input.source;
Eigen::Matrix4f RT = (registration.count(input.source->getConfig()["uri"].get<string>()) > 0) ?
registration[(string)input.source->getConfig()["uri"]] : Eigen::Matrix4f::Identity();
T.push_back(RT);
input.source->setPose(RT);
string uri = input.source->getURI();
auto T = transformations.find(uri);
if (T == transformations.end()) {
LOG(ERROR) << "Camera pose for " + uri + " not found in transformations";
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());
break;
}
input.source->setPose(T->second);
}
}
// Displays and Inputs configured
LOG(INFO) << "Using sources:";
for (auto &input : inputs) { LOG(INFO) << " " + (string) input.source->getConfig()["uri"]; }
for (auto &input : inputs) { LOG(INFO) << " " + (string) input.source->getURI(); }
//vector<PointCloud<PointXYZRGB>::Ptr> clouds(inputs.size());
ftl::rgbd::Display *display = ftl::create<ftl::rgbd::Display>(root, "display");
ftl::rgbd::VirtualSource *virt = ftl::create<ftl::rgbd::VirtualSource>(root, "virtual", net);
ftl::voxhash::SceneRep *scene = ftl::create<ftl::voxhash::SceneRep>(root, "voxelhash");
virt->setScene(scene);
display->setSource(virt);
unsigned char frameCount = 0;
bool paused = false;
......@@ -470,7 +220,6 @@ static void run(ftl::Configurable *root) {
}
frameCount++;
display->update();
}
}
......
applications/reconstruct/src/registration.cpp
View file @ 903917f1
......@@ -26,12 +26,64 @@ namespace ftl {
namespace registration {
using ftl::rgbd::CameraParameters;
using ftl::rgbd::RGBDSource;
using std::string;
using std::vector;
using std::pair;
using std::map;
using std::optional;
using pcl::PointCloud;
using pcl::PointXYZ;
using pcl::PointXYZRGB;
using cv::Mat;
using Eigen::Matrix4f;
void from_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
for (auto it = json.begin(); it != json.end(); ++it) {
Eigen::Matrix4f 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) {
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;
}
}
bool loadTransformations(const string &path, map<string, Matrix4f> &data) {
std::ifstream file(path);
if (!file.is_open()) {
LOG(ERROR) << "Error loading transformations from file " << path;
return false;
}
nlohmann::json json_registration;
file >> json_registration;
from_json(json_registration, data);
return true;
}
bool saveTransformations(const string &path, map<string, 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;
}
// todo template: fitPlane<typename T>(PointCloud<T> cloud_in, PointCloud<T> cloud_out)
//
......@@ -76,11 +128,11 @@ float fitPlaneError(PointCloud<PointXYZ>::Ptr cloud_in, float distance_threshold
score += d * d;
}
return score / cloud_proj.size();
return (score / cloud_proj.size()) * 10000000.0f;
}
//template<typename T = PointXYZ> typename
PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners, const Mat &disp, const CameraParameters &p) {
PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners, const Mat &depth, const CameraParameters &p) {
int corners_len = corners.size();
vector<cv::Vec3f> points(corners_len);
......@@ -102,7 +154,7 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
for (int i = 0; i < corners_len; i++) {
double x = corners[i].x;
double y = corners[i].y;
double d = disp.at<float>((int) y, (int) x); // * 1000.0f; // todo: better estimation
double d = depth.at<float>((int) y, (int) x); // * 1000.0f; // todo: better estimation
//cv::Vec4d homg_pt = Q_ * cv::Vec4d(x, y, d, 1.0);
//cv::Vec3d p = cv::Vec3d(homg_pt.val) / homg_pt[3];
......@@ -112,16 +164,13 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
point.y = (((double)y + CY) / FY) * d; // / 1000.0f;
point.z = d;
// no check since disparities assumed to be good in the calibration pattern
// if (fabs(d-minDisparity) <= FLT_EPSILON)
cloud->push_back(point);
}
return cloud;
}
bool findChessboardCorners(Mat &rgb, const Mat &disp, const CameraParameters &p, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out, float error_threshold) {
bool findChessboardCorners(Mat &rgb, const Mat &depth, const CameraParameters &p, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out, float error_threshold) {
vector<cv::Point2f> corners(pattern_size.width * pattern_size.height);
#if CV_VERSION_MAJOR >= 4
......@@ -133,7 +182,7 @@ bool findChessboardCorners(Mat &rgb, const Mat &disp, const CameraParameters &p,
cv::drawChessboardCorners(rgb, pattern_size, Mat(corners), retval);
if (!retval) { return false; }
auto corners_cloud = cornersToPointCloud(corners, disp, p);
auto corners_cloud = cornersToPointCloud(corners, depth, p);
// simple check that the values make some sense
float error = fitPlaneError(corners_cloud, error_threshold); // should use different parameter?
LOG(INFO) << "MSE against estimated plane: " << error;
......@@ -200,6 +249,323 @@ Eigen::Matrix4f findTransformation(vector<PointCloud<PointXYZ>::Ptr> clouds_sour
return T;
}
Registration::Registration(nlohmann::json &config) :
ftl::Configurable(config) {
target_source_ = get<string>("targetsource");
if (!target_source_) {
LOG(WARNING) << "targetsource not set";
}
}
RGBDSource* Registration::getSource(size_t idx) {
return sources_[idx];
}
bool Registration::isTargetSourceSet() {
return (bool) target_source_;
}
bool Registration::isTargetSourceFound() {
for (RGBDSource* source : sources_ ) {
if (isTargetSource(source)) return true;
}
return false;
}
bool Registration::isTargetSource(RGBDSource *source) {
if (target_source_) { return source->getURI() == *target_source_; }
return false;
}
bool Registration::isTargetSource(size_t idx) {
if (idx >= sources_.size()) return false; // assert
return isTargetSource(sources_[idx]);
}
size_t Registration::getTargetSourceIdx() {
if (!target_source_) return 0;
for (size_t idx = 0; idx < sources_.size(); ++idx) {
if (isTargetSource(sources_[idx])) return idx;
}
return 0;
}
void Registration::addSource(RGBDSource *source) {
// TODO: check that source is not already included
sources_.push_back(source);
}
/**
* @param adjacency matrix
* @param index of starting vertex
* @param (out) edges connecting each level
* @returns true if graph connected (all vertices visited), otherwise false
*
* Breadth First Search
*/
bool isConnected(vector<vector<bool>> matrix, size_t start_idx, vector<vector<pair<size_t, size_t>>> &edges) {
vector<bool> visited(matrix.size(), false);
DCHECK(start_idx < matrix.size());
edges.clear();
vector<size_t> level { start_idx };
visited[start_idx] = true;
size_t visited_count = 1;
while(level.size() != 0) {
vector<size_t> level_prev = level;
level = {};
vector<pair<size_t, size_t>> new_edges;
for (size_t current : level_prev) {
for (size_t i = 0; i < matrix.size(); ++i) {
if (matrix[current][i] && !visited[i]) {
visited[i] = true;
visited_count += 1;
level.push_back(i);
// could also save each level's vertices
new_edges.push_back(pair(current, i));
}
}}
if (new_edges.size() > 0) edges.push_back(new_edges);
}
return visited_count == matrix.size();
}
bool isConnected(vector<vector<bool>> matrix, size_t start_idx = 0) {
vector<vector<pair<size_t, size_t>>> edges;
return isConnected(matrix, start_idx, edges);
}
/**
* @param Adjacency matrix
* @returns Vector containing degree of each vertex
*/
vector<uint> verticleDegrees(vector<vector<bool>> matrix) {
vector<uint> res(matrix.size(), 0);
for (size_t i = 0; i < matrix.size(); ++i) {
for (size_t j = 0; j < matrix.size(); ++j) {
if (matrix[i][j]) res[i] = res[i] + 1;
}}
return res;
}
bool Registration::connectedVisibility() {
return isConnected(visibility_, getTargetSourceIdx());
}
void Registration::resetVisibility() {
visibility_ = vector(sources_.size(), vector<bool>(sources_.size(), false));
}
void Registration::run() {
resetVisibility();
do {
vector<bool> visible(sources_.size(), false);
for (size_t i = 0; i < sources_.size(); ++i) {
bool retval = findFeatures(sources_[i], i);
visible[i] = retval;
}
for (size_t i = 0; i < visible.size(); ++i) {
for (size_t j = 0; j < visible.size(); ++j) {
bool val = visible[i] && visible[j];
visibility_[i][j] = visibility_[i][j] || val;
visibility_[j][i] = visibility_[j][i] || val;
}}
}
while(processData());
}
bool Registration::findTransformations(map<string, Matrix4f> &data) {
vector<Matrix4f> T;
data.clear();
if (!findTransformations(T)) return false;
for (size_t i = 0; i < sources_.size(); ++i) {
data[sources_[i]->getURI()] = T[i];
}
return true;
}
ChessboardRegistration* ChessboardRegistration::create(nlohmann::json &config) {
if (config.value<bool>("chain", false)) {
return new ChessboardRegistrationChain(config);
}
else {
return new ChessboardRegistration(config);
}
}
ChessboardRegistration::ChessboardRegistration(nlohmann::json &config) :
Registration(config) {
auto patternsize = get<vector<int>>("patternsize");
if (!patternsize) { LOG(FATAL) << "Registration run enabled but pattern size not set"; }
pattern_size_ = cv::Size((*patternsize)[0], (*patternsize)[1]);
auto maxerror = get<float>("maxerror");
if (!maxerror) { LOG(WARNING) << "maxerror not set"; }
auto delay = get<int>("delay");
if (!delay) { LOG(INFO) << "delay not set in configuration"; }
auto iter = get<int>("iterations");
if (!iter) { LOG(INFO) << "iterations not set in configuration"; }
auto chain = get<bool>("chain");
if (!chain) { LOG(INFO) << "input chaining disabled"; }
else { LOG(INFO) << "Input chaining enabled"; }
error_threshold_ = maxerror ? *maxerror : std::numeric_limits<float>::infinity();
iter_ = iter ? *iter : 10;
delay_ = delay ? *delay : 50;
}
void ChessboardRegistration::run() {
if (!isTargetSourceFound()) {
LOG(WARNING) << "targetsource not found in sources";
}
if (data_.size() != getSourcesCount()) {
data_ = vector<vector<optional<PointCloud<PointXYZ>::Ptr>>>(getSourcesCount());
}
iter_remaining_ = iter_;
// TODO: Move GUI elsewhere. Also applies to processData() and findFeatures()
for (size_t i = 0; i < getSourcesCount(); ++i) {
cv::namedWindow("Registration: " + getSource(i)->getURI(),
cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
}
Registration::run();
for (size_t i = 0; i < getSourcesCount(); ++i) {
cv::destroyWindow("Registration: " + getSource(i)->getURI());
}
}
bool ChessboardRegistration::findFeatures(RGBDSource *source, size_t idx) {
optional<PointCloud<PointXYZ>::Ptr> result;
PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
Mat rgb, depth;
source->getRGBD(rgb, depth);
bool retval = findChessboardCorners(rgb, depth, source->getParameters(), pattern_size_, cloud, error_threshold_);
if (retval) {
result.emplace(cloud);
}
data_[idx].push_back(result);
cv::imshow("Registration: " + source->getURI(), rgb);
return retval;
}
bool ChessboardRegistration::processData() {
bool retval = connectedVisibility();
resetVisibility();
if (retval) {
iter_remaining_--;
}
else{
LOG(INFO) << "Pattern not visible in all inputs";
for (auto &sample : data_) { sample.pop_back(); }
}
//std::this_thread::sleep_for(std::chrono::milliseconds(delay_));
cv::waitKey(delay_); // OpenCV GUI doesn't show otherwise
return iter_remaining_ > 0;
}
bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) {
data.clear();
vector<bool> status(getSourcesCount(), false);
size_t idx_target = getTargetSourceIdx();
for (size_t idx = 0; idx < getSourcesCount(); ++idx) {
Matrix4f T;
if (idx == idx_target) {
T.setIdentity();
}
else {
vector<PointCloud<PointXYZ>::Ptr> d;
vector<PointCloud<PointXYZ>::Ptr> d_target;
d.reserve(iter_);
d_target.reserve(iter_);
for (size_t i = 0; i < iter_; ++i) {
auto val = data_[idx][i];
auto val_target = data_[idx_target][i];
if (val && val_target) {
d.push_back(*val);
d_target.push_back(*val_target);
}
}
T = findTransformation(d, d_target);
}
data.push_back(T);
}
return true;
}
ChessboardRegistrationChain::ChessboardRegistrationChain(nlohmann::json &config) :
ChessboardRegistration(config) {
error_threshold_ = std::numeric_limits<float>::infinity();
}
bool ChessboardRegistrationChain::processData() {
for (auto &sample : data_ ) { sample.clear(); }
bool retval = isConnected(visibility_, getTargetSourceIdx(), edges_);
if (retval) {
LOG(INFO) << "Chain complete, depth: " << edges_.size();
return false;
}
else{
LOG(5) << "Chain not complete ";
}
return true;
}
bool ChessboardRegistrationChain::findTransformations(vector<Matrix4f> &data) {
// TODO Change to group registration: register all sources which have visibility
// to the target source in chain.
LOG(INFO) << "Running pairwise registration";
data = vector<Matrix4f>(getSourcesCount(), Matrix4f::Identity());
for (vector<pair<size_t, size_t>> level : edges_) {
for (pair<size_t, size_t> edge : level) {
LOG(INFO) << "Registering source "
<< getSource(edge.second)->getURI() << " to source"
<< getSource(edge.first)->getURI();
nlohmann::json conf(config_);
conf["targetsource"] = getSource(edge.first)->getURI();
conf["chain"] = false;
vector<Matrix4f> result;
ChessboardRegistration reg(conf);
reg.addSource(getSource(edge.first));
reg.addSource(getSource(edge.second));
reg.run();
if (!reg.findTransformations(result)) { return false; }
data[edge.second] = data[edge.first] * result[1];
}
}
return true;
}
} // namespace registration
} // namespace ftl
......
components/common/cpp/src/configuration.cpp
View file @ 903917f1
......@@ -230,6 +230,15 @@ bool ftl::config::update(const std::string &puri, const json_t &value) {
return false;
}
// If there is an ID it still may be a configurable so check!
if (r["$id"].is_string()) {
Configurable *cfg = find(r["$id"].get<string>());
if (cfg) {
cfg->set<json_t>(tail, value);
return true;
}
}
r[tail] = value;
return true;
}
......
components/rgbd-sources/include/ftl/rgbd_source.hpp
View file @ 903917f1
......@@ -82,7 +82,7 @@ class RGBDSource : public ftl::Configurable {
cv::Mat depth_;
private:
Eigen::Matrix4f pose_;
Eigen::Matrix4f pose_ = Eigen::Matrix4f::Identity();
private:
static std::map<std::string,std::function<RGBDSource*(nlohmann::json&,ftl::net::Universe*)>> *sources__;
......
components/rgbd-sources/src/rgbd_streamer.cpp
View file @ 903917f1
......@@ -196,11 +196,15 @@ void Streamer::_schedule() {
pool_.push([this,uri,&jobs,&job_mtx,&job_cv](int id) {
StreamSource *src = sources_[uri];
try {
//auto start = std::chrono::high_resolution_clock::now();
//try {
src->src->grab();
} catch(...) {
LOG(ERROR) << "Grab Exception for: " << uri;
}
//} catch(...) {
// LOG(ERROR) << "Grab Exception for: " << uri;
//}
/*std::chrono::duration<double> elapsed =
std::chrono::high_resolution_clock::now() - start;
LOG(INFO) << "GRAB Elapsed: " << elapsed.count();*/
unique_lock<shared_mutex> lk(src->mutex);
src->state |= ftl::rgbd::detail::kGrabbed;
......@@ -218,6 +222,8 @@ void Streamer::_schedule() {
pool_.push([this,uri,&jobs,&job_mtx,&job_cv](int id) {
StreamSource *src = sources_[uri];
//auto start = std::chrono::high_resolution_clock::now();
if (src->rgb.rows > 0 && src->depth.rows > 0 && src->clients[0].size() > 0) {
vector<unsigned char> rgb_buf;
cv::imencode(".jpg", src->rgb, rgb_buf);
......@@ -247,6 +253,10 @@ void Streamer::_schedule() {
}
}
/*std::chrono::duration<double> elapsed =
std::chrono::high_resolution_clock::now() - start;
LOG(INFO) << "Stream Elapsed: " << elapsed.count();*/
unique_lock<shared_mutex> lk(src->mutex);
DLOG(1) << "Tx Frame: " << uri;
src->state |= ftl::rgbd::detail::kTransmitted;
......
components/rgbd-sources/src/stereovideo_source.cpp
View file @ 903917f1
......@@ -111,15 +111,12 @@ static void disparityToDepth(const cv::Mat &disparity, cv::Mat &depth, const cv:
}
void StereoVideoSource::grab() {
// TODO(Nick) find a way to move this to last part ... but grab can't
// be called twice by different threads and it is currently
// FIXME Call to grab from multiple threads
unique_lock<mutex> lk(mutex_);
calib_->rectified(left_, right_);
cv::Mat disp;
disp_->compute(left_, right_, disp);
unique_lock<mutex> lk(mutex_);
left_.copyTo(rgb_);
disparityToDepth(disp, depth_, calib_->getQ());
}
......
config/config.jsonc
View file @ 903917f1
......@@ -149,15 +149,13 @@
"display": { "$ref": "#displays/left" },
"virtual": { "$ref": "#virtual_cams/default" },
"voxelhash": { "$ref": "#hash_conf/default" },
"registration": {
"reference-source" : "ftl://utu.fi#vision_default/source",
"calibration" : {
"max_error": 25,
"run": false,
"iterations" : 10,
"delay" : 500,
"patternsize" : [9, 6]
}
"merge": {
"register": false,
"targetsource" : "ftl://utu.fi#vision_default/source",
"maxerror": 25,
"iterations" : 10,
"delay" : 500,
"patternsize" : [9, 6]
}
},
......@@ -172,15 +170,14 @@
"display": { "$ref": "#displays/left" },
"virtual": { "$ref": "#virtual_cams/default" },
"voxelhash": { "$ref": "#hash_conf/default" },
"registration": {
"reference-source" : "ftl://utu.fi/node4#vision_default/source",
"calibration" : {
"max_error": 25,
"run": false,
"iterations" : 10,
"delay" : 500,
"patternsize" : [9, 6]
}
"merge": {
"targetsource" : "ftl://utu.fi/node4#vision_default/source",
"register": false,
"chain": false,
"maxerror": 25,
"iterations" : 10,
"delay" : 500,
"patternsize" : [9, 6]
},
"stream": {}
},
......