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
Show whitespace changes
Inline Side-by-side
Showing
with 246 additions and 4163 deletions
applications/calibration-multi/src/multicalibrate.cpp deleted 100644 → 0
View file @ e2e259ab
#include "multicalibrate.hpp"
#include "calibration_data.hpp"
#include "calibration.hpp"
#include <ftl/calibration/optimize.hpp>
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <loguru.hpp>
#include <map>
using cv::Mat;
using cv::Size;
using cv::Point2d;
using cv::Point3d;
using cv::Vec4d;
using cv::Scalar;
using std::string;
using std::vector;
using std::map;
using std::pair;
using std::make_pair;
double CalibrationTarget::estimateScale(vector<Point3d> points) {
// 1. calculate statistics
// 2. reject possible outliers
// 3. calculate scale factor
double f = 0.0;
double S = 0.0;
double m = 0.0;
vector<double> d(points.size() / 2, 0.0);
for (size_t i = 0; i < points.size(); i += 2) {
const Point3d &p1 = points[i];
const Point3d &p2 = points[i + 1];
Point3d p = p1 - p2;
double x = sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
double prev_mean = m;
d[i/2] = x;
f = f + 1.0;
m = m + (x - m) / f;
S = S + (x - m) * (x - prev_mean);
}
double stddev = sqrt(S / f);
f = 0.0;
int outliers = 0;
double scale = 0.0;
for (double l : d) {
// TODO: * Parameterize how large deviation allowed
// * Validate this actually improves quality
if (abs(l - m) > 3.0 * stddev) {
outliers++;
}
else {
f += 1.0;
scale += 1.0 / l;
}
DCHECK(scale != INFINITY);
}
if (outliers != 0) {
LOG(WARNING) << "Outliers (large std. deviation in scale): " << outliers;
}
LOG(INFO) << "calibration target std. dev. " << stddev << " (" << (int) f << " samples), scale: " << scale * calibration_bar_length_ / f;
return scale * calibration_bar_length_ / f;
// TODO: LM-optimization for scale.
}
MultiCameraCalibrationNew::MultiCameraCalibrationNew(
size_t n_cameras, size_t reference_camera, Size resolution,
CalibrationTarget target, int fix_intrinsics) :
target_(target),
visibility_graph_(n_cameras),
is_calibrated_(false),
n_cameras_(n_cameras),
reference_camera_(reference_camera),
min_visible_points_(50),
fix_intrinsics_(fix_intrinsics == 1 ? 5 : 0),
resolution_(resolution),
K_(n_cameras),
dist_coeffs_(n_cameras),
R_(n_cameras),
t_(n_cameras),
points3d_optimized_(n_cameras),
points3d_(n_cameras),
points2d_(n_cameras),
visible_(n_cameras),
fm_method_(cv::FM_8POINT), // RANSAC/LMEDS results need validation (does not work)
fm_ransac_threshold_(0.95),
fm_confidence_(0.90)
{
for (auto &K : K_) { K = Mat::eye(Size(3, 3), CV_64FC1); }
for (auto &d : dist_coeffs_) { d = Mat(Size(5, 1), CV_64FC1, Scalar(0.0)); }
}
Mat MultiCameraCalibrationNew::getCameraMat(size_t idx) {
DCHECK(idx < n_cameras_);
Mat K;
K_[idx].copyTo(K);
return K;
}
Mat MultiCameraCalibrationNew::getCameraMatNormalized(size_t idx, double scale_x, double scale_y)
{
Mat K = getCameraMat(idx);
CHECK((scale_x != 0.0 && scale_y != 0.0) || ((scale_x == 0.0) && scale_y == 0.0));
scale_x = scale_x / (double) resolution_.width;
scale_y = scale_y / (double) resolution_.height;
Mat scale(Size(3, 3), CV_64F, 0.0);
scale.at<double>(0, 0) = scale_x;
scale.at<double>(1, 1) = scale_y;
scale.at<double>(2, 2) = 1.0;
return (scale * K);
}
Mat MultiCameraCalibrationNew::getDistCoeffs(size_t idx) {
DCHECK(idx < n_cameras_);
Mat D;
dist_coeffs_[idx].copyTo(D);
return D;
}
void MultiCameraCalibrationNew::setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs) {
CHECK(idx < n_cameras_);
CHECK(K.size() == Size(3, 3));
CHECK(distCoeffs.total() == 5);
K.convertTo(K_[idx], CV_64FC1);
distCoeffs.convertTo(dist_coeffs_[idx], CV_64FC1);
}
void MultiCameraCalibrationNew::setCameraParameters(size_t idx, const Mat &K) {
DCHECK(idx < n_cameras_);
setCameraParameters(idx, K, dist_coeffs_[idx]);
}
void MultiCameraCalibrationNew::addPoints(vector<vector<Point2d>> points, vector<int> visible) {
DCHECK(points.size() == visible.size());
DCHECK(visible.size() == n_cameras_);
for (size_t i = 0; i < n_cameras_; i++) {
visible_[i].insert(visible_[i].end(), points[i].size(), visible[i]);
points2d_[i].insert(points2d_[i].end(), points[i].begin(), points[i].end());
}
visibility_graph_.update(visible);
}
void MultiCameraCalibrationNew::reset() {
is_calibrated_ = false;
weights_ = vector(n_cameras_, vector(points2d_[0].size(), 0.0));
inlier_ = vector(n_cameras_, vector(points2d_[0].size(), 0));
points3d_ = vector(n_cameras_, vector(points2d_[0].size(), Point3d()));
points3d_optimized_ = vector(points2d_[0].size(), Point3d());
R_ = vector<Mat>(n_cameras_, Mat::eye(Size(3, 3), CV_64FC1));
t_ = vector<Mat>(n_cameras_, Mat(Size(1, 3), CV_64FC1, Scalar(0.0)));
}
void MultiCameraCalibrationNew::saveInput(const string &filename) {
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
saveInput(fs);
fs.release();
}
void MultiCameraCalibrationNew::saveInput(cv::FileStorage &fs) {
fs << "resolution" << resolution_;
fs << "K" << K_;
fs << "D" << dist_coeffs_;
fs << "points2d" << points2d_;
fs << "visible" << visible_;
}
void MultiCameraCalibrationNew::loadInput(const std::string &filename, const vector<size_t> &cameras_in) {
points2d_.clear();
points3d_.clear();
points3d_optimized_.clear();
visible_.clear();
inlier_.clear();
K_.clear();
dist_coeffs_.clear();
cv::FileStorage fs(filename, cv::FileStorage::READ);
vector<Mat> K;
vector<vector<Point2d>> points2d;
vector<vector<int>> visible;
fs["K"] >> K;
fs["D"] >> dist_coeffs_;
fs["points2d"] >> points2d;
fs["visible"] >> visible;
fs["resolution"] >> resolution_;
fs.release();
vector<size_t> cameras;
if (cameras_in.size() == 0) {
cameras.resize(K.size());
size_t i = 0;
for (auto &c : cameras) { c = i++; }
}
else {
cameras.reserve(cameras_in.size());
for (auto &c : cameras_in) { cameras.push_back(c); }
}
n_cameras_ = cameras.size();
points2d_.resize(n_cameras_);
points3d_.resize(n_cameras_);
visible_.resize(n_cameras_);
for (auto const &c : cameras) {
K_.push_back(K[c]);
LOG(INFO) << K[c];
}
for (size_t c = 0; c < n_cameras_; c++) {
points2d_[c].reserve(visible[0].size());
points3d_[c].reserve(visible[0].size());
visible_[c].reserve(visible[0].size());
points3d_optimized_.reserve(visible[0].size());
}
visibility_graph_ = Visibility(n_cameras_);
dist_coeffs_.resize(n_cameras_);
vector<vector<Point2d>> points2d_add(n_cameras_, vector<Point2d>());
vector<int> visible_add(n_cameras_);
for (size_t i = 0; i < visible[0].size(); i += target_.n_points) {
int count = 0;
for (size_t c = 0; c < n_cameras_; c++) {
count += visible[c][i];
points2d_add[c].clear();
points2d_add[c].insert(
points2d_add[c].begin(),
points2d[cameras[c]].begin() + i,
points2d[cameras[c]].begin() + i + target_.n_points);
visible_add[c] = visible[cameras[c]][i];
}
if (count >= 2) {
addPoints(points2d_add, visible_add);
}
}
reset();
DCHECK(points2d_.size() == n_cameras_);
DCHECK(points2d_.size() == visible_.size());
size_t len = visible_[0].size();
for (size_t i = 0; i < n_cameras_; i++) {
DCHECK(visible_[i].size() == len);
DCHECK(points2d_[i].size() == visible_[i].size());
}
}
size_t MultiCameraCalibrationNew::getViewsCount() {
return points2d_[0].size() / target_.n_points;
}
size_t MultiCameraCalibrationNew::getOptimalReferenceCamera() {
return (size_t) visibility_graph_.getOptimalCamera();
}
bool MultiCameraCalibrationNew::isVisible(size_t camera, size_t idx) {
return visible_[camera][idx] == 1;
}
bool MultiCameraCalibrationNew::isValid(size_t camera, size_t idx) {
return inlier_[camera][idx] >= 0;
}
bool MultiCameraCalibrationNew::isValid(size_t idx) {
for (auto camera : inlier_) {
if (camera[idx] > 0) return true;
}
return false;
}
vector<Point2d> MultiCameraCalibrationNew::getPoints(size_t camera, size_t idx) {
return vector<Point2d> (points2d_[camera].begin() + idx * (target_.n_points),
points2d_[camera].begin() + idx * (target_.n_points + 1));
}
void MultiCameraCalibrationNew::updatePoints3D(size_t camera, Point3d new_point,
size_t idx, const Mat &R, const Mat &t) {
int &f = inlier_[camera][idx];
Point3d &point = points3d_[camera][idx];
new_point = transformPoint(new_point, R, t);
if (f == -1) return;
if (f > 0) {
// TODO: remove parameter (10.0 cm - 1.0m); over 0.25m difference
// would most likely suggest very bad triangulation (sync? wrong match?)
// instead store all triangulations and handle outliers
// (perhaps inverse variance weighted mean?)
if (euclideanDistance(point, new_point) > 10.0) {
LOG(ERROR) << "bad value (skipping) " << "(" << point << " vs " << new_point << ")";
f = -1;
}
else {
point = (point * f + new_point) / (double) (f + 1);
f++;
}
}
else {
point = new_point;
f = 1;
}
}
void MultiCameraCalibrationNew::updatePoints3D(size_t camera, vector<Point3d> points,
vector<size_t> idx, const Mat &R, const Mat &t) {
for (size_t i = 0; i < idx.size(); i++) {
updatePoints3D(camera, points[i], idx[i], R, t);
}
}
void MultiCameraCalibrationNew::getVisiblePoints(
vector<size_t> cameras, vector<vector<Point2d>> &points, vector<size_t> &idx) {
size_t n_points_total = points2d_[0].size();
DCHECK(cameras.size() <= n_cameras_);
DCHECK(n_points_total % target_.n_points == 0);
idx.clear();
idx.reserve(n_points_total);
points.clear();
points.resize(cameras.size(), {});
for (size_t i = 0; i < n_points_total; i += target_.n_points) {
bool visible_all = true;
for (auto c : cameras) {
for (size_t j = 0; j < target_.n_points; j++) {
visible_all &= isVisible(c, i + j);
}
}
if (!visible_all) { continue; }
for (size_t j = 0; j < target_.n_points; j++) {
idx.push_back(i + j);
}
for (size_t c = 0; c < cameras.size(); c++) {
points[c].insert(points[c].end(),
points2d_[cameras[c]].begin() + i,
points2d_[cameras[c]].begin() + i + target_.n_points
);
}
}
for (auto p : points) { DCHECK(idx.size() == p.size()); }
}
double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camera_to, Mat &rmat, Mat &tvec) {
vector<size_t> idx;
vector<Point2d> points1, points2;
{
vector<vector<Point2d>> points2d;
getVisiblePoints({camera_from, camera_to}, points2d, idx);
points1 = points2d[0];
points2 = points2d[1];
}
DCHECK(points1.size() % target_.n_points == 0);
DCHECK(points1.size() == points2.size());
// cameras possibly lack line of sight?
DCHECK(points1.size() > 8);
Mat &K1 = K_[camera_from];
Mat &K2 = K_[camera_to];
/*
vector<uchar> inliers;
Mat F, E;
F = cv::findFundamentalMat(points1, points2, fm_method_, fm_ransac_threshold_, fm_confidence_, inliers);
if (F.empty())
{
LOG(ERROR) << "Fundamental matrix estimation failed. Possibly degenerate configuration?";
return INFINITY;
}
E = K2.t() * F * K1;
// Only include inliers
if (fm_method_ == cv::FM_LMEDS || fm_method_ == cv::FM_RANSAC) {
vector<Point2d> inliers1, inliers2;
vector<size_t> inliers_idx;
inliers1.reserve(points1.size());
inliers2.reserve(points1.size());
inliers_idx.reserve(points1.size());
for (size_t i = 0; i < inliers.size(); i += target_.n_points) {
bool inlier = true;
for (size_t j = 0; j < target_.n_points; j++) {
inlier &= inliers[i+j];
}
if (inlier) {
inliers1.insert(inliers1.end(), points1.begin() + i, points1.begin() + i + target_.n_points);
inliers2.insert(inliers2.end(), points2.begin() + i, points2.begin() + i + target_.n_points);
inliers_idx.insert(inliers_idx.end(), idx.begin() + i, idx.begin() + i + target_.n_points);
}
}
LOG(INFO) << "Total points: " << points1.size() << ", inliers: " << inliers1.size();
double ratio_good_points = (double) inliers1.size() / (double) points1.size();
if (ratio_good_points < 0.66) {
// TODO: ...
LOG(WARNING) << "Over 1/3 of points rejected!";
if (ratio_good_points < 0.33) { LOG(FATAL) << "Over 2/3 points rejected!"; }
}
DCHECK(inliers1.size() == inliers_idx.size());
DCHECK(inliers2.size() == inliers_idx.size());
std::swap(inliers1, points1);
std::swap(inliers2, points2);
std::swap(inliers_idx, idx);
}
// Estimate initial rotation matrix and translation vector and triangulate
// points (in camera 1 coordinate system).
Mat R1, R2, t1, t2;
R1 = Mat::eye(Size(3, 3), CV_64FC1);
t1 = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
vector<Point3d> points3d;
// Convert homogeneous coordinates
{
Mat points3dh;
recoverPose(E, points1, points2, K1, K2, R2, t2, 1000.0, points3dh);
points3d.reserve(points3dh.cols);
for (int col = 0; col < points3dh.cols; col++) {
Point3d p = Point3d(points3dh.at<double>(0, col),
points3dh.at<double>(1, col),
points3dh.at<double>(2, col))
/ points3dh.at<double>(3, col);
points3d.push_back(p);
}
}
DCHECK(points3d.size() == points1.size());
// Estimate and apply scale factor
{
double scale = ftl::calibration::optimizeScale(object_points_, points3d);
t1 = t1 * scale;
t2 = t2 * scale;
}
// Reprojection error before BA
{
// SBA should report squared mean error
const double err1 = reprojectionError(points3d, points1, K1, R1, t1);
const double err2 = reprojectionError(points3d, points2, K2, R2, t2);
if (abs(err1 - err2) > 2.0) {
LOG(INFO) << "Initial reprojection error (camera " << camera_from << "): " << err1;
LOG(INFO) << "Initial reprojection error (camera " << camera_to << "): " << err2;
}
LOG(INFO) << "Initial reprojection error (" << camera_from << ", " << camera_to << "): "
<< sqrt(err1 * err1 + err2 * err2);
}
// Bundle Adjustment
double err = INFINITY;
{
auto cameras = vector<ftl::calibration::Camera> {
ftl::calibration::Camera(K1, dist_coeffs_[camera_from], R1, t1),
ftl::calibration::Camera(K2, dist_coeffs_[camera_to], R2, t2)
};
ftl::calibration::BundleAdjustment ba;
ba.addCameras(cameras);
for (size_t i = 0; i < points3d.size(); i++) {
ba.addPoint({points1[i], points2[i]}, points3d[i]);
}
ftl::calibration::BundleAdjustment::Options options;
options.fix_camera_extrinsic = {0};
options.optimize_intrinsic = false;
ba.run(options);
// TODO: ...
err = sqrt(ba.reprojectionError(0) * ba.reprojectionError(1));
R2 = cameras[1].rmat();
t2 = Mat(cameras[1].tvec());
}
calculateTransform(R2, t2, R1, t1, rmat, tvec);
*/
Mat R1, R2, t1, t2;
R1 = Mat::eye(Size(3, 3), CV_64FC1);
t1 = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
vector<Point3d> points3d;
double err = ftl::calibration::computeExtrinsicParameters(K1, dist_coeffs_[camera_from], K2, dist_coeffs_[camera_to], points1, points2, object_points_, R2, t2, points3d);
calculateTransform(R2, t2, R1, t1, rmat, tvec);
// Store and average 3D points for both cameras (skip garbage)
if (err < 10.0) {
Mat rmat1, tvec1;
updatePoints3D(camera_from, points3d, idx, R1, t1);
updatePoints3D(camera_to, points3d, idx, R2, t2);
}
else {
LOG(ERROR) << "Large RMS error ("
<< reprojectionError(points3d, points2, K2, rmat, tvec)
<< "), not updating points!";
}
//LOG(INFO) << reprojectionError(points3d, points1, K1, R1, t1);
//LOG(INFO) << reprojectionError(points3d, points2, K2, R2, t2);
return err;
}
Point3d MultiCameraCalibrationNew::getPoint3D(size_t camera, size_t idx) {
return points3d_[camera][idx];
}
void MultiCameraCalibrationNew::calculateMissingPoints3D() {
points3d_optimized_.clear();
points3d_optimized_.resize(points3d_[reference_camera_].size());
for (size_t i = 0; i < points3d_optimized_.size(); i++) {
if (inlier_[reference_camera_][i] > 0) {
points3d_optimized_[i] = points3d_[reference_camera_][i];
continue;
}
if (!isValid(i)) continue;
double f = 0.0;
Point3d point(0.0, 0.0, 0.0);
for (size_t c = 0; c < n_cameras_; c++) {
if (inlier_[c][i] <= 0) { continue; }
point += transformPoint(getPoint3D(c, i), R_[c], t_[c]);
f += 1.0;
}
DCHECK(f != 0.0);
points3d_optimized_[i] = point / f;
}
}
double MultiCameraCalibrationNew::getReprojectionError(size_t c_from, size_t c_to, const Mat &K, const Mat &R, const Mat &t) {
vector<Point2d> points2d;
vector<Point3d> points3d;
for (size_t i = 0; i < points2d_[c_from].size(); i++) {
if (!isValid(i) || !isVisible(c_from, i) || !isVisible(c_to, i)) continue;
points2d.push_back(points2d_[c_from][i]);
points3d.push_back(points3d_[c_to][i]);
}
return reprojectionError(points3d, points2d, K, R, t);
}
double MultiCameraCalibrationNew::getReprojectionErrorOptimized(size_t c_from, const Mat &K, const Mat &R, const Mat &t) {
vector<Point2d> points2d;
vector<Point3d> points3d;
for (size_t i = 0; i < points2d_[c_from].size(); i++) {
if (!isValid(i) || !isVisible(c_from, i)) continue;
points2d.push_back(points2d_[c_from][i]);
points3d.push_back(points3d_optimized_[i]);
}
return reprojectionError(points3d, points2d, K, R, t);
}
double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
if (reference_camera != -1) {
DCHECK(reference_camera >= 0 && reference_camera < static_cast<int>(n_cameras_));
reference_camera_ = reference_camera;
}
for (const auto &K : K_) {
LOG(INFO) << K;
}
reset(); // remove all old calibration results
map<pair<size_t, size_t>, pair<Mat, Mat>> transformations;
// All cameras should be calibrated pairwise; otherwise all possible 3D
// points are not necessarily triangulated
auto paths = visibility_graph_.findShortestPaths(reference_camera_);
for (size_t c1 = 0; c1 < n_cameras_; c1++) {
for (size_t c2 = c1; c2 < n_cameras_; c2++) {
if (c1 == c2) {
transformations[make_pair(c1, c2)] =
make_pair(Mat::eye(Size(3, 3), CV_64FC1),
Mat(Size(1, 3), CV_64FC1, Scalar(0.0))
);
continue;
}
size_t n_visible = getVisiblePointsCount({c1, c2});
if (n_visible < min_visible_points_) {
LOG(INFO) << "Not enough (" << min_visible_points_ << ") points between "
<< "cameras " << c1 << " and " << c2 << " (" << n_visible << " points), "
<< "skipping";
continue;
}
LOG(INFO) << "Running pairwise calibration for cameras "
<< c1 << " and " << c2 << "(" << n_visible << " points)";
if (transformations.find(make_pair(c2, c1)) != transformations.end()) {
continue;
}
Mat R, t, R_i, t_i;
// TODO: threshold parameter, 16.0 possibly too high
if (calibratePair(c1, c2, R, t) > 16.0) {
LOG(ERROR) << "Pairwise calibration failed, skipping cameras "
<< c1 << " and " << c2;
visibility_graph_.deleteEdge(c1, c2);
continue;
}
calculateInverse(R, t, R_i, t_i);
transformations[make_pair(c2, c1)] = make_pair(R, t);
transformations[make_pair(c1, c2)] = make_pair(R_i, t_i);
}}
for (size_t c = 0; c < paths.size(); c++) {
Mat R_chain = Mat::eye(Size(3, 3), CV_64FC1);
Mat t_chain = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
LOG(INFO) << "Chain for camera " << c;
for (auto e: paths[c]) {
CHECK(transformations.find(e) != transformations.end()) << "chain not calculated; pairwise calibration possibly failed earlier?";
LOG(INFO) << e.first << " -> " << e.second;
Mat R = transformations[e].first;
Mat t = transformations[e].second;
R_chain = R * R_chain;
t_chain = t + R * t_chain;
}
R_[c] = R_chain;
t_[c] = t_chain;
/*R_[c] = transformations[make_pair(reference_camera_, c)].first;
t_[c] = transformations[make_pair(reference_camera_, c)].second;
DCHECK(R_[c].size() == Size(3, 3));
DCHECK(t_[c].size() == Size(1, 3));*/
}
calculateMissingPoints3D();
for (size_t c_from = 0; c_from < n_cameras_; c_from++) {
if (c_from == reference_camera_) continue;
Mat R, t;
calculateInverse(R_[c_from], t_[c_from], R, t);
LOG(INFO) << "Error before BA, cameras " << reference_camera_ << " and " << c_from << ": "
<< getReprojectionErrorOptimized(c_from, K_[c_from], R, t);
}
double err;
{
auto cameras = vector<ftl::calibration::Camera>();
for (size_t i = 0; i < n_cameras_; i++) {
calculateInverse(R_[i], t_[i], R_[i], t_[i]);
cameras.push_back(ftl::calibration::Camera(K_[i], dist_coeffs_[i], R_[i], t_[i]));
}
ftl::calibration::BundleAdjustment ba;
ba.addCameras(cameras);
for (size_t i = 0; i < points3d_optimized_.size(); i++) {
auto &p = points3d_optimized_[i];
DCHECK(!isnanl(p.x) && !isnanl(p.y) && !isnanl(p.z));
int count = 0;
for (size_t c = 0; c < n_cameras_; c++) {
if (isVisible(c, i) && isValid(c, i)) { count++; }
}
if (count < 2) continue;
vector<bool> visible(n_cameras_);
vector<Point2d> points2d(n_cameras_);
for (size_t c = 0; c < n_cameras_; c++) {
bool good = isVisible(c, i) && isValid(c, i);
visible[c] = good;
points2d[c] = points2d_[c][i];
}
ba.addPoint(visible, points2d, p);
}
ba.addObject(object_points_);
ftl::calibration::BundleAdjustment::Options options;
options.loss = ftl::calibration::BundleAdjustment::Options::Loss::CAUCHY;
options.optimize_intrinsic = !fix_intrinsics_;
options.fix_distortion = true;
options.max_iter = 50;
options.fix_camera_extrinsic = {reference_camera};
options.verbose = true;
options.max_iter = 500;
err = ba.reprojectionError();
ba.run(options);
for (size_t i = 0; i < n_cameras_; i++) {
R_[i] = cameras[i].rmat();
t_[i] = cameras[i].tvec();
K_[i] = cameras[i].intrinsicMatrix();
//dist_coeffs_[i] = D; // not updated
calculateInverse(R_[i], t_[i], R_[i], t_[i]);
}
}
for (size_t c_from = 0; c_from < n_cameras_; c_from++) {
if (c_from == reference_camera_) continue;
Mat R, t;
calculateInverse(R_[c_from], t_[c_from], R, t);
LOG(INFO) << "Error (RMS) after BA, cameras " << reference_camera_ << " and " << c_from << ": "
<< getReprojectionErrorOptimized(c_from, K_[c_from], R, t);
}
is_calibrated_ = true;
return err;
}
void MultiCameraCalibrationNew::projectPointsOriginal(size_t camera_src, size_t camera_dst, size_t idx, vector<Point2d> &points) {
}
void MultiCameraCalibrationNew::projectPointsOptimized(size_t camera_dst, size_t idx, vector<Point2d> &points) {
// TODO: indexing does not match input (points may be skipped in loadInput())
points.clear();
size_t i = target_.n_points * idx;
if (!isValid(i)) return;
Point3d p1(points3d_optimized_[i]);
Point3d p2(points3d_optimized_[i + 1]);
if (!std::isfinite(p1.x) || !std::isfinite(p2.x)) {
// DEBUG: should not happen
LOG(ERROR) << "Bad point! (no valid triangulation)";
return;
}
Mat R, tvec, rvec;
calculateTransform(R_[reference_camera_], t_[reference_camera_], R_[camera_dst], t_[camera_dst], R, tvec);
cv::Rodrigues(R, rvec);
cv::projectPoints( vector<Point3d> { p1, p2 },
rvec, tvec, K_[camera_dst], dist_coeffs_[camera_dst], points);
}
void MultiCameraCalibrationNew::getCalibration(vector<Mat> &R, vector<Mat> &t) {
DCHECK(is_calibrated_);
R.resize(n_cameras_);
t.resize(n_cameras_);
for (size_t i = 0; i < n_cameras_; i++) {
R_[i].copyTo(R[i]);
t_[i].copyTo(t[i]);
}
}
\ No newline at end of file
applications/calibration-multi/src/multicalibrate.hpp deleted 100644 → 0
View file @ e2e259ab
#pragma once
#include <opencv2/core.hpp>
#include "visibility.hpp"
#include "util.hpp"
using cv::Mat;
using cv::Size;
using cv::Point2d;
using cv::Point3d;
using cv::Vec4d;
using cv::Scalar;
using std::vector;
using std::pair;
class CalibrationTarget {
public:
explicit CalibrationTarget(double length):
n_points(2),
calibration_bar_length_(length)
{}
/* @brief Estimate scale factor.
* @param 3D points (can pass n views)
*/
double estimateScale(vector<Point3d> points3d);
size_t n_points;
private:
double calibration_bar_length_;
};
class MultiCameraCalibrationNew {
public:
MultiCameraCalibrationNew( size_t n_cameras, size_t reference_camera,
Size resolution, CalibrationTarget target,
int fix_intrinsics=1);
void setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs);
void setCameraParameters(size_t idx, const Mat &K);
void addPoints(vector<vector<Point2d>> points2d, vector<int> visibility);
size_t getViewsCount();
size_t getCamerasCount() { return n_cameras_; }
size_t getOptimalReferenceCamera();
size_t getMinVisibility() { return visibility_graph_.getMinVisibility(); }
size_t getViewsCount(size_t camera) { return visibility_graph_.getViewsCount(camera); }
void setFixIntrinsic(int value) { fix_intrinsics_ = (value == 1 ? 5 : 0); }
void loadInput(const std::string &filename, const vector<size_t> &cameras = {});
void saveInput(cv::FileStorage &fs);
void saveInput(const std::string &filename);
Mat getCameraMat(size_t idx);
Mat getCameraMatNormalized(size_t idx, double scale_x = 1.0, double scale_y = 1.0);
Mat getDistCoeffs(size_t idx);
double calibrateAll(int reference_camera = -1);
double getReprojectionError();
void getCalibration(vector<Mat> &R, vector<Mat> &t);
void projectPointsOriginal(size_t camera_src, size_t camera_dst, size_t idx, vector<Point2d> &points);
void projectPointsOptimized(size_t camera_dst, size_t idx, vector<Point2d> &points);
std::vector<cv::Point3d> object_points_;
protected:
bool isVisible(size_t camera, size_t idx);
bool isValid(size_t camera, size_t idx);
bool isValid(size_t idx);
Point3d getPoint3D(size_t camera, size_t i);
vector<Point2d> getPoints(size_t camera, size_t idx);
vector<vector<Point2d>> getAllPoints(size_t camera, vector<size_t> idx);
void getVisiblePoints( vector<size_t> cameras,
vector<vector<Point2d>> &points,
vector<size_t> &idx);
size_t getVisiblePointsCount(vector<size_t> cameras) {
// TODO: for pairs can use visibility graph adjacency matrix
vector<vector<Point2d>> points2d;
vector<size_t> idx;
getVisiblePoints(cameras, points2d, idx);
return idx.size();
}
size_t getTotalPointsCount() {
return points2d_[0].size();
}
vector<Point3d> getPoints3D(size_t idx);
/* @brief Find points which are visible on all cameras. Returns
* corresponding indices in idx vector.
*/
void getVisiblePoints3D(vector<size_t> cameras,
vector<vector<Point3d>> &points,
vector<size_t> &idx);
/* @brief Update 3D points with new values. If no earlier data, new data
* is used as is, otherwise calculates average.
*/
void updatePoints3D(size_t camera, Point3d new_point, size_t idx, const Mat &R, const Mat &t);
void updatePoints3D(size_t camera, vector<Point3d> new_points, vector<size_t> idx, const Mat &R, const Mat &t);
/* @brief Calculates 3D points that are not visible in reference camera
* from transformations in visible cameras.
*/
void calculateMissingPoints3D();
void getTransformation(size_t camera_from, size_t camera_to, Mat &R, Mat &T);
double calibratePair(size_t camera_from, size_t camera_to, Mat &R, Mat &T);
/* @brief Calculate reprojection error of visible points (triangulation) */
double getReprojectionError(size_t c_from, size_t c_to, const Mat &K, const Mat &R, const Mat &T);
/* @brief Calculate reprojection error of visible points (optimized/averaged points) */
double getReprojectionErrorOptimized(size_t c_from, const Mat &K, const Mat &R, const Mat &T);
/* @brief Remove old calibration data calculated by calibrateAll */
void reset();
private:
CalibrationTarget target_;
Visibility visibility_graph_;
bool is_calibrated_;
size_t n_cameras_;
size_t reference_camera_;
size_t min_visible_points_;
int fix_intrinsics_;
Size resolution_;
vector<Mat> K_;
vector<Mat> dist_coeffs_;
vector<Mat> R_;
vector<Mat> t_;
vector<Point3d> points3d_optimized_;
vector<vector<Point3d>> points3d_;
vector<vector<Point2d>> points2d_;
vector<vector<int>> visible_;
vector<vector<int>> inlier_; // "inlier"
vector<vector<double>> weights_;
int fm_method_;
double fm_ransac_threshold_;
double fm_confidence_;
};
applications/calibration-multi/src/util.cpp deleted 100644 → 0
View file @ e2e259ab
#include "util.hpp"
#include <loguru.hpp>
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/aruco.hpp>
using std::vector;
using cv::Mat;
using cv::Point2i;
using cv::Point2d;
using cv::Point3d;
using cv::Size;
using cv::Scalar;
/* @brief Visualize epipolar lines for given points in the other image.
* @param Points in image
* @param Corresponding image where to draw the lines
* @param Fundamental matrix
* @param Line color
* @param Which image (1 or 2), see OpenCV's computeCorrespondEpilines()
*/
void drawEpipolarLines(vector<Point2d> const &points, Mat &img, Mat const &F, Scalar color, int image) {
Mat lines;
cv::computeCorrespondEpilines(points, image, F, lines);
for (int i = 0; i < lines.rows; i++) {
cv::Vec3f l = lines.at<cv::Vec3f>(i);
float a = l[0];
float b = l[1];
float c = l[2];
float x0, y0, x1, y1;
x0 = 0;
y0 = (-c -a * x0) / b;
x1 = img.cols;
y1 = (-c -a * x1) / b;
cv::line(img, cv::Point(x0, y0), cv::Point(x1,y1), color, 1);
}
}
/* @breif Find calibration points. AruCo markers, two per image.
* visible parameter input/ouput
*/
int findCorrespondingPoints(vector<Mat> imgs, vector<vector<Point2d>> &points,
vector<int> &visible) {
using namespace cv;
int count = 0;
visible.resize(imgs.size(), 1);
points.clear();
points.resize(imgs.size(), vector<Point2d>(2, Point2d(0.0, 0.0)));
auto dictionary = aruco::getPredefinedDictionary(aruco::DICT_5X5_50);
vector<vector<Point2f>> corners;
vector<int> ids;
for (size_t i = 0; i < imgs.size(); i++) {
if (visible[i] == 0) continue;
aruco::detectMarkers(imgs[i], dictionary, corners, ids);
if (corners.size() == 2) {
Point2d center0((corners[0][0] + corners[0][1] + corners[0][2] + corners[0][3]) / 4.0);
Point2d center1((corners[1][0] + corners[1][1] + corners[1][2] + corners[1][3]) / 4.0);
if (ids[0] != 0) { std::swap(center0, center1); }
points[i][0] = center0; points[i][1] = center1;
visible[i] = 1;
count++;
}
else {
visible[i] = 0;
}
}
return count;
}
/* @brief Find AruCo marker centers.
* @param (input) image
* @param (output) found centers
* @param (output) marker IDs
*/
void findMarkerCenters(Mat &img, vector<Point2d> &points, vector<int> &ids, int dict) {
using namespace cv;
points.clear();
auto dictionary = aruco::getPredefinedDictionary(dict);
vector<vector<Point2f>> corners;
aruco::detectMarkers(img, dictionary, corners, ids);
for (size_t j = 0; j < corners.size(); j++) {
Point2f center((corners[j][0] + corners[j][1] + corners[j][2] + corners[j][3]) / 4.0);
points.push_back(center);
}
}
/* OpenCV's recoverPose() expects both cameras to have identical intrinsic
* parameters.
*/
int recoverPose(Mat &E, vector<Point2d> &_points1, vector<Point2d> &_points2,
Mat &_cameraMatrix1, Mat &_cameraMatrix2,
Mat &_R, Mat &_t, double distanceThresh,
Mat &triangulatedPoints) {
Mat points1, points2, cameraMatrix1, cameraMatrix2, cameraMatrix;
Mat(_points1.size(), 2, CV_64FC1, _points1.data()).convertTo(points1, CV_64F);
Mat(_points2.size(), 2, CV_64FC1, _points2.data()).convertTo(points2, CV_64F);
_cameraMatrix1.convertTo(cameraMatrix1, CV_64F);
_cameraMatrix2.convertTo(cameraMatrix2, CV_64F);
cameraMatrix = Mat::eye(Size(3, 3), CV_64FC1);
double fx1 = cameraMatrix1.at<double>(0,0);
double fy1 = cameraMatrix1.at<double>(1,1);
double cx1 = cameraMatrix1.at<double>(0,2);
double cy1 = cameraMatrix1.at<double>(1,2);
double fx2 = cameraMatrix2.at<double>(0,0);
double fy2 = cameraMatrix2.at<double>(1,1);
double cx2 = cameraMatrix2.at<double>(0,2);
double cy2 = cameraMatrix2.at<double>(1,2);
points1.col(0) = (points1.col(0) - cx1) / fx1;
points1.col(1) = (points1.col(1) - cy1) / fy1;
points2.col(0) = (points2.col(0) - cx2) / fx2;
points2.col(1) = (points2.col(1) - cy2) / fy2;
// TODO mask
// cameraMatrix = I (for details, see OpenCV's recoverPose() source code)
// modules/calib3d/src/five-point.cpp (461)
//
// https://github.com/opencv/opencv/blob/371bba8f54560b374fbcd47e7e02f015ac4969ad/modules/calib3d/src/five-point.cpp#L461
return cv::recoverPose(E, points1, points2, cameraMatrix, _R, _t, distanceThresh, cv::noArray(), triangulatedPoints);
}
/* @brief Calculate RMS reprojection error
* @param 3D points
* @param Expected 2D points
* @param Camera matrix
* @param Rotation matrix/vector
* @param Translation vector
*/
double reprojectionError( const vector<Point3d> &points3d, const vector<Point2d> &points2d,
const Mat &K, const Mat &rvec, const Mat &tvec) {
DCHECK(points3d.size() == points2d.size());
Mat _rvec;
if (rvec.size() == Size(3, 3)) { cv::Rodrigues(rvec, _rvec); }
else { _rvec = rvec; }
DCHECK(_rvec.size() == Size(1, 3) || _rvec.size() == Size(3, 1));
vector<Point2d> points_reprojected;
cv::projectPoints(points3d, _rvec, tvec, K, cv::noArray(), points_reprojected);
int n_points = points2d.size();
double err = 0.0;
for (int i = 0; i < n_points; i++) {
Point2d a = points2d[i] - points_reprojected[i];
err += a.x * a.x + a.y * a.y;
}
return sqrt(err / n_points);
}
\ No newline at end of file
applications/calibration-multi/src/util.hpp deleted 100644 → 0
View file @ e2e259ab
#pragma once
#include <loguru.hpp>
#include <opencv2/core.hpp>
#include <opencv2/aruco.hpp>
using std::vector;
using cv::Mat;
using cv::Point2i;
using cv::Point2d;
using cv::Point3d;
using cv::Size;
using cv::Scalar;
/* @brief Visualize epipolar lines for given points in the other image.
* @param Points in image
* @param Corresponding image where to draw the lines
* @param Fundamental matrix
* @param Line color
* @param Which image (1 or 2), see OpenCV's computeCorrespondEpilines()
*/
void drawEpipolarLines(vector<Point2d> const &points, Mat &img, Mat const &F, Scalar color, int image=1);
/* @breif Find calibration points. AruCo markers, two per image.
*/
int findCorrespondingPoints(vector<Mat> imgs, vector<vector<Point2d>> &points,
vector<int> &visible);
/* @brief Find AruCo marker centers.
* @param (input) image
* @param (output) found centers
* @param (output) marker IDs
*/
void findMarkerCenters(Mat &img, vector<Point2d> &points, vector<int> &ids, int dict=cv::aruco::DICT_4X4_50);
/* OpenCV's recoverPose() expects both cameras to have identical intrinsic
* parameters.
*
* https://github.com/opencv/opencv/blob/371bba8f54560b374fbcd47e7e02f015ac4969ad/modules/calib3d/src/five-point.cpp#L461
*/
int recoverPose(Mat &E, vector<Point2d> &_points1, vector<Point2d> &_points2,
Mat &_cameraMatrix1, Mat &_cameraMatrix2,
Mat &_R, Mat &_t, double distanceThresh,
Mat &triangulatedPoints);
/* @brief Calculate RMS reprojection error
* @param 3D points
* @param Expected 2D points
* @param Camera matrix
* @param Rotation matrix/vector
* @param Translation vector
*/
double reprojectionError( const vector<Point3d> &points3d, const vector<Point2d> &points2d,
const Mat &K, const Mat &rvec, const Mat &tvec);
inline double euclideanDistance(Point3d a, Point3d b) {
Point3d c = a - b;
return sqrt(c.x*c.x + c.y*c.y + c.z*c.z);
}
inline Point3d transformPoint(Point3d p, Mat R, Mat t) {
DCHECK(R.size() == Size(3, 3));
DCHECK(t.size() == Size(1, 3));
return Point3d(Mat(R * Mat(p) + t));
}
inline Point3d inverseTransformPoint(Point3d p, Mat R, Mat t) {
DCHECK(R.size() == Size(3, 3));
DCHECK(t.size() == Size(1, 3));
return Point3d(Mat(R.t() * (Mat(p) - t)));
}
inline Mat getMat4x4(const Mat &R, const Mat &t) {
DCHECK(R.size() == Size(3, 3));
DCHECK(t.size() == Size(1, 3));
Mat M = Mat::eye(Size(4, 4), CV_64FC1);
R.copyTo(M(cv::Rect(0, 0, 3, 3)));
t.copyTo(M(cv::Rect(3, 0, 1, 3)));
return M;
}
inline void getRT(const Mat RT, Mat &R, Mat &t) {
R = RT(cv::Rect(0, 0, 3, 3));
t = RT(cv::Rect(3, 0, 1, 3));
}
// calculate transforms from (R1, t1) to (R2, t2), where parameters
// (R1, t1) and (R2, t2) map to same (target) coordinate system
inline void calculateTransform(const Mat &R1, const Mat &T1, const Mat &R2, const Mat &T2, Mat &R, Mat &tvec, Mat &M) {
Mat M_src = getMat4x4(R1, T1);
Mat M_dst = getMat4x4(R2, T2);
M = M_dst.inv() * M_src;
R = M(cv::Rect(0, 0, 3, 3));
tvec = M(cv::Rect(3, 0, 1, 3));
}
inline void calculateTransform(const Mat &R1, const Mat &T1, const Mat &R2, const Mat &T2,Mat &R, Mat &tvec) {
Mat M;
calculateTransform(R1, T1, R2, T2, R, tvec, M);
}
inline void calculateInverse(const Mat &R2, const Mat &T2, Mat &R, Mat &T) {
Mat R1 = Mat::eye(Size(3, 3), CV_64FC1);
Mat T1(Size(1, 3), CV_64FC1, Scalar(0.0));
calculateTransform(R1, T1, R2, T2, R, T);
}
\ No newline at end of file
applications/calibration-multi/src/visibility.cpp deleted 100644 → 0
View file @ e2e259ab
#include <numeric>
#include <loguru.hpp>
#include <queue>
#include "visibility.hpp"
using cv::Mat;
using cv::Scalar;
using cv::Size;
using std::vector;
using std::pair;
using std::make_pair;
Visibility::Visibility(int n_cameras) : n_cameras_(n_cameras) {
visibility_ = Mat(Size(n_cameras, n_cameras), CV_32SC1, Scalar(0));
count_ = vector(n_cameras, 0);
}
void Visibility::update(vector<int> &visible) {
DCHECK(visible.size() == (size_t) n_cameras_);
for (int i = 0; i < n_cameras_; i++) {
if (visible[i] == 0) continue;
count_[i]++;
for (int j = 0; j < n_cameras_; j++) {
if (i == j) continue;
if (visible[j] == 1) visibility_.at<int>(i, j)++;
}
}
}
int Visibility::getOptimalCamera() {
// most visible on average
int best_i = 0;
double best_score = -INFINITY;
for (int i = 0; i < visibility_.rows; i++) {
double score = 0.0;
for (int x = 0; x < visibility_.cols; x++) {
score += visibility_.at<int>(i, x);
}
score = score / (double) visibility_.cols;
if (score > best_score) {
best_i = i;
best_score = score;
}
}
return best_i;
}
void Visibility::deleteEdge(int camera1, int camera2)
{
visibility_.at<int>(camera1, camera2) = 0;
visibility_.at<int>(camera2, camera1) = 0;
}
int Visibility::getMinVisibility() {
int min_count = INT_MAX;
for (int i = 0; i < n_cameras_; i++) {
if (count_[i] < min_count) {
min_count = count_[i];
}
}
return min_count;
}
int Visibility::getViewsCount(int camera) {
return count_[camera];
}
vector<vector<pair<int, int>>> Visibility::findShortestPaths(int reference) {
DCHECK(reference < n_cameras_);
vector<vector<pair<int, int>>> res(n_cameras_);
for (int i = 0; i < n_cameras_; i++) {
res[i] = findShortestPath(i, reference);
}
return res;
}
vector<pair<int, int>> Visibility::findShortestPath(int from, int to) {
if (from == to) return vector<pair<int, int>>();
vector<bool> visited(n_cameras_, false);
vector<double> distances(n_cameras_, INFINITY);
vector<int> previous(n_cameras_, -1);
distances[from] = 0.0;
auto cmp = [](pair<int, double> u, pair<int, double> v) { return u.second > v.second; };
std::priority_queue<pair<int, double>, vector<pair<int, double>>, decltype(cmp)> pq(cmp);
pq.push(make_pair(from, distances[from]));
while(!pq.empty()) {
pair<int, double> current = pq.top();
pq.pop();
int current_id = current.first;
double current_distance = distances[current_id];
visited[current_id] = true;
for (int i = 0; i < n_cameras_; i++) {
int count = visibility_.at<int>(current_id, i);
if (count == 0) continue; // not connected
double distance = 1.0 / (double) count;
double new_distance = current_distance + distance;
if (distances[i] > new_distance) {
distances[i] = new_distance;
previous[i] = current_id;
pq.push(make_pair(i, distances[i]));
}
}
}
vector<pair<int, int>> res;
int prev = previous[to];
int current = to;
do {
res.push_back(make_pair(current, prev));
current = prev;
prev = previous[prev];
}
while(prev != -1);
std::reverse(res.begin(), res.end());
return res;
}
vector<int> Visibility::getClosestCameras(int c) {
// initialize original index locations
vector<int> idx(n_cameras_);
iota(idx.begin(), idx.end(), 0);
int* views = visibility_.ptr<int>(c);
// sort indexes based on comparing values in v
sort(idx.begin(), idx.end(),
[views](size_t i1, size_t i2) {return views[i1] < views[i2];});
return idx;
}
\ No newline at end of file
applications/calibration-multi/src/visibility.hpp deleted 100644 → 0
View file @ e2e259ab
#pragma once
#include <opencv2/core.hpp>
using cv::Mat;
using std::vector;
using std::pair;
class Visibility {
public:
explicit Visibility(int n_cameras);
/**
* @breif Update visibility graph.
* @param Which cameras see the feature(s) in this iteration
*/
void update(vector<int> &visible);
/**
* @brief For all cameras, find shortest (optimal) paths to reference
* camera
* @param Id of reference camera
*
* Calculates shortest path in weighted graph using Dijstra's
* algorithm. Weights are inverse of views between cameras (nodes)
*
* @todo Add constant weight for each edge (prefer less edges)
*/
vector<vector<pair<int, int>>> findShortestPaths(int reference);
vector<int> getClosestCameras(int c);
void deleteEdge(int camera1, int camera2);
int getOptimalCamera();
/** @brief Returns the smallest visibility count (any camera)
*/
int getMinVisibility();
/** @brief Returns the visibility camera's value
*/
int getViewsCount(int camera);
protected:
/**
* @brief Find shortest path between nodes
* @param Source node id
* @param Destination node id
*/
vector<pair<int, int>> findShortestPath(int from, int to);
private:
int n_cameras_; // @brief number of cameras
Mat visibility_; // @brief adjacency matrix
vector<int> count_;
};
applications/calibration/CMakeLists.txt deleted 100644 → 0
View file @ e2e259ab
set(CALIBSRC
src/main.cpp
src/lens.cpp
src/stereo.cpp
src/align.cpp
src/common.cpp
)
add_executable(ftl-calibrate ${CALIBSRC})
target_include_directories(ftl-calibrate PRIVATE src)
target_link_libraries(ftl-calibrate ftlcalibration ftlcommon Threads::Threads ${OpenCV_LIBS})
applications/calibration/src/align.cpp deleted 100644 → 0
View file @ e2e259ab
#include "align.hpp"
#include <loguru.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>
using std::map;
using std::string;
using cv::Mat;
using std::vector;
using cv::Point2f;
using cv::Size;
struct Rec4f {
float left;
float right;
float top;
float bottom;
};
bool loadIntrinsicsMap(const std::string &ifile, const cv::Size &imageSize, Mat &map1, Mat &map2, Mat &cameraMatrix, float scale) {
using namespace cv;
FileStorage fs;
// reading intrinsic parameters
fs.open((ifile).c_str(), FileStorage::READ);
if (!fs.isOpened()) {
LOG(WARNING) << "Could not open intrinsics file : " << ifile;
return false;
}
LOG(INFO) << "Intrinsics from: " << ifile;
Mat D1;
fs["M"] >> cameraMatrix;
fs["D"] >> D1;
//cameraMatrix *= scale;
initUndistortRectifyMap(cameraMatrix, D1, Mat::eye(3,3, CV_64F), cameraMatrix, imageSize, CV_16SC2,
map1, map2);
return true;
}
inline bool hasOption(const map<string, string> &options, const std::string &opt) {
return options.find(opt) != options.end();
}
inline std::string getOption(map<string, string> &options, const std::string &opt) {
auto str = options[opt];
return str.substr(1,str.size()-2);
}
static const float kPI = 3.14159f;
/*static float calculateZRotation(const vector<Point2f> &points, Size &boardSize) {
Point2f tl = points[boardSize.width * (boardSize.height / 2)];
Point2f tr = points[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
float dx = tr.x - tl.x;
float dy = tr.y - tl.y;
float angle = atan2(dy, dx) * (180.0f / kPI);
return angle;
}
static Point2f parallaxDistortion(const vector<Point2f> &points, Size &boardSize) {
Point2f tl = points[0];
Point2f tr = points[boardSize.width-1];
Point2f bl = points[(boardSize.height-1)*boardSize.width];
Point2f br = points[points.size()-1];
float dx1 = tr.x - tl.x;
float dx2 = br.x - bl.x;
float ddx = dx1 - dx2;
float dy1 = bl.y - tl.y;
float dy2 = br.y - tr.y;
float ddy = dy1 - dy2;
return Point2f(ddx, ddy);
}*/
static float distanceTop(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
Point2f tl = points[0];
Point2f tr = points[boardSize.width-1];
float pixSize = tr.x - tl.x;
float mmSize = boardSize.width * squareSize;
float focal = camMatrix.at<double>(0,0);
return ((mmSize / pixSize) * focal) / 1000.0f;
}
static float distanceBottom(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
Point2f bl = points[(boardSize.height-1)*boardSize.width];
Point2f br = points[points.size()-1];
float pixSize = br.x - bl.x;
float mmSize = boardSize.width * squareSize;
float focal = camMatrix.at<double>(0,0);
return ((mmSize / pixSize) * focal) / 1000.0f;
}
static float distanceLeft(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
Point2f bl = points[(boardSize.height-1)*boardSize.width];
Point2f tl = points[0];
float pixSize = bl.y - tl.y;
float mmSize = boardSize.height * squareSize;
float focal = camMatrix.at<double>(0,0);
return ((mmSize / pixSize) * focal) / 1000.0f;
}
static float distanceRight(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
Point2f tr = points[boardSize.width-1];
Point2f br = points[points.size()-1];
float pixSize = br.y - tr.y;
float mmSize = boardSize.height * squareSize;
float focal = camMatrix.at<double>(0,0);
return ((mmSize / pixSize) * focal) / 1000.0f;
}
static Rec4f distances(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
return {
-distanceLeft(camMatrix, points, boardSize, squareSize),
-distanceRight(camMatrix, points, boardSize, squareSize),
-distanceTop(camMatrix, points, boardSize, squareSize),
-distanceBottom(camMatrix, points, boardSize, squareSize)
};
}
/*static float distance(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
Point2f tl = points[boardSize.width * (boardSize.height / 2)];
Point2f tr = points[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
float pixSize = tr.x - tl.x;
float mmSize = boardSize.width * squareSize;
float focal = camMatrix.at<double>(0,0);
return ((mmSize / pixSize) * focal) / 1000.0f;
}
static Point2f diffY(const vector<Point2f> &pointsA, const vector<Point2f> &pointsB, Size &boardSize) {
Point2f tlA = pointsA[boardSize.width * (boardSize.height / 2)];
Point2f trA = pointsA[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
Point2f tlB = pointsB[boardSize.width * (boardSize.height / 2)];
Point2f trB = pointsB[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
float d1 = tlA.y - tlB.y;
float d2 = trA.y - trB.y;
return Point2f(d1,d2);
}*/
static const float kDistanceThreshold = 0.005f;
static void showAnaglyph(const Mat &frame_l, const Mat &frame_r, Mat &img3d) {
using namespace cv;
float data[] = {0.299f, 0.587f, 0.114f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.299, 0.587, 0.114};
Mat m(2, 9, CV_32FC1, data);
//Mat img3d;
img3d = Mat(frame_l.size(), CV_8UC3);
for (int y=0; y<img3d.rows; y++) {
unsigned char *row3d = img3d.ptr(y);
const unsigned char *rowL = frame_l.ptr(y);
const unsigned char *rowR = frame_r.ptr(y);
for (int x=0; x<img3d.cols*3; x+=3) {
const uchar lb = rowL[x+0];
const uchar lg = rowL[x+1];
const uchar lr = rowL[x+2];
const uchar rb = rowR[x+0];
const uchar rg = rowR[x+1];
const uchar rr = rowR[x+2];
row3d[x+0] = lb*m.at<float>(0,6) + lg*m.at<float>(0,7) + lr*m.at<float>(0,8) + rb*m.at<float>(1,6) + rg*m.at<float>(1,7) + rr*m.at<float>(1,8);
row3d[x+1] = lb*m.at<float>(0,3) + lg*m.at<float>(0,4) + lr*m.at<float>(0,5) + rb*m.at<float>(1,3) + rg*m.at<float>(1,4) + rr*m.at<float>(1,5);
row3d[x+2] = lb*m.at<float>(0,0) + lg*m.at<float>(0,1) + lr*m.at<float>(0,2) + rb*m.at<float>(1,0) + rg*m.at<float>(1,1) + rr*m.at<float>(1,2);
}
}
//imshow("Anaglyph", img3d);
//return img3d;
}
void ftl::calibration::align(map<string, string> &opt) {
using namespace cv;
float squareSize = 36.0f;
VideoCapture camA(0);
VideoCapture camB(1);
if (!camA.isOpened() || !camB.isOpened()) {
LOG(ERROR) << "Could not open a camera device";
return;
}
camA.set(cv::CAP_PROP_FRAME_WIDTH, 1280); // TODO Use settings
camA.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
camB.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
camB.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
Mat map1, map2, cameraMatrix;
Size imgSize(1280,720);
loadIntrinsicsMap((hasOption(opt, "profile")) ? getOption(opt,"profile") : "./panasonic.yml", imgSize, map1, map2, cameraMatrix, 1.0f);
Size boardSize(9,6);
#if CV_VERSION_MAJOR >= 4
int chessBoardFlags = CALIB_CB_NORMALIZE_IMAGE;
#else
int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
if (!settings.useFisheye) {
// fast check erroneously fails with high distortions like fisheye
chessBoardFlags |= CALIB_CB_FAST_CHECK;
}
#endif
bool anaglyph = true;
while (true) {
Mat frameA, fA;
Mat frameB, fB;
camA.grab();
camB.grab();
camA.retrieve(frameA);
camB.retrieve(frameB);
remap(frameA, fA, map1, map2, INTER_LINEAR);
remap(frameB, fB, map1, map2, INTER_LINEAR);
// Get the chessboard
vector<Point2f> pointBufA;
vector<Point2f> pointBufB;
bool foundA, foundB;
foundA = findChessboardCornersSB(fA, boardSize,
pointBufA, chessBoardFlags);
foundB = findChessboardCornersSB(fB, boardSize,
pointBufB, chessBoardFlags);
// Step 1: Position cameras correctly with respect to chessboard
// - print distance estimate etc
// Step 2: Show individual camera tilt degrees with left / right indicators
// Show also up down tilt perspective error
// Step 3: Display current baseline in mm
if (foundA) {
// Draw the corners.
//drawChessboardCorners(fA, boardSize,
// Mat(pointBufA), foundA);
}
if (foundB) {
// Draw the corners.
//drawChessboardCorners(fB, boardSize,
// Mat(pointBufB), foundB);
}
Mat anag;
showAnaglyph(fA, fB, anag);
if (foundA) {
Rec4f dists = distances(cameraMatrix, pointBufA, boardSize, squareSize);
//Rec4f angs = angles(pointBufA, boardSize);
// TODO Check angles also...
bool lrValid = std::abs(dists.left-dists.right) <= kDistanceThreshold;
bool tbValid = std::abs(dists.top-dists.bottom) <= kDistanceThreshold;
bool tiltUp = dists.top < dists.bottom && !tbValid;
bool tiltDown = dists.top > dists.bottom && !tbValid;
bool rotLeft = dists.left > dists.right && !lrValid;
bool rotRight = dists.left < dists.right && !lrValid;
// TODO Draw lines
Point2f bl = pointBufA[(boardSize.height-1)*boardSize.width];
Point2f tl = pointBufA[0];
Point2f tr = pointBufA[boardSize.width-1];
Point2f br = pointBufA[pointBufA.size()-1];
line(anag, tl, tr, (!lrValid && tiltUp) ? Scalar(0,0,255) : Scalar(0,255,0));
line(anag, bl, br, (!lrValid && tiltDown) ? Scalar(0,0,255) : Scalar(0,255,0));
line(anag, tl, bl, (!tbValid && rotLeft) ? Scalar(0,0,255) : Scalar(0,255,0));
line(anag, tr, br, (!tbValid && rotRight) ? Scalar(0,0,255) : Scalar(0,255,0));
//fA = fA(Rect(tl.x - 100, tl.y- 100, br.x-tl.x + 200, br.y-tl.y + 200));
// Show distance error between cameras
// Show estimated baseline
//if (step == 0) {
// Point2f pd = parallaxDistortion(pointBufA, boardSize);
// putText(fA, string("Distort: ") + std::to_string(pd.x) + string(",") + std::to_string(pd.y), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
//} else if (step == 1) {
//float d = distance(cameraMatrix, pointBufA, boardSize, squareSize);
//putText(anag, string("Distance: ") + std::to_string(-d) + string("m"), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
// } else if (step == 2) {
//float angle = calculateZRotation(pointBufA, boardSize) - 180.0f;
//putText(anag, string("Angle: ") + std::to_string(angle), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
//} else if (step == 3) {
// Point2f vd = diffY(pointBufA, pointBufB, boardSize);
// putText(fA, string("Vertical: ") + std::to_string(vd.x) + string(",") + std::to_string(vd.y), Point(10,200), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
// }
if (foundB) {
//if (step == 0) {
//Point2f pd = parallaxDistortion(pointBufB, boardSize);
//putText(fB, string("Distort: ") + std::to_string(pd.x) + string(",") + std::to_string(pd.y), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
//} else if (step == 1) {
//float d = distance(cameraMatrix, pointBufB, boardSize, squareSize);
//putText(fB, string("Distance: ") + std::to_string(-d) + string("m"), Point(10,100), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
//} else if (step == 2) {
//float angle = calculateZRotation(pointBufB, boardSize) - 180.0f;
//putText(fB, string("Angle: ") + std::to_string(angle), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
//}
Rec4f dists = distances(cameraMatrix, pointBufB, boardSize, squareSize);
//Rec4f angs = angles(pointBufA, boardSize);
// TODO Check angles also...
bool lrValid = std::abs(dists.left-dists.right) <= kDistanceThreshold;
bool tbValid = std::abs(dists.top-dists.bottom) <= kDistanceThreshold;
bool tiltUp = dists.top < dists.bottom && !tbValid;
bool tiltDown = dists.top > dists.bottom && !tbValid;
bool rotLeft = dists.left > dists.right && !lrValid;
bool rotRight = dists.left < dists.right && !lrValid;
// TODO Draw lines
Point2f bbl = pointBufB[(boardSize.height-1)*boardSize.width];
Point2f btl = pointBufB[0];
Point2f btr = pointBufB[boardSize.width-1];
Point2f bbr = pointBufB[pointBufB.size()-1];
line(anag, btl, btr, (!lrValid && tiltUp) ? Scalar(0,0,255) : Scalar(0,255,0));
line(anag, bbl, bbr, (!lrValid && tiltDown) ? Scalar(0,0,255) : Scalar(0,255,0));
line(anag, btl, bbl, (!tbValid && rotLeft) ? Scalar(0,0,255) : Scalar(0,255,0));
line(anag, btr, bbr, (!tbValid && rotRight) ? Scalar(0,0,255) : Scalar(0,255,0));
float baseline1 = std::abs(tl.x - btl.x);
float baseline2 = std::abs(tr.x - btr.x);
float baseline3 = std::abs(bl.x - bbl.x);
float baseline4 = std::abs(br.x - bbr.x);
float boardWidth = (std::abs(tl.x-tr.x) + std::abs(btl.x - btr.x)) / 2.0f;
float baseline = ((baseline1 + baseline2 + baseline3 + baseline4) / 4.0f) / boardWidth * (boardSize.width*squareSize);
putText(anag, string("Baseline: ") + std::to_string(baseline) + string("mm"), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,255,0), 2);
}
}
/*if (anaglyph) {
showAnaglyph(fA,fB);
} else {
imshow("Left", fA);
imshow("Right", fB);
}*/
imshow("Anaglyph", anag);
char key = static_cast<char>(waitKey(20));
if (key == 27)
break;
if (key == 32) anaglyph = !anaglyph;
}
}
\ No newline at end of file
applications/calibration/src/align.hpp deleted 100644 → 0
View file @ e2e259ab
#ifndef _FTL_CALIBRATION_ALIGN_HPP_
#define _FTL_CALIBRATION_ALIGN_HPP_
#include <map>
#include <string>
namespace ftl {
namespace calibration {
void align(std::map<std::string, std::string> &opt);
}
}
#endif // _FTL_CALIBRATION_ALIGN_HPP_
applications/calibration/src/common.cpp deleted 100644 → 0
View file @ e2e259ab
#include <loguru.hpp>
#include <ftl/config.h>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include "common.hpp"
using std::vector;
using std::map;
using std::string;
using cv::Mat;
using cv::Vec2f, cv::Vec3f;
using cv::Size;
using cv::stereoCalibrate;
namespace ftl {
namespace calibration {
// Options
string getOption(const map<string, string> &options, const string &opt) {
const string str = options.at(opt);
return str.substr(1, str.size() - 2);
}
bool hasOption(const map<string, string> &options, const string &opt) {
return options.find(opt) != options.end();
}
int getOptionInt(const map<string, string> &options, const string &opt, int default_value) {
if (!hasOption(options, opt)) return default_value;
return std::stoi(options.at(opt));
}
double getOptionDouble(const map<string, string> &options, const string &opt, double default_value) {
if (!hasOption(options, opt)) return default_value;
return std::stod(options.at(opt));
}
string getOptionString(const map<string, string> &options, const string &opt, string default_value) {
if (!hasOption(options, opt)) return default_value;
return getOption(options, opt);
}
// Save/load files
bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &R1, Mat &R2, Mat &P1, Mat &P2, Mat &Q) {
cv::FileStorage fs;
fs.open(ofile, cv::FileStorage::WRITE);
if (fs.isOpened()) {
fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1"
<< P1 << "P2" << P2 << "Q" << Q;
fs.release();
return true;
} else {
LOG(ERROR) << "Error: can not save the extrinsic parameters";
}
return false;
}
bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const vector<Mat>& D, const Size &size)
{
cv::FileStorage fs(ofile, cv::FileStorage::WRITE);
if (fs.isOpened())
{
fs << "resolution" << size;
fs << "K" << M << "D" << D;
fs.release();
return true;
}
else
{
LOG(ERROR) << "Error: can not save the intrinsic parameters to '" << ofile << "'";
}
return false;
}
bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1, Size &size) {
using namespace cv;
FileStorage fs;
// reading intrinsic parameters
fs.open((ifile).c_str(), FileStorage::READ);
if (!fs.isOpened()) {
LOG(WARNING) << "Could not open intrinsics file : " << ifile;
return false;
}
LOG(INFO) << "Intrinsics from: " << ifile;
fs["resolution"] >> size;
fs["K"] >> K1;
fs["D"] >> D1;
return true;
}
Grid::Grid(int rows, int cols, int width, int height,
int offset_x, int offset_y) {
rows_ = rows;
cols_ = cols;
width_ = width;
height_ = height;
offset_x_ = offset_x;
offset_y_ = offset_y;
cell_width_ = width_ / cols_;
cell_height_ = height_ / rows_;
reset();
corners_ = vector<std::pair<cv::Point, cv::Point>>();
for (int r = 0; r < rows_; r++) {
for (int c = 0; c < cols_; c++) {
int x1 = offset_x_ + c * cell_width_;
int y1 = offset_y_ + r * cell_height_;
int x2 = offset_x_ + (c + 1) * cell_width_ - 1;
int y2 = offset_y_ + (r + 1) * cell_height_ - 1;
corners_.push_back(std::pair(cv::Point(x1, y1), cv::Point(x2, y2)));
}}
}
void Grid::drawGrid(Mat &rgb) {
for (int i = 0; i < rows_ * cols_; ++i) {
bool visited = visited_[i];
cv::Scalar color = visited ? cv::Scalar(24, 255, 24) : cv::Scalar(24, 24, 255);
cv::rectangle(rgb, corners_[i].first, corners_[i].second, color, 2);
}
}
int Grid::checkGrid(cv::Point p1, cv::Point p2) {
// TODO calculate directly
for (int i = 0; i < rows_ * cols_; ++i) {
auto &corners = corners_[i];
if (p1.x >= corners.first.x &&
p1.x <= corners.second.x &&
p1.y >= corners.first.y &&
p1.y <= corners.second.y &&
p2.x >= corners.first.x &&
p2.x <= corners.second.x &&
p2.y >= corners.first.y &&
p2.y <= corners.second.y) {
return i;
}
}
return -1;
}
void Grid::updateGrid(int i) {
if (i >= 0 && i < static_cast<int>(visited_.size()) && !visited_[i]) {
visited_[i] = true;
visited_count_ += 1;
}
}
bool Grid::isVisited(int i) {
if (i >= 0 && i < static_cast<int>(visited_.size())) {
return visited_[i];
}
return false;
}
bool Grid::isComplete() {
return visited_count_ == static_cast<int>(visited_.size());
}
void Grid::reset() {
visited_count_ = 0;
visited_ = vector<bool>(rows_ * cols_, false);
// reset visited
}
// Calibration classes for different patterns
CalibrationChessboard::CalibrationChessboard(const map<string, string> &opt) {
pattern_size_ = Size( getOptionInt(opt, "cols", 9),
getOptionInt(opt, "rows", 6));
image_size_ = Size( getOptionInt(opt, "width", 1280),
getOptionInt(opt, "height", 720));
pattern_square_size_ = getOptionDouble(opt, "square_size", 0.036);
LOG(INFO) << "Chessboard calibration parameters";
LOG(INFO) << " rows: " << pattern_size_.height;
LOG(INFO) << " cols: " << pattern_size_.width;
LOG(INFO) << " width: " << image_size_.width;
LOG(INFO) << " height: " << image_size_.height;
LOG(INFO) << " square_size: " << pattern_square_size_;
LOG(INFO) << "-----------------------------------";
// From OpenCV (4.1.0) Documentation
//
// CALIB_CB_NORMALIZE_IMAGE Normalize the image gamma with equalizeHist before detection.
// CALIB_CB_EXHAUSTIVE Run an exhaustive search to improve detection rate.
// CALIB_CB_ACCURACY Up sample input image to improve sub-pixel accuracy due to
// aliasing effects. This should be used if an accurate camera
// calibration is required.
chessboard_flags_ = cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ACCURACY;
}
void CalibrationChessboard::objectPoints(vector<Vec3f> &out) {
out.reserve(pattern_size_.width * pattern_size_.height);
for (int row = 0; row < pattern_size_.height; ++row) {
for (int col = 0; col < pattern_size_.width; ++col) {
out.push_back(Vec3f(col * pattern_square_size_, row * pattern_square_size_, 0));
}}
}
bool CalibrationChessboard::findPoints(Mat &img, vector<Vec2f> &points) {
return cv::findChessboardCornersSB(img, pattern_size_, points, chessboard_flags_);
}
void CalibrationChessboard::drawCorners(Mat &img, const vector<Vec2f> &points) {
using cv::Point2i;
vector<Point2i> corners(4);
corners[1] = Point2i(points[0]);
corners[0] = Point2i(points[pattern_size_.width - 1]);
corners[2] = Point2i(points[pattern_size_.width * (pattern_size_.height - 1)]);
corners[3] = Point2i(points.back());
cv::Scalar color = cv::Scalar(200, 200, 200);
for (int i = 0; i <= 4; i++)
{
cv::line(img, corners[i % 4], corners[(i + 1) % 4], color, 2);
}
}
void CalibrationChessboard::drawPoints(Mat &img, const vector<Vec2f> &points) {
cv::drawChessboardCorners(img, pattern_size_, points, true);
}
}
}
applications/calibration/src/common.hpp deleted 100644 → 0
View file @ e2e259ab
#ifndef _FTL_CALIBRATION_COMMON_HPP_
#define _FTL_CALIBRATION_COMMON_HPP_
#include <map>
#include <string>
#include <opencv2/core.hpp>
namespace ftl {
namespace calibration {
std::string getOption(const std::map<std::string, std::string> &options, const std::string &opt);
bool hasOption(const std::map<std::string, std::string> &options, const std::string &opt);
int getOptionInt(const std::map<std::string, std::string> &options, const std::string &opt, int default_value);
double getOptionDouble(const std::map<std::string, std::string> &options, const std::string &opt, double default_value);
std::string getOptionString(const std::map<std::string, std::string> &options, const std::string &opt, std::string default_value);
bool loadIntrinsics(const std::string &ifile, std::vector<cv::Mat> &K, std::vector<cv::Mat> &D, cv::Size &size);
bool saveIntrinsics(const std::string &ofile, const std::vector<cv::Mat> &K, const std::vector<cv::Mat> &D, const cv::Size &size);
// TODO loadExtrinsics()
bool saveExtrinsics(const std::string &ofile, cv::Mat &R, cv::Mat &T, cv::Mat &R1, cv::Mat &R2, cv::Mat &P1, cv::Mat &P2, cv::Mat &Q);
class Grid {
private:
int rows_;
int cols_;
int width_;
int height_;
int cell_width_;
int cell_height_;
int offset_x_;
int offset_y_;
int visited_count_;
std::vector<std::pair<cv::Point, cv::Point>> corners_;
std::vector<bool> visited_;
public:
Grid(int rows, int cols, int width, int height, int offset_x, int offset_y);
void drawGrid(cv::Mat &rgb);
int checkGrid(cv::Point p1, cv::Point p2);
void updateGrid(int i);
bool isVisited(int i);
bool isComplete();
void reset();
};
/**
* @brief Wrapper for OpenCV's calibration methods. Paramters depend on
* implementation (different types of patterns).
*
* Calibration objects may store state; eg. from previous views of calibration
* images.
*/
class Calibration {
public:
/**
* @brief Calculate reference points for given pattern
* @param Output parameter
*/
void objectPoints(std::vector<cv::Vec3f> &out);
/**
* @brief Try to find calibration pattern in input image
* @param Input image
* @param Output parameter for found point image coordinates
* @returns true if pattern found, otherwise false
*/
bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
/**
* @brief Draw points to image
* @param Image to draw to
* @param Pattern points (in image coordinates)
*/
void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
};
/**
* @brief Chessboard calibration pattern. Uses OpenCV's
* findChessboardCornersSB function.
* @todo Parameters hardcoded in constructor
*
* All parameters (command line parameters):
* - rows, cols: pattern size (inner corners)
* - square_size: millimeters (TODO: meters)
* - width, height: image size, pixels
* - flags: see ChessboardCornersSB documentation (TODO: not implemented)
*/
class CalibrationChessboard : Calibration {
public:
explicit CalibrationChessboard(const std::map<std::string, std::string> &opt);
void objectPoints(std::vector<cv::Vec3f> &out);
bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
void drawCorners(cv::Mat &img, const std::vector<cv::Vec2f> &points);
private:
int chessboard_flags_ = 0;
float pattern_square_size_;
cv::Size pattern_size_;
cv::Size image_size_;
};
// TODO other patterns, circles ...
}
}
#endif // _FTL_CALIBRATION_COMMON_HPP_
applications/calibration/src/lens.cpp deleted 100644 → 0
View file @ e2e259ab
#include "common.hpp"
#include "lens.hpp"
#include <ftl/config.h>
#include <ftl/calibration/parameters.hpp>
#include <loguru.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 <vector>
#include <atomic>
#include <thread>
using std::map;
using std::string;
using std::vector;
using cv::Mat;
using cv::Vec2f;
using cv::Vec3f;
using cv::Size;
using namespace ftl::calibration;
void ftl::calibration::intrinsic(map<string, string> &opt) {
LOG(INFO) << "Begin intrinsic calibration";
// TODO PARAMETERS TO CONFIG FILE
const Size image_size = Size( getOptionInt(opt, "width", 1920),
getOptionInt(opt, "height", 1080));
const int n_cameras = getOptionInt(opt, "n_cameras", 2);
const int iter = getOptionInt(opt, "iter", 20);
const int delay = getOptionInt(opt, "delay", 1000);
const double aperture_width = getOptionDouble(opt, "aperture_width", 6.2);
const double aperture_height = getOptionDouble(opt, "aperture_height", 4.6);
const string filename_intrinsics = getOptionString(opt, "profile", FTL_LOCAL_CONFIG_ROOT "/calibration.yml");
CalibrationChessboard calib(opt);
bool use_guess = getOptionInt(opt, "use_guess", 1);
//bool use_guess_distortion = getOptionInt(opt, "use_guess_distortion", 0);
LOG(INFO) << "Intrinsic calibration parameters";
LOG(INFO) << " profile: " << filename_intrinsics;
LOG(INFO) << " n_cameras: " << n_cameras;
LOG(INFO) << " width: " << image_size.width;
LOG(INFO) << " height: " << image_size.height;
LOG(INFO) << " iter: " << iter;
LOG(INFO) << " delay: " << delay;
LOG(INFO) << " aperture_width: " << aperture_width;
LOG(INFO) << " aperture_height: " << aperture_height;
LOG(INFO) << " use_guess: " << use_guess << "\n";
LOG(WARNING) << "WARNING: This application overwrites existing files and does not previous values!";
//LOG(INFO) << " use_guess_distortion: " << use_guess_distortion;
LOG(INFO) << "-----------------------------------";
// assume no tangential and thin prism distortion and only estimate first
// three radial distortion coefficients
int calibrate_flags = cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5 | cv::CALIB_FIX_K6 |
cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_S1_S2_S3_S4 | cv::CALIB_FIX_ASPECT_RATIO;
vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
for (Mat &d : dist_coeffs) {
d = Mat(Size(8, 1), CV_64FC1, cv::Scalar(0.0));
}
if (use_guess) {
camera_matrix.clear();
vector<Mat> tmp;
Size tmp_size;
loadIntrinsics(filename_intrinsics, camera_matrix, tmp, tmp_size);
CHECK(camera_matrix.size() == static_cast<unsigned int>(n_cameras));
if ((tmp_size != image_size) && (!tmp_size.empty())) {
for (Mat &K : camera_matrix) {
K = ftl::calibration::scaleCameraMatrix(K, image_size, tmp_size);
}
}
if (tmp_size.empty()) {
LOG(ERROR) << "No valid calibration found.";
}
else {
calibrate_flags |= cv::CALIB_USE_INTRINSIC_GUESS;
}
}
vector<cv::VideoCapture> cameras;
cameras.reserve(n_cameras);
for (int c = 0; c < n_cameras; c++) { cameras.emplace_back(c); }
for (auto &camera : cameras)
{
if (!camera.isOpened()) {
LOG(ERROR) << "Could not open camera device";
return;
}
camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width);
camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
}
vector<vector<vector<Vec2f>>> image_points(n_cameras);
vector<vector<vector<Vec3f>>> object_points(n_cameras);
vector<Mat> img(n_cameras);
vector<Mat> img_display(n_cameras);
vector<int> count(n_cameras, 0);
Mat display(Size(image_size.width * n_cameras, image_size.height), CV_8UC3);
for (int c = 0; c < n_cameras; c++) {
img_display[c] = Mat(display, cv::Rect(c * image_size.width, 0, image_size.width, image_size.height));
}
std::mutex m;
std::atomic<bool> ready = false;
auto capture = std::thread(
[n_cameras, delay, &m, &ready, &count, &calib, &img, &image_points, &object_points]() {
vector<Mat> tmp(n_cameras);
while(true) {
if (!ready) {
std::this_thread::sleep_for(std::chrono::milliseconds(delay));
continue;
}
m.lock();
ready = false;
for (int c = 0; c < n_cameras; c++) {
img[c].copyTo(tmp[c]);
}
m.unlock();
for (int c = 0; c < n_cameras; c++) {
vector<Vec2f> points;
if (calib.findPoints(tmp[c], points)) {
count[c]++;
}
else { continue; }
vector<Vec3f> points_ref;
calib.objectPoints(points_ref);
Mat camera_matrix, dist_coeffs;
image_points[c].push_back(points);
object_points[c].push_back(points_ref);
}
std::this_thread::sleep_for(std::chrono::milliseconds(delay));
}
});
while (iter > *std::min_element(count.begin(), count.end())) {
if (m.try_lock()) {
for (auto &camera : cameras) { camera.grab(); }
for (int c = 0; c < n_cameras; c++) {
cameras[c].retrieve(img[c]);
}
ready = true;
m.unlock();
}
for (int c = 0; c < n_cameras; c++) {
img[c].copyTo(img_display[c]);
m.lock();
if (image_points[c].size() > 0) {
for (auto &points : image_points[c]) {
calib.drawCorners(img_display[c], points);
}
calib.drawPoints(img_display[c], image_points[c].back());
}
m.unlock();
}
cv::namedWindow("Cameras", cv::WINDOW_KEEPRATIO | cv::WINDOW_NORMAL);
cv::imshow("Cameras", display);
cv::waitKey(10);
}
cv::destroyAllWindows();
//bool calib_ok = true;
for (int c = 0; c < n_cameras; c++) {
LOG(INFO) << "Calculating intrinsic paramters for camera " << std::to_string(c);
vector<Mat> rvecs, tvecs;
double rms = cv::calibrateCamera(
object_points[c], image_points[c],
image_size, camera_matrix[c], dist_coeffs[c],
rvecs, tvecs, calibrate_flags
);
LOG(INFO) << "final reprojection RMS error: " << rms;
if (!ftl::calibration::validate::distortionCoefficients(dist_coeffs[c], image_size)) {
LOG(ERROR) << "Calibration failed: invalid distortion coefficients:\n"
<< dist_coeffs[c];
LOG(WARNING) << "Estimating only intrinsic parameters for camera " << std::to_string(c);
dist_coeffs[c] = Mat(Size(8, 1), CV_64FC1, cv::Scalar(0.0));
calibrate_flags |= cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 | cv::CALIB_FIX_K3;
c--;
continue;
}
//calib_ok = true;
calibrate_flags &= ~cv::CALIB_FIX_K1 & ~cv::CALIB_FIX_K2 & ~cv::CALIB_FIX_K3;
double fovx, fovy, focal_length, aspect_ratio;
cv::Point2d principal_point;
// TODO: check for valid aperture width/height; do not run if not valid values
cv::calibrationMatrixValues(camera_matrix[c], image_size, aperture_width, aperture_height,
fovx, fovy, focal_length, principal_point, aspect_ratio);
LOG(INFO) << "";
LOG(INFO) << " fovx (deg): " << fovx;
LOG(INFO) << " fovy (deg): " << fovy;
LOG(INFO) << " focal length (mm): " << focal_length;
LOG(INFO) << " principal point (mm): " << principal_point;
LOG(INFO) << " aspect ratio: " << aspect_ratio;
LOG(INFO) << "";
LOG(INFO) << "Camera matrix:\n" << camera_matrix[c];
LOG(INFO) << "Distortion coefficients:\n" << dist_coeffs[c];
LOG(INFO) << "";
}
saveIntrinsics(filename_intrinsics, camera_matrix, dist_coeffs, image_size);
LOG(INFO) << "intrinsic paramaters saved to: " << filename_intrinsics;
vector<Mat> map1(n_cameras), map2(n_cameras);
for (int c = 0; c < n_cameras; c++) {
cv::initUndistortRectifyMap(camera_matrix[c], dist_coeffs[c], Mat::eye(3,3, CV_64F), camera_matrix[c],
image_size, CV_16SC2, map1[c], map2[c]);
}
while (cv::waitKey(25) != 27) {
for (auto &camera : cameras ) { camera.grab(); }
for (int c = 0; c < n_cameras; c++) {
cameras[c].retrieve(img[c]);
cv::remap(img[c], img[c], map1[c], map2[c], cv::INTER_CUBIC);
cv::imshow("Camera " + std::to_string(c), img[c]);
}
}
}
applications/calibration/src/lens.hpp deleted 100644 → 0
View file @ e2e259ab
#ifndef _FTL_CALIBRATION_LENS_HPP_
#define _FTL_CALIBRATION_LENS_HPP_
#include <map>
#include <string>
namespace ftl {
namespace calibration {
void intrinsic(std::map<std::string, std::string> &opt);
}
}
#endif // _FTL_CALIBRATION_LENS_HPP_
applications/calibration/src/main.cpp deleted 100644 → 0
View file @ e2e259ab
#include <loguru.hpp>
#include <ftl/configuration.hpp>
#include "lens.hpp"
#include "stereo.hpp"
#include "align.hpp"
int main(int argc, char **argv) {
loguru::g_preamble_date = false;
loguru::g_preamble_uptime = false;
loguru::g_preamble_thread = false;
loguru::init(argc, argv, "--verbosity");
argc--;
argv++;
// Process Arguments
auto options = ftl::config::read_options(&argv, &argc);
ftl::calibration::intrinsic(options);
return 0;
}
applications/calibration/src/stereo.cpp deleted 100644 → 0
View file @ e2e259ab
#include <loguru.hpp>
#include <ftl/config.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include "common.hpp"
#include "stereo.hpp"
using std::vector;
using std::map;
using std::string;
using cv::Mat;
using cv::Vec2f, cv::Vec3f;
using cv::Size;
using cv::stereoCalibrate;
using namespace ftl::calibration;
void ftl::calibration::stereo(map<string, string> &opt) {
LOG(INFO) << "Begin stereo calibration";
// TODO PARAMETERS TO CONFIG FILE
// image size, also used by CalibrationChessboard
Size image_size = Size( getOptionInt(opt, "width", 1280),
getOptionInt(opt, "height", 720));
// iterations
int iter = getOptionInt(opt, "iter", 50);
// delay between images
double delay = getOptionInt(opt, "delay", 250);
// max_error for a single image; if error larger image discarded
double max_error = getOptionDouble(opt, "max_error", 1.0);
// scaling/cropping (see OpenCV stereoRectify())
float alpha = getOptionDouble(opt, "alpha", 0);
// intrinsics filename
string filename_intrinsics = getOptionString(opt, "profile", "./panasonic.yml");
bool use_grid = (bool) getOptionInt(opt, "use_grid", 0);
LOG(INFO) << "Stereo calibration parameters";
LOG(INFO) << " profile: " << filename_intrinsics;
LOG(INFO) << " width: " << image_size.width;
LOG(INFO) << " height: " << image_size.height;
LOG(INFO) << " iter: " << iter;
LOG(INFO) << " delay: " << delay;
LOG(INFO) << " max_error: " << max_error;
LOG(INFO) << " alpha: " << alpha;
LOG(INFO) << " use_grid: " << use_grid;
LOG(INFO) << "-----------------------------------";
CalibrationChessboard calib(opt);
vector<Grid> grids;
int grid_i = 0;
// grid parameters, 3x3 grid; one small grid and one large grid. Grids are cycled until
// iter reaches zero
grids.push_back(Grid(3, 3,
(3.0f/4.0f) * image_size.width, (3.0f/4.0f) * image_size.height,
((1.0f/4.0f) * image_size.width) / 2, ((1.0f/4.0f) * image_size.height) / 2));
grids.push_back(Grid(3, 3, image_size.width, image_size.height, 0, 0));
Grid grid = grids[grid_i];
// PARAMETERS
int stereocalibrate_flags =
cv::CALIB_FIX_INTRINSIC | cv::CALIB_FIX_PRINCIPAL_POINT | cv::CALIB_FIX_ASPECT_RATIO |
cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_SAME_FOCAL_LENGTH |
cv::CALIB_FIX_K3 | cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5;
vector<cv::VideoCapture> cameras { cv::VideoCapture(0), cv::VideoCapture(1) };
for (auto &camera : cameras ) {
if (!camera.isOpened()) {
LOG(ERROR) << "Could not open camera device";
return;
}
camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width);
camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
}
// image points to calculate the parameters after all input data is captured
vector<vector<vector<Vec2f>>> image_points(2);
vector<vector<Vec3f>> object_points;
// image points for each grid, updated to image_points and object_points
// after is grid complete
vector<vector<vector<Vec2f>>> image_points_grid(9, vector<vector<Vec2f>>(2));
vector<vector<Vec3f>> object_points_grid(9);
vector<Mat> dist_coeffs(2);
vector<Mat> camera_matrices(2);
Size intrinsic_resolution;
if (!loadIntrinsics(filename_intrinsics, camera_matrices, dist_coeffs, intrinsic_resolution))
{
LOG(FATAL) << "Failed to load intrinsic camera parameters from file.";
}
if (intrinsic_resolution != image_size)
{
LOG(FATAL) << "Intrinsic resolution is not same as input resolution (TODO)";
}
Mat R, T, E, F, per_view_errors;
// capture calibration patterns
while (iter > 0) {
int res = 0;
int grid_pos = -1;
vector<Mat> new_img(2);
vector<vector<Vec2f>> new_points(2);
int delay_remaining = delay;
for (; delay_remaining > 50; delay_remaining -= 50) {
cv::waitKey(50);
for (size_t i = 0; i < 2; i++) {
auto &camera = cameras[i];
auto &img = new_img[i];
camera.grab();
camera.retrieve(img);
if (use_grid && i == 0) grid.drawGrid(img);
cv::imshow("Camera " + std::to_string(i), img);
}
}
for (size_t i = 0; i < 2; i++) {
auto &img = new_img[i];
auto &points = new_points[i];
// TODO move to "findPoints"-thread
if (calib.findPoints(img, points)) {
calib.drawPoints(img, points);
res++;
}
cv::imshow("Camera " + std::to_string(i), img);
}
if (res != 2) { LOG(WARNING) << "Input not detected on all inputs"; continue; }
if (use_grid) {
// top left and bottom right corners; not perfect but good enough
grid_pos = grid.checkGrid(
cv::Point(new_points[0][0]),
cv::Point(new_points[0][new_points[0].size()-1])
);
if (grid_pos == -1) { LOG(WARNING) << "Captured pattern not inside grid cell"; continue; }
}
vector<Vec3f> points_ref;
calib.objectPoints(points_ref);
/* doesn't seem to be very helpful (error almost always low enough)
// calculate reprojection error with single pair of images
// reject it if RMS reprojection error too high
int flags = stereocalibrate_flags;
double rms_iter = stereoCalibrate(
vector<vector<Vec3f>> { points_ref },
vector<vector<Vec2f>> { new_points[0] },
vector<vector<Vec2f>> { new_points[1] },
camera_matrices[0], dist_coeffs[0],
camera_matrices[1], dist_coeffs[1],
image_size, R, T, E, F, per_view_errors,
flags);
LOG(INFO) << "rms for pattern: " << rms_iter;
if (rms_iter > max_error) {
LOG(WARNING) << "RMS reprojection error too high, maximum allowed error: " << max_error;
continue;
}*/
if (use_grid) {
// store results in result grid
object_points_grid[grid_pos] = points_ref;
for (size_t i = 0; i < 2; i++) { image_points_grid[grid_pos][i] = new_points[i]; }
grid.updateGrid(grid_pos);
if (grid.isComplete()) {
LOG(INFO) << "Grid complete";
grid.reset();
grid_i = (grid_i + 1) % grids.size();
grid = grids[grid_i];
// copy results
object_points.insert(object_points.end(), object_points_grid.begin(), object_points_grid.end());
for (size_t i = 0; i < image_points_grid.size(); i++) {
for (size_t j = 0; j < 2; j++) { image_points[j].push_back(image_points_grid[i][j]); }
}
iter--;
}
}
else {
object_points.push_back(points_ref);
for (size_t i = 0; i < 2; i++) { image_points[i].push_back(new_points[i]); }
iter--;
}
}
// calculate stereoCalibration using all input images (which have low enough
// RMS error in previous step)
LOG(INFO) << "Calculating extrinsic stereo parameters using " << object_points.size() << " samples.";
CHECK(object_points.size() == image_points[0].size());
CHECK(object_points.size() == image_points[1].size());
double rms = stereoCalibrate(object_points,
image_points[0], image_points[1],
camera_matrices[0], dist_coeffs[0],
camera_matrices[1], dist_coeffs[1],
image_size, R, T, E, F, per_view_errors,
stereocalibrate_flags,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 120, 1e-6)
);
LOG(INFO) << "Final extrinsic calibration RMS (reprojection error): " << rms;
for (int i = 0; i < per_view_errors.rows * per_view_errors.cols; i++) {
LOG(4) << "error for sample " << i << ": " << ((double*) per_view_errors.data)[i];
}
Mat R1, R2, P1, P2, Q;
cv::Rect validRoi[2];
stereoRectify(
camera_matrices[0], dist_coeffs[0],
camera_matrices[1], dist_coeffs[1],
image_size, R, T, R1, R2, P1, P2, Q,
0, alpha, image_size,
&validRoi[0], &validRoi[1]
);
saveExtrinsics(FTL_LOCAL_CONFIG_ROOT "/extrinsics.yml", R, T, R1, R2, P1, P2, Q);
LOG(INFO) << "Stereo camera extrinsics saved to: " << FTL_LOCAL_CONFIG_ROOT "/extrinsics.yml";
for (size_t i = 0; i < 2; i++) { cv::destroyWindow("Camera " + std::to_string(i)); }
// Visualize results
vector<Mat> map1(2), map2(2);
cv::initUndistortRectifyMap(camera_matrices[0], dist_coeffs[0], R1, P1, image_size, CV_16SC2, map1[0], map2[0]);
cv::initUndistortRectifyMap(camera_matrices[1], dist_coeffs[1], R2, P2, image_size, CV_16SC2, map1[1], map2[1]);
vector<Mat> in(2);
vector<Mat> out(2);
// vector<Mat> out_gray(2);
// Mat diff, diff_color;
while(cv::waitKey(25) == -1) {
for(size_t i = 0; i < 2; i++) {
auto &camera = cameras[i];
camera.grab();
camera.retrieve(in[i]);
auto p = cv::Point2i(camera_matrices[i].at<double>(0, 2), camera_matrices[i].at<double>(1, 2));
cv::drawMarker(in[i], p, cv::Scalar(51, 204, 51), cv::MARKER_CROSS, 40, 1);
cv::drawMarker(in[i], p, cv::Scalar(51, 204, 51), cv::MARKER_SQUARE, 25);
cv::remap(in[i], out[i], map1[i], map2[i], cv::INTER_CUBIC);
// draw lines
for (int r = 50; r < image_size.height; r = r+50) {
cv::line(out[i], cv::Point(0, r), cv::Point(image_size.width-1, r), cv::Scalar(0,0,255), 1);
}
if (i == 0) { // left camera
auto p_r = cv::Point2i(-Q.at<double>(0, 3), -Q.at<double>(1, 3));
cv::drawMarker(out[i], p_r, cv::Scalar(0, 0, 204), cv::MARKER_CROSS, 30);
cv::drawMarker(out[i], p_r, cv::Scalar(0, 0, 204), cv::MARKER_SQUARE);
}
cv::imshow("Camera " + std::to_string(i) + " (unrectified)", in[i]);
cv::imshow("Camera " + std::to_string(i) + " (rectified)", out[i]);
}
/* not useful
cv::absdiff(out_gray[0], out_gray[1], diff);
cv::applyColorMap(diff, diff_color, cv::COLORMAP_JET);
cv::imshow("Difference", diff_color);
*/
}
cv::destroyAllWindows();
}
applications/calibration/src/stereo.hpp deleted 100644 → 0
View file @ e2e259ab
#ifndef _FTL_CALIBRATION_STEREO_HPP_
#define _FTL_CALIBRATION_STEREO_HPP_
#include <map>
#include <string>
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
namespace ftl {
namespace calibration {
void stereo(std::map<std::string, std::string> &opt);
}
}
#endif // _FTL_CALIBRATION_STEREO_HPP_
applications/ftl2mkv/src/main.cpp
View file @ 70063e8d
......@@ -5,6 +5,7 @@
#include <ftl/rgbd/camera.hpp>
#include <ftl/codecs/hevc.hpp>
#include <ftl/codecs/h264.hpp>
#include <ftl/codecs/decoder.hpp>
#include <fstream>
......@@ -17,7 +18,7 @@ extern "C" {
using ftl::codecs::codec_t;
using ftl::codecs::Channel;
static AVStream *add_video_stream(AVFormatContext *oc, const ftl::codecs::Packet &pkt)
static AVStream *add_video_stream(AVFormatContext *oc, const ftl::codecs::Packet &pkt, const ftl::rgbd::Camera &cam)
{
//AVCodecContext *c;
AVStream *st;
......@@ -47,13 +48,13 @@ static AVStream *add_video_stream(AVFormatContext *oc, const ftl::codecs::Packet
//st->nb_frames = 0;
st->codecpar->codec_id = codec_id;
st->codecpar->codec_type = AVMEDIA_TYPE_VIDEO;
st->codecpar->width = ftl::codecs::getWidth(pkt.definition);
st->codecpar->width = cam.width; //ftl::codecs::getWidth(pkt.definition);
//if (pkt.flags & ftl::codecs::kFlagStereo) st->codecpar->width *= 2;
st->codecpar->height = ftl::codecs::getHeight(pkt.definition);
st->codecpar->height = cam.height; //ftl::codecs::getHeight(pkt.definition);
st->codecpar->format = AV_PIX_FMT_NV12;
st->codecpar->bit_rate = 4000000;
if (pkt.flags & ftl::codecs::kFlagStereo) av_dict_set(&st->metadata, "stereo_mode", "left_right", 0);
//if (pkt.flags & ftl::codecs::kFlagStereo) av_dict_set(&st->metadata, "stereo_mode", "left_right", 0);
//if (pkt.flags & ftl::codecs::kFlagStereo) av_dict_set(&oc->metadata, "stereo_mode", "1", 0);
//if (pkt.flags & ftl::codecs::kFlagStereo) av_dict_set_int(&st->metadata, "StereoMode", 1, 0);
......@@ -78,6 +79,50 @@ static AVStream *add_video_stream(AVFormatContext *oc, const ftl::codecs::Packet
return st;
}
static AVStream *add_audio_stream(AVFormatContext *oc, const ftl::codecs::Packet &pkt)
{
//AVCodecContext *c;
AVStream *st;
st = avformat_new_stream(oc, 0);
if (!st) {
fprintf(stderr, "Could not alloc stream\n");
exit(1);
}
AVCodecID codec_id = AV_CODEC_ID_OPUS;
st->codecpar->codec_id = codec_id;
st->codecpar->codec_type = AVMEDIA_TYPE_AUDIO;
st->codecpar->channel_layout = 0;
st->codecpar->channels = 2;
st->codecpar->sample_rate = 48000;
st->codecpar->frame_size = 960;
return st;
}
static uint32_t make_id(const ftl::codecs::StreamPacket &spkt) {
return (((spkt.streamID << 8) + spkt.frame_number) << 8) + int(spkt.channel);
}
struct StreamState {
int64_t first_ts = 100000000000000000ll;
std::list<std::pair<ftl::codecs::StreamPacket, ftl::codecs::Packet>> packets;
bool seen_key = false;
AVStream *stream = nullptr;
int64_t last_ts = 0;
void insert(const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
for (auto i = packets.begin(); i != packets.end(); ++i) {
if (i->first.timestamp > spkt.timestamp) {
packets.insert(i, std::make_pair(spkt, pkt));
return;
}
}
packets.push_back(std::make_pair(spkt, pkt));
}
};
int main(int argc, char **argv) {
std::string filename;
......@@ -109,8 +154,12 @@ int main(int argc, char **argv) {
AVOutputFormat *fmt;
AVFormatContext *oc;
AVStream *video_st[10][2] = {nullptr};
StreamState video_st[10];
int stream_count = 0;
std::unordered_map<uint32_t, int> mapping;
// TODO: Remove in newer versions
av_register_all();
fmt = av_guess_format(NULL, outputfile.c_str(), NULL);
......@@ -127,7 +176,11 @@ int main(int argc, char **argv) {
return -1;
}
oc->oformat = fmt;
// TODO: Use URL in newer versions
snprintf(oc->filename, sizeof(oc->filename), "%s", outputfile.c_str());
//oc->url = (char*)av_malloc(outputfile.size()+1);
//snprintf(oc->url, outputfile.size()+1, "%s", outputfile.c_str());
/* open the output file, if needed */
if (!(fmt->flags & AVFMT_NOFILE)) {
......@@ -144,26 +197,63 @@ int main(int argc, char **argv) {
//bool stream_added[10] = {false};
int64_t first_ts = 10000000000000ll;
// TODO: In future, find a better way to discover number of streams...
// Read entire file to find all streams before reading again to write data
bool res = r.read(90000000000000, [&first_ts,&current_stream,&current_channel,&r,&video_st,oc](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel) && current_channel != -1) return;
if (spkt.frame_number == current_stream || current_stream == 255) {
bool res = r.read(90000000000000, [&current_stream,&current_channel,&r,&video_st,oc,&mapping,&stream_count,root](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
if (spkt.channel != Channel::Audio && current_stream != 255 && spkt.streamID != current_stream) return;
if (spkt.channel != Channel::Colour && spkt.channel != Channel::Right && spkt.channel != Channel::Audio) return;
//if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel) && current_channel != -1) return;
//if (spkt.frame_number == current_stream || current_stream == 255) {
if (pkt.codec != codec_t::HEVC && pkt.codec != codec_t::H264) {
if (pkt.codec != codec_t::HEVC && pkt.codec != codec_t::H264 && pkt.codec != codec_t::OPUS) {
return;
}
if (spkt.frame_number >= 10) return; // TODO: Allow for more than 10
if (spkt.timestamp < first_ts) first_ts = spkt.timestamp;
//if (video_st[spkt.frame_number][(spkt.channel == Channel::Left) ? 0 : 1] == nullptr) {
if ((pkt.codec == codec_t::HEVC && ftl::codecs::hevc::isIFrame(pkt.data.data(), pkt.data.size())) ||
(pkt.codec == codec_t::H264 && ftl::codecs::h264::isIFrame(pkt.data.data(), pkt.data.size()))) {
if (mapping.count(make_id(spkt)) == 0) {
int id = stream_count++;
if (id >= 10) return;
auto *dec = ftl::codecs::allocateDecoder(pkt);
if (!dec) return;
if (spkt.timestamp < video_st[id].first_ts) video_st[id].first_ts = spkt.timestamp;
cv::cuda::GpuMat m;
dec->decode(pkt, m);
ftl::rgbd::Camera cam;
cam.width = m.cols;
cam.height = m.rows;
// Use decoder to get frame size...
video_st[id].stream = add_video_stream(oc, pkt, cam);
ftl::codecs::free(dec);
mapping[make_id(spkt)] = id;
}
} else if (pkt.codec == codec_t::OPUS) {
if (mapping.count(make_id(spkt)) == 0) {
int id = stream_count++;
if (id >= 10) return;
if (spkt.timestamp < video_st[id].first_ts) video_st[id].first_ts = spkt.timestamp;
if (video_st[spkt.frame_number][(spkt.channel == Channel::Left) ? 0 : 1] == nullptr) {
video_st[spkt.frame_number][(spkt.channel == Channel::Left) ? 0 : 1] = add_video_stream(oc, pkt);
video_st[id].stream = add_audio_stream(oc, pkt);
video_st[id].seen_key = true;
video_st[id].last_ts = root->value("audio_delay", 1000); // second delay?
mapping[make_id(spkt)] = id;
}
}
//}
});
r.end();
......@@ -182,64 +272,175 @@ int main(int argc, char **argv) {
LOG(ERROR) << "Failed to write stream header";
}
bool seen_key[10] = {false};
res = r.read(90000000000000, [first_ts,&current_stream,&current_channel,&r,&video_st,oc,&seen_key](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel) && current_channel != -1) return;
if (spkt.frame_number == current_stream || current_stream == 255) {
res = r.read(90000000000000, [&current_stream,&current_channel,&r,&video_st,oc,&mapping](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
//if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel) && current_channel != -1) return;
//if (spkt.frame_number == current_stream || current_stream == 255) {
if (pkt.codec != codec_t::HEVC && pkt.codec != codec_t::H264) {
if (pkt.codec != codec_t::HEVC && pkt.codec != codec_t::H264 && pkt.codec != codec_t::OPUS) {
return;
}
//LOG(INFO) << "Reading packet: (" << (int)spkt.streamID << "," << (int)spkt.channel << ") " << (int)pkt.codec << ", " << (int)pkt.definition;
if (spkt.frame_number >= 10) return; // TODO: Allow for more than 10
auto i = mapping.find(make_id(spkt));
if (i == mapping.end()) return;
int id = i->second;
if (!video_st[id].stream) return;
bool keyframe = false;
if (pkt.codec == codec_t::HEVC) {
if (ftl::codecs::hevc::isIFrame(pkt.data.data(), pkt.data.size())) {
seen_key[spkt.frame_number] = true;
video_st[id].seen_key = true;
keyframe = true;
}
} else if (pkt.codec == codec_t::H264) {
if (ftl::codecs::h264::isIFrame(pkt.data.data(), pkt.data.size())) {
seen_key[spkt.frame_number] = true;
video_st[id].seen_key = true;
keyframe = true;
}
}
if (!seen_key[spkt.frame_number]) return;
if (!video_st[id].seen_key) return;
//if (spkt.timestamp > last_ts) framecount++;
//last_ts = spkt.timestamp;
video_st[id].insert(spkt, pkt);
if (video_st[id].packets.size() > 5) {
auto &spkt = video_st[id].packets.front().first;
auto &pkt = video_st[id].packets.front().second;
if (pkt.codec == codec_t::OPUS) {
// Must unpack the audio
const unsigned char *inptr = pkt.data.data();
int count = 0;
int frames = 0;
for (size_t i=0; i<pkt.data.size(); ) {
AVPacket avpkt;
av_init_packet(&avpkt);
avpkt.stream_index= video_st[id].stream->index;
const short *len = (const short*)inptr;
if (*len == 0) break;
if (frames == 10) break;
inptr += 2;
i += (*len)+2;
avpkt.pts = video_st[id].last_ts;
avpkt.dts = avpkt.pts;
avpkt.data= const_cast<uint8_t*>(inptr);
avpkt.size= *len;
avpkt.duration = 20;
video_st[id].last_ts += avpkt.duration;
/* write the compressed frame in the media file */
auto ret = av_write_frame(oc, &avpkt);
if (ret != 0) {
LOG(ERROR) << "Error writing audio frame: " << ret;
}
inptr += *len;
++frames;
}
} else {
AVPacket avpkt;
av_init_packet(&avpkt);
avpkt.stream_index= video_st[id].stream->index;
if (keyframe) avpkt.flags |= AV_PKT_FLAG_KEY;
//avpkt.pts = framecount*50; //spkt.timestamp - r.getStartTime();
avpkt.pts = spkt.timestamp - first_ts;
avpkt.pts = spkt.timestamp - video_st[id].first_ts;
avpkt.dts = avpkt.pts;
avpkt.stream_index= video_st[spkt.frame_number][(spkt.channel == Channel::Left) ? 0 : 1]->index;
avpkt.data= const_cast<uint8_t*>(pkt.data.data());
avpkt.size= pkt.data.size();
avpkt.duration = 1;
avpkt.duration = 0;
/* write the compressed frame in the media file */
auto ret = av_write_frame(oc, &avpkt);
if (ret != 0) {
LOG(ERROR) << "Error writing video frame: " << ret;
}
}
//LOG(INFO) << "write frame: " << avpkt.pts << "," << avpkt.stream_index << "," << avpkt.size;
video_st[id].packets.pop_front();
}
//}
});
for (int i=0; i<10; ++i) {
while (video_st[i].packets.size() > 0) {
int id = i;
auto &spkt = video_st[i].packets.front().first;
auto &pkt = video_st[i].packets.front().second;
if (pkt.codec == codec_t::OPUS) {
// Must unpack the audio
const unsigned char *inptr = pkt.data.data();
int count = 0;
int frames = 0;
for (size_t i=0; i<pkt.data.size(); ) {
AVPacket avpkt;
av_init_packet(&avpkt);
avpkt.stream_index= video_st[id].stream->index;
const short *len = (const short*)inptr;
if (*len == 0) break;
if (frames == 10) break;
inptr += 2;
i += (*len)+2;
avpkt.pts = video_st[id].last_ts;
avpkt.dts = avpkt.pts;
avpkt.data= const_cast<uint8_t*>(inptr);
avpkt.size= *len;
avpkt.duration = 20;
video_st[id].last_ts += avpkt.duration;
/* write the compressed frame in the media file */
auto ret = av_write_frame(oc, &avpkt);
if (ret != 0) {
LOG(ERROR) << "Error writing frame: " << ret;
LOG(ERROR) << "Error writing audio frame: " << ret;
}
inptr += *len;
++frames;
}
} else {
AVPacket avpkt;
av_init_packet(&avpkt);
avpkt.stream_index= video_st[id].stream->index;
//if (keyframe) avpkt.flags |= AV_PKT_FLAG_KEY;
avpkt.pts = spkt.timestamp - video_st[id].first_ts;
avpkt.dts = avpkt.pts;
avpkt.data= const_cast<uint8_t*>(pkt.data.data());
avpkt.size= pkt.data.size();
avpkt.duration = 0;
/* write the compressed frame in the media file */
auto ret = av_write_frame(oc, &avpkt);
if (ret != 0) {
LOG(ERROR) << "Error writing video frame: " << ret;
}
}
video_st[i].packets.pop_front();
}
}
});
av_write_trailer(oc);
//avcodec_close(video_st->codec);
for (int i=0; i<10; ++i) {
if (video_st[i][0]) av_free(video_st[i][0]);
if (video_st[i][1]) av_free(video_st[i][1]);
if (video_st[i].stream) av_free(video_st[i].stream);
}
if (!(fmt->flags & AVFMT_NOFILE)) {
......
applications/gui/src/camera.cpp deleted 100644 → 0
View file @ e2e259ab
#include "camera.hpp"
#include "pose_window.hpp"
#include "screen.hpp"
#include <nanogui/glutil.h>
#include <ftl/profiler.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/cudaarithm.hpp>
#include <ftl/operators/antialiasing.hpp>
#include <ftl/cuda/normals.hpp>
#include <ftl/render/colouriser.hpp>
#include <ftl/cuda/transform.hpp>
#include <ftl/operators/gt_analysis.hpp>
#include <ftl/operators/poser.hpp>
#include <ftl/cuda/colour_cuda.hpp>
#include <ftl/streams/parsers.hpp>
#include <ftl/render/overlay.hpp>
#include "statsimage.hpp"
#define LOGURU_REPLACE_GLOG 1
#include <loguru.hpp>
#include <fstream>
#ifdef HAVE_OPENVR
#include "vr.hpp"
#endif
using ftl::rgbd::isValidDepth;
using ftl::gui::GLTexture;
using ftl::gui::PoseWindow;
using ftl::codecs::Channel;
using ftl::codecs::Channels;
using cv::cuda::GpuMat;
static int vcamcount = 0;
static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
Eigen::Affine3d rx =
Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
Eigen::Affine3d ry =
Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
Eigen::Affine3d rz =
Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
return rz * rx * ry;
}
ftl::gui::Camera::Camera(ftl::gui::Screen *screen, int fsmask, int fid, ftl::codecs::Channel c)
: screen_(screen), fsmask_(fsmask), fid_(fid), texture1_(GLTexture::Type::BGRA), texture2_(GLTexture::Type::BGRA), depth1_(GLTexture::Type::Float), channel_(c),channels_(0u) {
eye_ = Eigen::Vector3d::Zero();
neye_ = Eigen::Vector4d::Zero();
rotmat_.setIdentity();
//up_ = Eigen::Vector3f(0,1.0f,0);
lerpSpeed_ = 0.999f;
sdepth_ = false;
ftime_ = (float)glfwGetTime();
pause_ = false;
#ifdef HAVE_OPENVR
vr_mode_ = false;
#endif
//channel_ = Channel::Left;
channels_ += c;
//channels_ += Channel::Depth;
width_ = 0;
height_ = 0;
// Create pose window...
//posewin_ = new PoseWindow(screen, src_->getURI());
//posewin_->setTheme(screen->windowtheme);
//posewin_->setVisible(false);
posewin_ = nullptr;
renderer_ = nullptr;
renderer2_ = nullptr;
post_pipe_ = nullptr;
record_stream_ = nullptr;
transform_ix_ = -1;
stereo_ = false;
rx_ = 0;
ry_ = 0;
framesets_ = nullptr;
colouriser_ = ftl::create<ftl::render::Colouriser>(screen->root(), "colouriser");
overlayer_ = ftl::create<ftl::overlay::Overlay>(screen->root(), "overlay");
// Is virtual camera?
if (fid == 255) {
renderer_ = ftl::create<ftl::render::CUDARender>(screen_->root(), std::string("vcam")+std::to_string(vcamcount++));
// Allow mask to be changed
fsmask_ = renderer_->value("fsmask", fsmask_);
renderer_->on("fsmask", [this](const ftl::config::Event &e) {
fsmask_ = renderer_->value("fsmask", fsmask_);
});
// Allow Pose origin to be changed
pose_source_ = renderer_->value("pose_source", pose_source_);
renderer_->on("pose_source", [this](const ftl::config::Event &e) {
pose_source_ = renderer_->value("pose_source", pose_source_);
});
intrinsics_ = ftl::create<ftl::Configurable>(renderer_, "intrinsics");
state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
state_.getRight() = state_.getLeft();
intrinsics_->on("width", [this](const ftl::config::Event &e) {
state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
state_.getRight() = state_.getLeft();
});
intrinsics_->on("focal", [this](const ftl::config::Event &e) {
state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
state_.getRight() = state_.getLeft();
});
{
Eigen::Matrix4d pose;
pose.setIdentity();
state_.setPose(pose);
for (auto &t : transforms_) {
t.setIdentity();
}
}
{
double camera_initial_x = intrinsics_->value("camera_x", 0.0);
double camera_initial_y = intrinsics_->value("camera_y", -1.75);
double camera_initial_z = intrinsics_->value("camera_z", 0.0);
double lookat_initial_x = intrinsics_->value("lookat_x", 1.0);
double lookat_initial_y = intrinsics_->value("lookat_y", 0.0);
double lookat_initial_z = intrinsics_->value("lookat_z", 0.0);
Eigen::Vector3f head(camera_initial_x, camera_initial_y, camera_initial_z);
Eigen::Vector3f lookat(lookat_initial_x, lookat_initial_y, lookat_initial_z);
// TODO up vector
Eigen::Matrix4f pose = nanogui::lookAt(head, head+lookat, Eigen::Vector3f(0.0f, 1.0f, 0.0f));
eye_ = Eigen::Vector3d(camera_initial_x, camera_initial_y, camera_initial_z);
neye_ = Eigen::Vector4d(eye_(0), eye_(1), eye_(2), 0.0);
rotmat_ = pose.cast<double>();
rotmat_.block(0, 3, 3, 1).setZero();
}
}
}
ftl::gui::Camera::~Camera() {
//delete writer_;
//delete fileout_;
}
void ftl::gui::Camera::drawUpdated(std::vector<ftl::rgbd::FrameSet*> &fss) {
// Only draw if frameset updated.
if (!stale_frame_.test_and_set()) {
draw(fss);
}
}
void ftl::gui::Camera::draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
if (fid_ != 255) {
for (auto *fs : fss) {
if (!usesFrameset(fs->id)) continue;
UNIQUE_LOCK(fs->mtx, lk);
ftl::rgbd::Frame *frame = nullptr;
if ((size_t)fid_ >= fs->frames.size()) return;
if (!fs->hasFrame(fid_)) return;
frame = &fs->frames[fid_];
if (!frame->hasChannel(channel_)) return;
auto &buf = colouriser_->colourise(*frame, channel_, 0);
auto &buf2 = frame->getTexture<uchar4>(Channel::Colour);
ftl::cuda::compositeInverse(buf2, buf, 0);
// For non-virtual cameras, copy the CUDA texture into the opengl
// texture device-to-device.
texture1_.make(buf.width(), buf.height());
auto dst1 = texture1_.map(0);
cudaMemcpy2D(dst1.data, dst1.step1(), buf.devicePtr(), buf.pitch(), buf.width()*4, buf.height(), cudaMemcpyDeviceToDevice);
ftl::cuda::flip<uchar4>(dst1, 0);
texture1_.unmap(0);
depth1_.make(buf.width(), buf.height());
dst1 = depth1_.map(0);
dst1.setTo(cv::Scalar(0.5f));
depth1_.unmap(0);
width_ = texture1_.width();
height_ = texture1_.height();
return;
}
}
//if (fsid_ >= fss.size()) return;
//auto &fs = *fss[fsid_];
_applyPoseEffects(fss);
UNIQUE_LOCK(mutex_, lk2);
//state_.getLeft().fx = intrinsics_->value("focal", 700.0f);
//state_.getLeft().fy = state_.getLeft().fx;
_draw(fss);
}
std::pair<const ftl::rgbd::Frame *, const ftl::codecs::Face *> ftl::gui::Camera::_selectFace(std::vector<ftl::rgbd::FrameSet*> &fss) {
for (auto *fset : fss) {
for (const auto &f : fset->frames) {
if (f.hasChannel(Channel::Faces)) {
std::vector<ftl::codecs::Face> data;
f.get(Channel::Faces, data);
if (data.size() > 0) {
return {&f,&(*data.rbegin())};
}
}
}
}
return {nullptr, nullptr};
}
void ftl::gui::Camera::_generateWindow(const ftl::rgbd::Frame &f, const ftl::codecs::Face &face, Eigen::Matrix4d &pose_adjust, ftl::render::ViewPort &vp) {
auto cam = ftl::rgbd::Camera::from(intrinsics_);
auto d = face;
float screenWidth = intrinsics_->value("screen_size", 0.6f); // In meters
float screenHeight = (9.0f/16.0f) * screenWidth;
float screenDistance = (d.depth > cam.minDepth && d.depth < cam.maxDepth) ? d.depth : intrinsics_->value("screen_dist_default", 0.5f); // Face distance from screen in meters
auto pos = f.getLeft().screenToCam(float(d.box.x+(d.box.width/2)), float(d.box.y+(d.box.height/2)), screenDistance);
Eigen::Vector3f eye;
eye[0] = -pos.x;
eye[1] = pos.y;
eye[2] = -pos.z;
//eye[3] = 0;
Eigen::Translation3f trans(eye);
Eigen::Affine3f t(trans);
Eigen::Matrix4f viewPose = t.matrix();
// Calculate where the screen is within current camera space
Eigen::Vector4f p1 = viewPose.cast<float>() * (Eigen::Vector4f(screenWidth/2.0, screenHeight/2.0, 0, 1));
Eigen::Vector4f p2 = viewPose.cast<float>() * (Eigen::Vector4f(screenWidth/2.0, -screenHeight/2.0, 0, 1));
Eigen::Vector4f p3 = viewPose.cast<float>() * (Eigen::Vector4f(-screenWidth/2.0, screenHeight/2.0, 0, 1));
Eigen::Vector4f p4 = viewPose.cast<float>() * (Eigen::Vector4f(-screenWidth/2.0, -screenHeight/2.0, 0, 1));
p1 = p1 / p1[3];
p2 = p2 / p2[3];
p3 = p3 / p3[3];
p4 = p4 / p4[3];
float2 p1screen = cam.camToScreen<float2>(make_float3(p1[0],p1[1],p1[2]));
float2 p2screen = cam.camToScreen<float2>(make_float3(p2[0],p2[1],p2[2]));
float2 p3screen = cam.camToScreen<float2>(make_float3(p3[0],p3[1],p3[2]));
float2 p4screen = cam.camToScreen<float2>(make_float3(p4[0],p4[1],p4[2]));
std::vector<cv::Point2f> quad_pts;
std::vector<cv::Point2f> squre_pts;
quad_pts.push_back(cv::Point2f(p1screen.x,p1screen.y));
quad_pts.push_back(cv::Point2f(p2screen.x,p2screen.y));
quad_pts.push_back(cv::Point2f(p3screen.x,p3screen.y));
quad_pts.push_back(cv::Point2f(p4screen.x,p4screen.y));
squre_pts.push_back(cv::Point2f(0,0));
squre_pts.push_back(cv::Point2f(0,cam.height));
squre_pts.push_back(cv::Point2f(cam.width,0));
squre_pts.push_back(cv::Point2f(cam.width,cam.height));
cv::Mat transmtx = cv::getPerspectiveTransform(quad_pts,squre_pts);
//cv::Mat transformed = cv::Mat::zeros(overlay_.rows, overlay_.cols, CV_8UC4);
//cv::warpPerspective(im1_, im1_, transmtx, im1_.size());
// TODO: Use the transmtx above for perspective distortion..
//ftl::render::ViewPort vp;
vp.x = std::min(p4screen.x, std::min(p3screen.x, std::min(p1screen.x,p2screen.x)));
vp.y = std::min(p4screen.y, std::min(p3screen.y, std::min(p1screen.y,p2screen.y)));
vp.width = std::max(p4screen.x, std::max(p3screen.x, std::max(p1screen.x,p2screen.x))) - vp.x;
vp.height = std::max(p4screen.y, std::max(p3screen.y, std::max(p1screen.y,p2screen.y))) - vp.y;
/*vp.warpMatrix.entries[0] = transmtx.at<float>(0,0);
vp.warpMatrix.entries[1] = transmtx.at<float>(1,0);
vp.warpMatrix.entries[2] = transmtx.at<float>(2,0);
vp.warpMatrix.entries[3] = transmtx.at<float>(0,1);
vp.warpMatrix.entries[4] = transmtx.at<float>(1,1);
vp.warpMatrix.entries[5] = transmtx.at<float>(2,1);
vp.warpMatrix.entries[6] = transmtx.at<float>(0,2);
vp.warpMatrix.entries[7] = transmtx.at<float>(1,2);
vp.warpMatrix.entries[8] = transmtx.at<float>(2,2);
vp.warpMatrix = vp.warpMatrix.getInverse(); //.getInverse();*/
//renderer_->setViewPort(ftl::render::ViewPortMode::Warping, vp);
pose_adjust = viewPose.cast<double>();
}
void ftl::gui::Camera::_applyPoseEffects(std::vector<ftl::rgbd::FrameSet*> &fss) {
if (renderer_->value("window_effect", false)) {
auto [frame,face] = _selectFace(fss);
if (face) {
Eigen::Matrix4d windowPose;
ftl::render::ViewPort windowViewPort;
_generateWindow(*frame, *face, windowPose, windowViewPort);
// Apply the window effect
renderer_->setViewPort(ftl::render::ViewPortMode::Stretch, windowViewPort);
state_.getPose() = windowPose * state_.getPose();
}
}
}
void ftl::gui::Camera::setStereo(bool v) {
UNIQUE_LOCK(mutex_, lk);
if (isVirtual()) {
stereo_ = v;
} else if (v && availableChannels().has(Channel::Right)) {
stereo_ = true;
} else {
stereo_ = false;
}
}
static ftl::codecs::Channel mapToSecondChannel(ftl::codecs::Channel c) {
switch (c) {
case Channel::Depth : return Channel::Depth2;
case Channel::Normals : return Channel::Normals2;
default: return c;
}
}
void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
frame_.reset();
frame_.setOrigin(&state_);
// Make sure an OpenGL pixel buffer exists
texture1_.make(state_.getLeft().width, state_.getLeft().height);
depth1_.make(state_.getLeft().width, state_.getLeft().height);
if (isStereo()) texture2_.make(state_.getRight().width, state_.getRight().height);
// Map the GL pixel buffer to a GpuMat
frame_.create<cv::cuda::GpuMat>(Channel::Colour) = texture1_.map(renderer_->getCUDAStream());
frame_.create<cv::cuda::GpuMat>(Channel::Depth) = depth1_.map(renderer_->getCUDAStream());
frame_.createTexture<float>(Channel::Depth);
if (isStereo()) frame_.create<cv::cuda::GpuMat>(Channel::Colour2) = texture2_.map((renderer2_) ? renderer2_->getCUDAStream() : 0);
// TODO: Remove;
overlay_.create(state_.getLeft().height, state_.getLeft().width, CV_8UC4);
//frame_.create<cv::Mat>(Channel::Overlay) = overlay_;
//overlay_.setTo(cv::Scalar(0,0,0,0));
//bool enable_overlay = overlayer_->value("enabled", false);
{
FTL_Profile("Render",0.034);
renderer_->begin(frame_, Channel::Colour);
if (isStereo()) {
if (!renderer2_) {
renderer2_ = ftl::create<ftl::render::CUDARender>(screen_->root(), std::string("vcam")+std::to_string(vcamcount++));
}
renderer2_->begin(frame_, Channel::Colour2);
}
try {
for (auto *fs : fss) {
if (!usesFrameset(fs->id)) continue;
fs->mtx.lock();
renderer_->submit(fs, ftl::codecs::Channels<0>(Channel::Colour), transforms_[fs->id]);
if (isStereo()) renderer2_->submit(fs, ftl::codecs::Channels<0>(Channel::Colour), transforms_[fs->id]);
//if (enable_overlay) {
// Generate and upload an overlay image.
// overlayer_->apply(*fs, overlay_, state_);
// frame_.upload(Channel::Overlay, renderer_->getCUDAStream());
//}
}
renderer_->render();
if (isStereo()) renderer2_->render();
if (channel_ != Channel::Left && channel_ != Channel::Right && channel_ != Channel::None) {
renderer_->blend(channel_);
if (isStereo()) {
renderer2_->blend(mapToSecondChannel(channel_));
}
}
//if (enable_overlay) {
// renderer_->blend(Channel::Overlay);
//}
renderer_->end();
if (isStereo()) renderer2_->end();
} catch(std::exception &e) {
LOG(ERROR) << "Exception in render: " << e.what();
}
for (auto *fs : fss) {
if (!usesFrameset(fs->id)) continue;
fs->mtx.unlock();
}
}
if (!post_pipe_) {
post_pipe_ = ftl::config::create<ftl::operators::Graph>(screen_->root(), "post_filters");
post_pipe_->append<ftl::operators::FXAA>("fxaa");
post_pipe_->append<ftl::operators::GTAnalysis>("gtanalyse");
}
post_pipe_->apply(frame_, frame_, 0);
channels_ = frame_.getChannels();
frame_.get<cv::cuda::GpuMat>(Channel::Depth).download(im_depth_);
cv::flip(im_depth_, im_depth_, 0);
//frame_.get<cv::cuda::GpuMat>(Channel::Normals).download(im_normals_);
//im_normals_.createMatHeader().convertTo(im_normals_f_, CV_32FC4);
//cv::flip(im_normals_f_, im_normals_f_, 0);
// Normalize depth map
frame_.get<cv::cuda::GpuMat>(Channel::Depth).convertTo(frame_.get<cv::cuda::GpuMat>(Channel::Depth), CV_32F, 1.0/8.0);
width_ = texture1_.width();
height_ = texture1_.height();
if (record_stream_ && record_stream_->active()) {
// TODO: Allow custom channel selection
ftl::rgbd::FrameSet fs2;
auto &f = fs2.frames.emplace_back();
fs2.count = 1;
fs2.mask = 1;
//fs2.stale = false;
fs2.set(ftl::data::FSFlag::STALE);
if (frame_.hasChannel(Channel::Colour2)) {
frame_.swapTo(Channels<0>(Channel::Colour) | Channel::Colour2, f);
ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour), 0);
ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour2), 0);
} else {
frame_.swapTo(Channels<0>(Channel::Colour), f); // Channel::Colour + Channel::Depth
ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour), 0);
}
fs2.timestamp = ftl::timer::get_time();
fs2.id = 0;
record_sender_->post(fs2);
record_stream_->select(0, Channels<0>(Channel::Colour));
// Reverse the flip
if (f.hasChannel(Channel::Colour2)) {
ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour), 0);
ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour2), 0);
} else {
ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour), 0);
}
f.swapTo(Channels<0>(Channel::Colour), frame_);
} else if (do_snapshot_) {
do_snapshot_ = false;
cv::Mat flipped;
cv::Mat im1;
frame_.get<cv::cuda::GpuMat>(Channel::Colour).download(im1);
{
//UNIQUE_LOCK(mutex_, lk);
cv::flip(im1, flipped, 0);
}
cv::cvtColor(flipped, flipped, cv::COLOR_BGRA2BGR);
cv::imwrite(snapshot_filename_, flipped);
}
// Unmap GL buffer from CUDA and finish updating GL texture
texture1_.unmap(renderer_->getCUDAStream());
depth1_.unmap(renderer_->getCUDAStream());
if (isStereo()) texture2_.unmap(renderer2_->getCUDAStream());
}
void ftl::gui::Camera::update(int fsid, const ftl::codecs::Channels<0> &c) {
if (!isVirtual() && ((1 << fsid) & fsmask_)) {
channels_ += c;
//if (c.has(Channel::Depth)) {
//channels_ += Channel::ColourNormals;
//}
}
}
void ftl::gui::Camera::update(std::vector<ftl::rgbd::FrameSet*> &fss) {
UNIQUE_LOCK(mutex_, lk);
framesets_ = &fss;
stale_frame_.clear();
if (screen_->activeCamera() == this) {
for (auto *fs : fss) {
if (!usesFrameset(fs->id)) continue;
for (auto &f : fs->frames) {
//if (f.hasChanged(Channel::Pose)) {
f.patchPose(T_);
//}
}
}
}
//if (fss.size() <= fsid_) return;
if (fid_ == 255) {
name_ = "Virtual Camera";
} else {
for (auto *fs : fss) {
if (!usesFrameset(fs->id)) continue;
ftl::rgbd::Frame *frame = nullptr;
if ((size_t)fid_ >= fs->frames.size()) return;
frame = &fs->frames[fid_];
channels_ = frame->getChannels();
if (frame->hasChannel(Channel::Messages)) {
msgs_.clear();
frame->get(Channel::Messages, msgs_);
}
auto n = frame->get<std::string>("name");
if (n) {
name_ = *n;
} else {
name_ = "No name";
}
state_.getLeft() = frame->getLeftCamera();
return;
}
}
}
void ftl::gui::Camera::setPose(const Eigen::Matrix4d &p) {
eye_[0] = p(0,3);
eye_[1] = p(1,3);
eye_[2] = p(2,3);
double sx = Eigen::Vector3d(p(0,0), p(1,0), p(2,0)).norm();
double sy = Eigen::Vector3d(p(0,1), p(1,1), p(2,1)).norm();
double sz = Eigen::Vector3d(p(0,2), p(1,2), p(2,2)).norm();
Eigen::Matrix4d rot = p;
rot(0,3) = 0.0;
rot(1,3) = 0.0;
rot(2,3) = 0.0;
rot(0,0) = rot(0,0) / sx;
rot(1,0) = rot(1,0) / sx;
rot(2,0) = rot(2,0) / sx;
rot(0,1) = rot(0,1) / sy;
rot(1,1) = rot(1,1) / sy;
rot(2,1) = rot(2,1) / sy;
rot(0,2) = rot(0,2) / sz;
rot(1,2) = rot(1,2) / sz;
rot(2,2) = rot(2,2) / sz;
rotmat_ = rot;
}
void ftl::gui::Camera::mouseMovement(int rx, int ry, int button) {
//if (!src_->hasCapabilities(ftl::rgbd::kCapMovable)) return;
if (fid_ < 255) return;
if (button == 1) {
rx_ += rx;
ry_ += ry;
/*float rrx = ((float)ry * 0.2f * delta_);
//orientation_[2] += std::cos(orientation_[1])*((float)rel[1] * 0.2f * delta_);
float rry = (float)rx * 0.2f * delta_;
float rrz = 0.0;
Eigen::Affine3d r = create_rotation_matrix(rrx, -rry, rrz);
rotmat_ = rotmat_ * r.matrix();*/
}
}
void ftl::gui::Camera::keyMovement(int key, int modifiers) {
//if (!src_->hasCapabilities(ftl::rgbd::kCapMovable)) return;
if (fid_ < 255) return;
if (key == 263 || key == 262) {
float mag = (modifiers & 0x1) ? 0.01f : 0.1f;
float scalar = (key == 263) ? -mag : mag;
neye_ += rotmat_*Eigen::Vector4d(scalar,0.0,0.0,1.0);
return;
} else if (key == 264 || key == 265) {
float mag = (modifiers & 0x1) ? 0.01f : 0.1f;
float scalar = (key == 264) ? -mag : mag;
neye_ += rotmat_*Eigen::Vector4d(0.0,0.0,scalar,1.0);
return;
} else if (key == 266 || key == 267) {
float mag = (modifiers & 0x1) ? 0.01f : 0.1f;
float scalar = (key == 266) ? -mag : mag;
neye_ += rotmat_*Eigen::Vector4d(0.0,scalar,0.0,1.0);
return;
} else if (key >= '0' && key <= '5' && modifiers == 2) { // Ctrl+NUMBER
int ix = key - (int)('0');
transform_ix_ = ix-1;
return;
}
}
void ftl::gui::Camera::showPoseWindow() {
posewin_->setVisible(true);
}
void ftl::gui::Camera::showSettings() {
}
#ifdef HAVE_OPENVR
bool ftl::gui::Camera::setVR(bool on) {
if (on == vr_mode_) {
LOG(WARNING) << "VR mode already enabled";
return on;
}
vr_mode_ = on;
if (on) {
setStereo(true);
UNIQUE_LOCK(mutex_, lk);
//src_->set("baseline", baseline_);
state_.getLeft().baseline = baseline_;
Eigen::Matrix3d intrinsic;
unsigned int size_x, size_y;
screen_->getVR()->GetRecommendedRenderTargetSize(&size_x, &size_y);
state_.getLeft().width = size_x;
state_.getLeft().height = size_y;
state_.getRight().width = size_x;
state_.getRight().height = size_y;
intrinsic = getCameraMatrix(screen_->getVR(), vr::Eye_Left);
CHECK(intrinsic(0, 2) < 0 && intrinsic(1, 2) < 0);
state_.getLeft().fx = intrinsic(0,0);
state_.getLeft().fy = intrinsic(0,0);
state_.getLeft().cx = intrinsic(0,2);
state_.getLeft().cy = intrinsic(1,2);
intrinsic = getCameraMatrix(screen_->getVR(), vr::Eye_Right);
CHECK(intrinsic(0, 2) < 0 && intrinsic(1, 2) < 0);
state_.getRight().fx = intrinsic(0,0);
state_.getRight().fy = intrinsic(0,0);
state_.getRight().cx = intrinsic(0,2);
state_.getRight().cy = intrinsic(1,2);
vr_mode_ = true;
}
else {
vr_mode_ = false;
setStereo(false);
UNIQUE_LOCK(mutex_, lk);
state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
state_.getRight() = state_.getLeft();
}
return vr_mode_;
}
#endif
void ftl::gui::Camera::setChannel(Channel c) {
UNIQUE_LOCK(mutex_, lk);
channel_ = c;
}
/*static void drawEdges( const cv::Mat &in, cv::Mat &out,
const int ksize = 3, double weight = -1.0, const int threshold = 32,
const int threshold_type = cv::THRESH_TOZERO)
{
cv::Mat edges;
cv::Laplacian(in, edges, 8, ksize);
cv::threshold(edges, edges, threshold, 255, threshold_type);
cv::Mat edges_color(in.size(), CV_8UC4);
cv::addWeighted(edges, weight, out, 1.0, 0.0, out, CV_8UC4);
}*/
void ftl::gui::Camera::active(bool a) {
if (a) {
} else {
neye_[0] = eye_[0];
neye_[1] = eye_[1];
neye_[2] = eye_[2];
}
}
void ftl::gui::Camera::drawOverlay(const Eigen::Vector2f &s) {
if (!framesets_) return;
//UNIQUE_LOCK(mutex_,lk);
for (auto *fs : *framesets_) {
if (!usesFrameset(fs->id)) continue;
// Generate and upload an overlay image.
overlayer_->draw(*fs, state_, s);
}
}
const void ftl::gui::Camera::captureFrame() {
float now = (float)glfwGetTime();
if (!screen_->isVR() && (now - ftime_) < 0.04f) return;
delta_ = now - ftime_;
ftime_ = now;
//LOG(INFO) << "Frame delta: " << delta_;
//if (src_ && src_->isReady()) {
if (width_ > 0 && height_ > 0) {
Eigen::Matrix4d viewPose;
if (screen_->isVR()) {
#ifdef HAVE_OPENVR
vr::VRCompositor()->SetTrackingSpace(vr::TrackingUniverseStanding);
vr::VRCompositor()->WaitGetPoses(rTrackedDevicePose_, vr::k_unMaxTrackedDeviceCount, NULL, 0 );
if (isStereo() && rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid )
{
Eigen::Matrix4d eye_l = ConvertSteamVRMatrixToMatrix4(
vr::VRSystem()->GetEyeToHeadTransform(vr::Eye_Left));
//Eigen::Matrix4d eye_r = ConvertSteamVRMatrixToMatrix4(
// vr::VRSystem()->GetEyeToHeadTransform(vr::Eye_Left));
float baseline_in = 2.0 * eye_l(0, 3);
if (baseline_in != baseline_) {
baseline_ = baseline_in;
//src_->set("baseline", baseline_);
state_.getLeft().baseline = baseline_;
state_.getRight().baseline = baseline_;
}
Eigen::Matrix4d pose = ConvertSteamVRMatrixToMatrix4(rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking);
Eigen::Vector3d ea = pose.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
Eigen::Vector3d vreye;
vreye[0] = pose(0, 3);
vreye[1] = -pose(1, 3);
vreye[2] = -pose(2, 3);
// NOTE: If modified, should be verified with VR headset!
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(-ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(-ea[2], Eigen::Vector3d::UnitZ());
//double rd = 180.0 / 3.141592;
//LOG(INFO) << "rotation x: " << ea[0] *rd << ", y: " << ea[1] * rd << ", z: " << ea[2] * rd;
// pose.block<3, 3>(0, 0) = R;
rotmat_.block(0, 0, 3, 3) = R;
// TODO: Apply a rotation to orient also
eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
Eigen::Translation3d trans(eye_ + vreye);
Eigen::Affine3d t(trans);
viewPose = t.matrix() * rotmat_;
} else {
//LOG(ERROR) << "No VR Pose";
}
#endif
} else {
if (pose_source_.size() == 0) {
// Use mouse to move camera
float rrx = ((float)ry_ * 0.2f * delta_);
float rry = (float)rx_ * 0.2f * delta_;
float rrz = 0.0;
Eigen::Affine3d r = create_rotation_matrix(rrx, -rry, rrz);
rotmat_ = rotmat_ * r.matrix();
rx_ = 0;
ry_ = 0;
eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
Eigen::Translation3d trans(eye_);
Eigen::Affine3d t(trans);
viewPose = t.matrix() * rotmat_;
} else {
// Use some other pose source.
if (!ftl::operators::Poser::get(pose_source_, viewPose)) {
LOG(ERROR) << "Missing pose: " << pose_source_;
}
}
}
{
UNIQUE_LOCK(mutex_, lk);
if (isVirtual()) {
if (transform_ix_ == -1) {
state_.setPose(viewPose);
} else if (transform_ix_ >= 0) {
transforms_[transform_ix_] = viewPose;
}
}
}
if (framesets_) draw(*framesets_);
}
//return texture1_;
}
void ftl::gui::Camera::snapshot(const std::string &filename) {
/*cv::Mat flipped;
{
UNIQUE_LOCK(mutex_, lk);
//cv::flip(im1_, flipped, 0);
}
cv::cvtColor(flipped, flipped, cv::COLOR_BGRA2BGR);
cv::imwrite(filename, flipped);*/
snapshot_filename_ = filename;
do_snapshot_ = true;
}
void ftl::gui::Camera::startVideoRecording(const std::string &filename, const std::string &uri) {
if (!record_stream_) {
file_stream_ = ftl::create<ftl::stream::File>(screen_->root(), "video2d");
file_stream_->setMode(ftl::stream::File::Mode::Write);
net_stream_ = ftl::create<ftl::stream::Net>(screen_->root(), "liveStream", screen_->net());
record_stream_ = ftl::create<ftl::stream::Broadcast>(screen_->root(), "recordStream");
//record_stream_->add(file_stream_);
//record_stream_->add(net_stream_);
record_sender_ = ftl::create<ftl::stream::Sender>(screen_->root(), "videoEncode");
record_sender_->value("codec", 2); // Default H264
record_sender_->set("iframes", 50); // Add iframes by default
record_sender_->value("stereo", false); // If both channels, then default to stereo
record_sender_->onRequest([this](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
if (spkt.channel == ftl::codecs::Channel::Pose) {
auto pose = ftl::stream::parsePose(pkt);
ftl::operators::Poser::set(std::string("live"), pose);
}
});
}
if (record_stream_->active()) return;
record_stream_->clear();
if (filename.size() > 0) {
file_stream_->set("filename", filename);
record_stream_->add(file_stream_);
}
if (uri.size() > 0) {
net_stream_->set("uri", uri);
record_stream_->add(net_stream_);
}
record_sender_->setStream(record_stream_);
LOG(INFO) << "About to record";
if (record_stream_->begin()) LOG(INFO) << "Recording started...";
}
void ftl::gui::Camera::stopVideoRecording() {
if (record_stream_ && record_stream_->active()) record_stream_->end();
}
float ftl::gui::Camera::getDepth(int x, int y) {
if (x < 0 || y < 0) { return NAN; }
UNIQUE_LOCK(mutex_, lk);
if (x >= im_depth_.cols || y >= im_depth_.rows) { return NAN; }
LOG(INFO) << y << ", " << x;
return im_depth_.createMatHeader().at<float>(y, x);
}
cv::Point3f ftl::gui::Camera::getPoint(int x, int y) {
if (x < 0 || y < 0) { return cv::Point3f(); }
UNIQUE_LOCK(mutex_, lk);
LOG(INFO) << y << ", " << x;
if (x >= im_depth_.cols || y >= im_depth_.rows) { return cv::Point3f(); }
float d = im_depth_.createMatHeader().at<float>(y, x);
auto point = frame_.getLeftCamera().screenToCam(x, y, d);
Eigen::Vector4d p(point.x, point.y, point.z, 1.0f);
Eigen::Matrix4d pose = frame_.getPose();
Eigen::Vector4d point_eigen = pose * p;
return cv::Point3f(point_eigen(0), point_eigen(1), point_eigen(2));
}
/*
cv::Point3f ftl::gui::Camera::getNormal(int x, int y) {
UNIQUE_LOCK(mutex_, lk);
LOG(INFO) << y << ", " << x;
if (x >= im_normals_.cols || y >= im_normals_.rows) { return cv::Point3f(); }
auto n = im_normals_f_.at<cv::Vec4f>(y, x);
return cv::Point3f(n[0], n[1], n[2]);
}
*/
void ftl::gui::Camera::setTransform(const Eigen::Matrix4d &T) {
T_ = T * T_;
}
Eigen::Matrix4d ftl::gui::Camera::getTransform() const {
return T_;
}
applications/gui/src/camera.hpp deleted 100644 → 0
View file @ e2e259ab
#ifndef _FTL_GUI_CAMERA_HPP_
#define _FTL_GUI_CAMERA_HPP_
#include <ftl/rgbd/frameset.hpp>
#include <ftl/render/CUDARender.hpp>
#include <ftl/render/overlay.hpp>
#include <ftl/codecs/writer.hpp>
#include "gltexture.hpp"
#include <ftl/streams/filestream.hpp>
#include <ftl/streams/netstream.hpp>
#include <ftl/streams/sender.hpp>
#include <ftl/codecs/faces.hpp>
#include <string>
#include <array>
#ifdef HAVE_OPENVR
#include <openvr/openvr.h>
#endif
class StatisticsImage;
namespace ftl {
namespace gui {
class Screen;
class PoseWindow;
class Camera {
public:
Camera(ftl::gui::Screen *screen, int fsmask, int fid, ftl::codecs::Channel chan=ftl::codecs::Channel::Colour);
~Camera();
Camera(const Camera &)=delete;
int width() const { return width_; }
int height() const { return height_; }
int getFramesetMask() const { return fsmask_; }
bool usesFrameset(int id) const { return fsmask_ & (1 << id); }
void setPose(const Eigen::Matrix4d &p);
void mouseMovement(int rx, int ry, int button);
void keyMovement(int key, int modifiers);
void showPoseWindow();
void showSettings();
void setChannel(ftl::codecs::Channel c);
const ftl::codecs::Channel getChannel() { return channel_; }
void togglePause();
void isPaused();
inline bool isVirtual() const { return fid_ == 255; }
const ftl::codecs::Channels<0> &availableChannels() { return channels_; }
inline bool isStereo() const { return stereo_; }
void setStereo(bool v);
/**
* Main function to obtain latest frames.
*/
void update(std::vector<ftl::rgbd::FrameSet *> &fss);
/**
* Update the available channels.
*/
void update(int fsid, const ftl::codecs::Channels<0> &c);
/**
* Draw virtual camera only if the frameset has been updated since last
* draw.
*/
void drawUpdated(std::vector<ftl::rgbd::FrameSet*> &fss);
void draw(std::vector<ftl::rgbd::FrameSet*> &fss);
void drawOverlay(const Eigen::Vector2f &);
inline int64_t getFrameTimeMS() const { return int64_t(delta_ * 1000.0f); }
const ftl::rgbd::Camera &getIntrinsics() const { return state_.getLeft(); }
const Eigen::Matrix4d getPose() const { UNIQUE_LOCK(mutex_, lk); return state_.getPose(); }
/**
* @internal. Used to inform the camera if it is the active camera or not.
*/
void active(bool);
const void captureFrame();
const GLTexture &getLeft() const { return texture1_; }
const GLTexture &getRight() const { return texture2_; }
const GLTexture &getDepth() const { return depth1_; }
void snapshot(const std::string &filename);
void startVideoRecording(const std::string &filename, const std::string &uri);
void stopVideoRecording();
//nlohmann::json getMetaData();
const std::string &name() const { return name_; }
StatisticsImage *stats_ = nullptr;
float getDepth(int x, int y);
float getDepth(float x, float y) { return getDepth((int) round(x), (int) round(y)); }
cv::Point3f getPoint(int x, int y);
cv::Point3f getPoint(float x, float y) { return getPoint((int) round(x), (int) round(y)); }
//cv::Point3f getNormal(int x, int y);
//cv::Point3f getNormal(float x, float y) { return getNormal((int) round(x), (int) round(y)); }
void setTransform(const Eigen::Matrix4d &T);
Eigen::Matrix4d getTransform() const;
const std::vector<std::string> &getMessages() const { return msgs_; }
#ifdef HAVE_OPENVR
bool isVR() { return vr_mode_; }
bool setVR(bool on);
#else
bool isVR() { return false; }
#endif
private:
Screen *screen_;
unsigned int fsmask_; // Frameset Mask
int fid_;
int width_;
int height_;
GLTexture texture1_; // first channel (always left at the moment)
GLTexture texture2_; // second channel ("right")
GLTexture depth1_;
ftl::gui::PoseWindow *posewin_;
//nlohmann::json meta_;
Eigen::Vector4d neye_;
Eigen::Vector3d eye_;
//Eigen::Vector3f orientation_;
Eigen::Matrix4d rotmat_;
float ftime_;
float delta_;
float lerpSpeed_;
bool sdepth_;
bool pause_;
bool do_snapshot_ = false;
std::string pose_source_;
std::string snapshot_filename_;
ftl::codecs::Channel channel_;
ftl::codecs::Channels<0> channels_;
cv::cuda::HostMem im_depth_;
//cv::cuda::HostMem im_normals_;
cv::Mat im_normals_f_;
cv::Mat overlay_; // first channel (left)
bool stereo_;
std::atomic_flag stale_frame_;
int rx_;
int ry_;
std::vector<ftl::rgbd::FrameSet*> *framesets_;
ftl::render::CUDARender *renderer_;
ftl::render::CUDARender *renderer2_;
ftl::render::Colouriser *colouriser_;
ftl::overlay::Overlay *overlayer_;
ftl::Configurable *intrinsics_;
ftl::operators::Graph *post_pipe_;
ftl::rgbd::Frame frame_;
ftl::rgbd::FrameState state_;
ftl::stream::File *file_stream_;
ftl::stream::Net *net_stream_;
ftl::stream::Broadcast *record_stream_;
ftl::stream::Sender *record_sender_;
std::string name_;
std::vector<std::string> msgs_;
int transform_ix_;
std::array<Eigen::Matrix4d,ftl::stream::kMaxStreams> transforms_; // Frameset transforms for virtual cam
Eigen::Matrix4d T_ = Eigen::Matrix4d::Identity();
mutable MUTEX mutex_;
#ifdef HAVE_OPENVR
vr::TrackedDevicePose_t rTrackedDevicePose_[ vr::k_unMaxTrackedDeviceCount ];
bool vr_mode_;
float baseline_;
#endif
void _downloadFrames(ftl::cuda::TextureObject<uchar4> &, ftl::cuda::TextureObject<uchar4> &);
void _downloadFrames(ftl::cuda::TextureObject<uchar4> &);
void _downloadFrames();
void _draw(std::vector<ftl::rgbd::FrameSet*> &fss);
void _applyPoseEffects(std::vector<ftl::rgbd::FrameSet*> &fss);
std::pair<const ftl::rgbd::Frame *, const ftl::codecs::Face *> _selectFace(std::vector<ftl::rgbd::FrameSet*> &fss);
void _generateWindow(const ftl::rgbd::Frame &, const ftl::codecs::Face &face, Eigen::Matrix4d &pose_adjust, ftl::render::ViewPort &vp);
};
}
}
#endif // _FTL_GUI_CAMERA_HPP_
applications/gui/src/ctrl_window.cpp deleted 100644 → 0
View file @ e2e259ab
#include "ctrl_window.hpp"
#include "config_window.hpp"
#include <nanogui/layout.h>
#include <nanogui/label.h>
#include <nanogui/combobox.h>
#include <nanogui/button.h>
#include <nanogui/entypo.h>
#include <vector>
#include <string>
using ftl::gui::ControlWindow;
using ftl::gui::ConfigWindow;
using std::string;
using std::vector;
ControlWindow::ControlWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
: nanogui::Window(parent, "Network Connections"), ctrl_(ctrl) {
setLayout(new nanogui::GroupLayout());
using namespace nanogui;
_updateDetails();
auto tools = new Widget(this);
tools->setLayout(new BoxLayout( Orientation::Horizontal,
Alignment::Middle, 0, 6));
auto button = new Button(tools, "", ENTYPO_ICON_PLUS);
button->setCallback([this] {
// Show new connection dialog
_addNode();
});
button->setTooltip("Add new node");
// commented-out buttons not working/useful
/*
button = new Button(tools, "", ENTYPO_ICON_CYCLE);
button->setCallback([this] {
ctrl_->restart();
});
button = new Button(tools, "", ENTYPO_ICON_CONTROLLER_PAUS);
button->setCallback([this] {
ctrl_->pause();
});
button->setTooltip("Pause all nodes");*/
new Label(this, "Select Node","sans-bold");
auto select = new ComboBox(this, node_titles_);
select->setCallback([this](int ix) {
//LOG(INFO) << "Change node: " << ix;
_changeActive(ix);
});
new Label(this, "Node Options","sans-bold");
tools = new Widget(this);
tools->setLayout(new BoxLayout( Orientation::Horizontal,
Alignment::Middle, 0, 6));
/*button = new Button(tools, "", ENTYPO_ICON_INFO);
button->setCallback([this] {
});
button->setTooltip("Node status information");*/
button = new Button(tools, "", ENTYPO_ICON_COG);
button->setCallback([this,parent] {
auto cfgwin = new ConfigWindow(parent, ctrl_);
cfgwin->setTheme(theme());
});
button->setTooltip("Edit node configuration");
/*button = new Button(tools, "", ENTYPO_ICON_CYCLE);
button->setCallback([this] {
ctrl_->restart(_getActiveID());
});
button->setTooltip("Restart this node");*/
/*button = new Button(tools, "", ENTYPO_ICON_CONTROLLER_PAUS);
button->setCallback([this] {
ctrl_->pause(_getActiveID());
});
button->setTooltip("Pause node processing");*/
ctrl->getNet()->onConnect([this,select](ftl::net::Peer *p) {
_updateDetails();
select->setItems(node_titles_);
});
_changeActive(0);
}
ControlWindow::~ControlWindow() {
}
void ControlWindow::_addNode() {
using namespace nanogui;
FormHelper *form = new FormHelper(this->screen());
form->addWindow(Vector2i(100,100), "Add Node");
auto var = form->addVariable("URI", add_node_uri_);
var->setValue("tcp://localhost:9001");
var->setFixedWidth(200);
form->addButton("Add", [this,form](){
ctrl_->getNet()->connect(add_node_uri_);
form->window()->setVisible(false);
delete form;
})->setIcon(ENTYPO_ICON_PLUS);
form->addButton("Close", [form]() {
form->window()->setVisible(false);
delete form;
})->setIcon(ENTYPO_ICON_CROSS);
}
void ControlWindow::_updateDetails() {
node_details_ = ctrl_->getControllers();
node_titles_.clear();
for (auto &d : node_details_) {
node_titles_.push_back(d["title"].get<string>());
}
}
void ControlWindow::_changeActive(int ix) {
active_ix_ = ix;
}
ftl::UUID ControlWindow::_getActiveID() {
return ftl::UUID(node_details_[active_ix_]["id"].get<string>());
}