Skip to content
Snippets Groups Projects
Commit ce345cbe authored by Sebastian Hahta's avatar Sebastian Hahta
Browse files

Merge branch '53-feature/reconstruct' into 'master' 

Resolve "Move registration out of main.cpp (reconstruction)"

Closes #53 and #24

See merge request nicolas.pope/ftl!21
parents 148ee7e5 02bcd120
No related branches found
No related tags found
1 merge request!21Resolve "Move registration out of main.cpp (reconstruction)"
Pipeline #11243 passed
Stage: all
Hide whitespace changes
Inline Side-by-side
applications/reconstruct/include/ftl/registration.hpp
+ 178
− 7
View file @ ce345cbe
......@@ -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
+ 60
− 311
View file @ ce345cbe
......@@ -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
+ 374
− 8
View file @ ce345cbe
......@@ -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
......
config/config.jsonc
+ 15
− 18
View file @ ce345cbe
......@@ -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": {}
},
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment