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

Calibrate class cleanup

parent 3c8f3cf3
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
.gitlab-ci.yml
+ 34
− 15
View file @ e3cc1912
......@@ -4,15 +4,14 @@
#
# Perhaps relevant in future https://gitlab.com/gitlab-org/gitlab-ce/issues/47063
variables:
CMAKE_ARGS_WINDOWS: '-DCMAKE_GENERATOR_PLATFORM=x64 -DNVPIPE_DIR="D:/Build/NvPipe" -DEigen3_DIR="C:/Program Files (x86)/Eigen3/share/eigen3/cmake" -DOpenCV_DIR="D:/Build/opencv-4.1.1" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1"'
stages:
- all
# - build
# - test
- deploy
#cache:
# paths:
# - build/
#### Linux
linux:
stage: all
......@@ -43,20 +42,40 @@ webserver-deploy:
- rsync -vr --delete web-service/ nodejs@${NODE_SERVER}:/srv/nodejs/web-service
- ssh nodejs@${NODE_SERVER} -- "npm install web-service/server && pm2 restart web-service"
windows:
stage: all
variables:
CMAKE_ARGS: '-DWITH_OPTFLOW=TRUE -DWITH_PCL=FALSE -DCMAKE_GENERATOR_PLATFORM=x64 -DNVPIPE_DIR="D:/Build/NvPipe" -DEigen3_DIR="C:/Program Files (x86)/Eigen3/share/eigen3/cmake" -DOpenCV_DIR="D:/Build/opencv-4.1.1" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1"'
DEPLOY_DIR: 'D:/Shared/AutoDeploy'
tags:
- win
script:
### Windows
.build-windows: &build-windows
- 'call "C:/Program Files (x86)/Microsoft Visual Studio/2017/Community/VC/Auxiliary/Build/vcvars64.bat"'
- mkdir build
- cd build
- cmake %CMAKE_ARGS% ..
- echo cmake %CMAKE_ARGS% %CMAKE_ARGS_WINDOWS% ..
- cmake %CMAKE_ARGS% %CMAKE_ARGS_WINDOWS% ..
- devenv ftl.utu.fi.sln /build Release
- rmdir /q /s "%DEPLOY_DIR%/%CI_COMMIT_REF_SLUG%"
- mkdir "%DEPLOY_DIR%/%CI_COMMIT_REF_SLUG%"
- 'copy "applications\vision\Release\ftl-vision.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
- 'copy "applications\calibration\Release\ftl-calibrate.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
\ No newline at end of file
- 'copy "applications\calibration\Release\ftl-calibrate.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
windows-vision:
except:
- master
stage: all
variables:
CMAKE_ARGS: '-DWITH_OPTFLOW=TRUE -DBUILD_VISION=TRUE -DBUILD_CALIBRATION=FALSE -DBUILDRECONSTRUCT=FALSE -DBUILDRENDERER=FALSE -DBUILD_TESTING=FALSE'
DEPLOY_DIR: 'D:/Shared/AutoDeploy'
tags:
- win
script:
- *build-windows
windows-master:
only:
- master
stage: all
variables:
CMAKE_ARGS: '-DWITH_OPTFLOW=TRUE'
DEPLOY_DIR: 'D:/Shared/AutoDeploy'
tags:
- win
script:
- *build-windows
components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
+ 111
− 135
View file @ e3cc1912
......@@ -13,6 +13,7 @@
#include <ctime>
#include "calibrate.hpp"
#include "ftl/exception.hpp"
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
......@@ -43,26 +44,23 @@ using std::string;
using std::vector;
Calibrate::Calibrate(nlohmann::json &config, cv::Size image_size, cv::cuda::Stream &stream) : ftl::Configurable(config) {
std::pair<Mat, Mat> map1, map2;
calibrated_ = _loadCalibration(image_size, map1, map2);
if (calibrated_) {
_updateIntrinsics();
LOG(INFO) << "Calibration loaded from file";
}
else {
LOG(WARNING) << "Calibration not loaded";
if (!_loadCalibration()) {
throw ftl::exception("Loading calibration failed");
}
_calculateRectificationParameters(image_size);
LOG(INFO) << "Calibration loaded from file";
rectify_ = value("rectify", true);;
this->on("use_intrinsics", [this](const ftl::config::Event &e) {
_updateIntrinsics();
rectify_ = value("rectify", true);
});
}
bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, std::pair<Mat, Mat> &map2) {
bool Calibrate::_loadCalibration() {
FileStorage fs;
// reading intrinsic parameters
// read intrinsic parameters
auto ifile = ftl::locateFile(value("intrinsics", std::string("intrinsics.yml")));
if (ifile) {
fs.open((*ifile).c_str(), FileStorage::READ);
......@@ -78,131 +76,90 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
return false;
}
fs["K"] >> K_;
fs["D"] >> D_;
fs["resolution"] >> calib_size_;
cv::Size calib_size;
{
vector<Mat> K, D;
fs["K"] >> K;
fs["D"] >> D;
fs["resolution"] >> calib_size;
K[0].copyTo(K1_);
K[1].copyTo(K2_);
D[0].copyTo(D1_);
D[1].copyTo(D2_);
if ((K_.size() != 2) || (D_.size() != 2)) {
LOG(ERROR) << "Not enough intrinsic paramters, expected 2";
return false;
}
fs.release();
CHECK(K1_.size() == Size(3, 3));
CHECK(K2_.size() == Size(3, 3));
CHECK(D1_.size() == Size(5, 1));
CHECK(D2_.size() == Size(5, 1));
if (calib_size_.empty()) {
LOG(ERROR) << "Calibration resolution missing";
return false;
}
for (const Mat &K : K_) {
if (K.size() != Size(3, 3)) {
LOG(ERROR) << "Invalid intrinsic parameters";
return false;
}
}
for (const Mat &D : D_) {
if (D.size() != Size(5, 1)) {
LOG(ERROR) << "Invalid intrinsic parameters";
return false;
}
}
// read extrinsic parameters
auto efile = ftl::locateFile(value("extrinsics", std::string("extrinsics.yml")));
if (efile) {
fs.open((*efile).c_str(), FileStorage::READ);
if (!fs.isOpened()) {
LOG(WARNING) << "Could not open extrinsics file";
LOG(ERROR) << "Could not open extrinsics file";
return false;
}
LOG(INFO) << "Extrinsics from: " << *efile;
}
else {
LOG(WARNING) << "Calibration extrinsics file not found";
LOG(ERROR) << "Calibration extrinsics file not found";
return false;
}
fs["R"] >> R_;
fs["T"] >> T_;
/* re-calculate rectification from camera parameters
fs["R1"] >> R1_;
fs["R2"] >> R2_;
fs["P1"] >> P1_;
fs["P2"] >> P2_;
fs["Q"] >> Q_;
*/
fs.release();
fs["pose"] >> pose_;
img_size_ = img_size;
if (pose_.size() != Size(4, 4)) {
LOG(ERROR) << "Pose not in calibration (using identity)";
// TODO: return false (raises exception in constructor)
// use config option to make pose optional (and not return false)
if (calib_size.empty())
{
LOG(WARNING) << "Calibration resolution missing!";
pose_ = Mat::eye(Size(4, 4), CV_64FC1);
}
else
{
double scale_x = ((double) img_size.width) / ((double) calib_size.width);
double scale_y = ((double) img_size.height) / ((double) calib_size.height);
Mat scale(cv::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;
K1_ = scale * K1_;
K2_ = scale * K2_;
}
double alpha = value("alpha", 0.0);
cv::stereoRectify(K1_, D1_, K2_, D2_, img_size_, R_, T_, R1_, R2_, P1_, P2_, Q_, 0, alpha);
if ((R_.size() != Size(3, 3)) ||
(T_.size() != Size(1, 3))) {
/* scaling not required as rectification is performed from scaled values
Q_.at<double>(0, 3) = Q_.at<double>(0, 3) * scale_x;
Q_.at<double>(1, 3) = Q_.at<double>(1, 3) * scale_y;
Q_.at<double>(2, 3) = Q_.at<double>(2, 3) * scale_x; // TODO: scaling?
Q_.at<double>(3, 3) = Q_.at<double>(3, 3) * scale_x;
*/
// cv::cuda::remap() works only with CV_32FC1
initUndistortRectifyMap(K1_, D1_, R1_, P1_, img_size_, CV_32FC1, map1.first, map2.first);
initUndistortRectifyMap(K2_, D2_, R2_, P2_, img_size_, CV_32FC1, map1.second, map2.second);
LOG(ERROR) << "Invalid extrinsic parameters";
return false;
}
fs.release();
return true;
}
void Calibrate::updateCalibration(const ftl::rgbd::Camera &p) {
Q_.at<double>(3, 2) = 1.0 / p.baseline;
Q_.at<double>(2, 3) = p.fx;
Q_.at<double>(0, 3) = p.cx;
Q_.at<double>(1, 3) = p.cy;
// FIXME:(Nick) Update camera matrix also...
_updateIntrinsics();
}
void Calibrate::_updateIntrinsics() {
// TODO: pass parameters?
Mat R1, R2, P1, P2;
ftl::rgbd::Camera params();
if (this->value("use_intrinsics", true)) {
// rectify
R1 = R1_;
R2 = R2_;
P1 = P1_;
P2 = P2_;
}
else {
// no rectification
R1 = Mat::eye(Size(3, 3), CV_64FC1);
R2 = R1;
P1 = Mat::zeros(Size(4, 3), CV_64FC1);
P2 = Mat::zeros(Size(4, 3), CV_64FC1);
K1_.copyTo(Mat(P1, cv::Rect(0, 0, 3, 3)));
K2_.copyTo(Mat(P2, cv::Rect(0, 0, 3, 3)));
}
// Set correct camera matrices for
// getCameraMatrix(), getCameraMatrixLeft(), getCameraMatrixRight()
Kl_ = Mat(P1, cv::Rect(0, 0, 3, 3));
Kr_ = Mat(P2, cv::Rect(0, 0, 3, 3));
void Calibrate::_calculateRectificationParameters(Size img_size) {
img_size_ = img_size;
Mat K1 = _getK(0, img_size);
Mat D1 = D_[0];
Mat K2 = _getK(1, img_size);
Mat D2 = D_[1];
double alpha = value("alpha", 0.0);
initUndistortRectifyMap(K1_, D1_, R1, P1, img_size_, CV_32FC1, map1_.first, map2_.first);
initUndistortRectifyMap(K2_, D2_, R2, P2, img_size_, CV_32FC1, map1_.second, map2_.second);
cv::stereoRectify( K1, D1, K2, D2,
img_size, R_, T_,
R1_, R2_, P1_, P2_, Q_, 0, alpha);
// TODO use fixed point maps for CPU (gpu remap() requires floating point)
initUndistortRectifyMap(K1, D1, R1_, P1_, img_size, CV_32FC1, map1_.first, map2_.first);
initUndistortRectifyMap(K2, D2, R2_, P2_, img_size, CV_32FC1, map1_.second, map2_.second);
// CHECK Is this thread safe!!!!
map1_gpu_.first.upload(map1_.first);
......@@ -211,29 +168,19 @@ void Calibrate::_updateIntrinsics() {
map2_gpu_.second.upload(map2_.second);
}
cv::Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
double scale_x = ((double) res.width) / ((double) img_size_.width);
double scale_y = ((double) res.height) / ((double) img_size_.height);
Mat scale(cv::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 * Kl_;
}
void Calibrate::updateCalibration(const ftl::rgbd::Camera &p) {
Q_.at<double>(3, 2) = 1.0 / p.baseline;
Q_.at<double>(2, 3) = p.fx;
Q_.at<double>(0, 3) = p.cx;
Q_.at<double>(1, 3) = p.cy;
cv::Mat Calibrate::getCameraMatrixRight(const cv::Size res) {
double scale_x = ((double) res.width) / ((double) img_size_.width);
double scale_y = ((double) res.height) / ((double) img_size_.height);
Mat scale(cv::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 * Kr_;
// FIXME:(Nick) Update camera matrix also...
}
void Calibrate::rectifyStereo(GpuMat &l, GpuMat &r, Stream &stream) {
if (!rectify_) { return; }
// cv::cuda::remap() can not use same Mat for input and output
// TODO: create tmp buffers only once
GpuMat l_tmp(l.size(), l.type());
GpuMat r_tmp(r.size(), r.type());
cv::cuda::remap(l, l_tmp, map1_gpu_.first, map2_gpu_.first, cv::INTER_LINEAR, 0, cv::Scalar(), stream);
......@@ -244,20 +191,49 @@ void Calibrate::rectifyStereo(GpuMat &l, GpuMat &r, Stream &stream) {
}
void Calibrate::rectifyStereo(cv::Mat &l, cv::Mat &r) {
if (!rectify_) { return; }
// cv::cuda::remap() can not use same Mat for input and output
cv::remap(l, l, map1_.first, map2_.first, cv::INTER_LINEAR, 0, cv::Scalar());
cv::remap(r, r, map1_.second, map2_.second, cv::INTER_LINEAR, 0, cv::Scalar());
}
/*GpuMat l_tmp(l.size(), l.type());
GpuMat r_tmp(r.size(), r.type());
cv::cuda::remap(l, l_tmp, map1_gpu_.first, map2_gpu_.first, cv::INTER_LINEAR, 0, cv::Scalar(), stream);
cv::cuda::remap(r, r_tmp, map1_gpu_.second, map2_gpu_.second, cv::INTER_LINEAR, 0, cv::Scalar(), stream);
stream.waitForCompletion();
l = l_tmp;
r = r_tmp;*/
static Mat scaleCameraIntrinsics(Mat K, Size size_new, Size size_old) {
Mat S(cv::Size(3, 3), CV_64F, 0.0);
double scale_x = ((double) size_new.width) / ((double) size_old.width);
double scale_y = ((double) size_new.height) / ((double) size_old.height);
S.at<double>(0, 0) = scale_x;
S.at<double>(1, 1) = scale_y;
S.at<double>(2, 2) = 1.0;
return S * K;
}
Mat Calibrate::_getK(size_t idx, Size size) {
CHECK(idx < K_.size());
CHECK(!size.empty());
return scaleCameraIntrinsics(K_[idx], size, calib_size_);
}
bool Calibrate::isCalibrated() {
return calibrated_;
}
\ No newline at end of file
Mat Calibrate::_getK(size_t idx) {
return _getK(idx, img_size_);
}
cv::Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
Mat M;
if (rectify_) {
M = Mat(P1_, cv::Rect(0, 0, 3, 3));
} else {
M = K_[0];
}
return scaleCameraIntrinsics(M, res, img_size_);
}
cv::Mat Calibrate::getCameraMatrixRight(const cv::Size res) {
Mat M;
if (rectify_) {
M = Mat(P2_, cv::Rect(0, 0, 3, 3));
} else {
M = K_[1];
}
return scaleCameraIntrinsics(M, res, img_size_);
}
components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+ 63
− 27
View file @ e3cc1912
......@@ -22,64 +22,100 @@ namespace rgbd {
namespace detail {
/**
* Manage local calibration details: undistortion, rectification and camera parameters.
* Calibration parameters can be acquired using ftl-calibrate app.
* Manage local calibration details: undistortion, rectification and camera
* parameters.
*/
class Calibrate : public ftl::Configurable {
public:
Calibrate(nlohmann::json &config, cv::Size image_size, cv::cuda::Stream &stream);
/**
* Get both left and right images from local source, but with intrinsic
* and extrinsic stereo calibration already applied.
*/
bool rectified(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::Stream &stream);
/**
* Rectify and remove distortions from from images l and r using cv::remap()
/* @brief Rectify and undistort stereo pair images (GPU)
*/
void rectifyStereo(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::Stream &stream);
/**
* Rectify and remove distortions from from images l and r using cv::remap()
/* @brief Rectify and undistort stereo pair images (CPU)
* @todo Uses same rectification maps as GPU version, according to OpenCV
* documentation for remap(), fixed point versions faster for CPU
*/
void rectifyStereo(cv::Mat &l, cv::Mat &r);
bool isCalibrated();
void updateCalibration(const ftl::rgbd::Camera &p);
// Get disparity to depth matrix.
/* @brief Get disparity to depth matrix
*
* 2020/01/15: Not used, StereoVideoSource creates a Camera object which
* is used to calculate depth from disparity (disp2depth.cu)
*/
const cv::Mat &getQ() const { return Q_; }
/* @brief Get intrinsic paramters for rectified camera
* @param Camera resolution
*/
cv::Mat getCameraMatrixLeft(const cv::Size res);
cv::Mat getCameraMatrixRight(const cv::Size res);
/* @brief Get camera pose from calibration
*/
cv::Mat getPose() { return pose_; };
/* @brief Enable/disable recitification. If disabled, instance returns
* original camera intrinsic parameters (getCameraMatrixLeft() and
getCameraMatrixRight() methods). When enabled (default), those
methods return camera parameters for rectified images.
* @param Rectification on/off
*/
void setRectify(bool enabled) { rectify_ = enabled; }
private:
void _updateIntrinsics();
bool _loadCalibration(cv::Size img_size, std::pair<cv::Mat, cv::Mat> &map1, std::pair<cv::Mat, cv::Mat> &map2);
// rectification enabled/disabled
bool rectify_;
/* @brief Get intrinsic matrix saved in calibration.
* @param Camera index (0 left, 1 right)
* @param Resolution
*/
cv::Mat _getK(size_t idx, cv::Size size);
cv::Mat _getK(size_t idx);
/* @brief Calculate rectification parameters and maps
* @param Camera resolution
*/
void _calculateRectificationParameters(cv::Size img_size);
/* @brief Load calibration from file
* @todo File names as arguments
*/
bool _loadCalibration();
private:
bool calibrated_;
// calibration resolution (loaded from file by _loadCalibration)
cv::Size calib_size_;
// camera resolution (set by _calculateRecitificationParameters)
cv::Size img_size_;
// rectification maps
std::pair<cv::Mat, cv::Mat> map1_;
std::pair<cv::Mat, cv::Mat> map2_;
std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map1_gpu_;
std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map2_gpu_;
// transformation from left to right camera: R_ and T_
cv::Mat R_;
cv::Mat T_;
// pose for left camera
cv::Mat pose_;
// parameters for rectification, see cv::stereoRectify() documentation
cv::Mat R_, T_, R1_, P1_, R2_, P2_;
cv::Mat R1_;
cv::Mat P1_;
cv::Mat R2_;
cv::Mat P2_;
// disparity to depth matrix
cv::Mat Q_;
// intrinsic paramters and distortion coefficients
cv::Mat K1_, D1_, K2_, D2_;
cv::Mat Kl_;
cv::Mat Kr_;
cv::Size img_size_;
// intrinsic parameters and distortion coefficients
std::vector<cv::Mat> K_;
std::vector<cv::Mat> D_;
};
}
......
components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+ 10
− 5
View file @ e3cc1912
......@@ -96,8 +96,7 @@ void StereoVideoSource::init(const string &file) {
pipeline_depth_->append<ftl::operators::AggreMLS>("mls"); // Perform MLS (using smoothing channel)*/
calib_ = ftl::create<Calibrate>(host_, "calibration", cv::Size(lsrc_->fullWidth(), lsrc_->fullHeight()), stream_);
if (!calib_->isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
// Generate camera parameters from camera matrix
cv::Mat K = calib_->getCameraMatrixLeft(color_size_);
params_ = {
......@@ -121,7 +120,10 @@ void StereoVideoSource::init(const string &file) {
host_->getConfig()["baseline"] = params_.baseline;
host_->getConfig()["doffs"] = params_.doffs;
// Add event handlers to allow calibration changes...
// TODO: remove (not used, fx/fy/baseline/.. do not change)
// in case they are modified, update using Calibrate
// (requires new method)
host_->on("baseline", [this](const ftl::config::Event &e) {
params_.baseline = host_->value("baseline", params_.baseline);
UNIQUE_LOCK(host_->mutex(), lk);
......@@ -134,13 +136,15 @@ void StereoVideoSource::init(const string &file) {
UNIQUE_LOCK(host_->mutex(), lk);
calib_->updateCalibration(params_);
});
//
host_->on("doffs", [this](const ftl::config::Event &e) {
params_.doffs = host_->value("doffs", params_.doffs);
});
// left and right masks (areas outside rectified images)
// only left mask used (not used)
// TODO: remove mask
cv::cuda::GpuMat mask_r_gpu(lsrc_->height(), lsrc_->width(), CV_8U, 255);
cv::cuda::GpuMat mask_l_gpu(lsrc_->height(), lsrc_->width(), CV_8U, 255);
calib_->rectifyStereo(mask_l_gpu, mask_r_gpu, stream_);
......@@ -149,6 +153,7 @@ void StereoVideoSource::init(const string &file) {
mask_l_gpu.download(mask_l);
mask_l_ = (mask_l == 0);
LOG(INFO) << "StereoVideo source ready...";
ready_ = true;
}
......@@ -162,7 +167,7 @@ ftl::rgbd::Camera StereoVideoSource::parameters(Channel chan) {
K = calib_->getCameraMatrixLeft(color_size_);
}
// TODO: remove hardcoded values (min/max)
// TODO: remove hardcoded values (min/max), move to Calibrate?
ftl::rgbd::Camera params = {
K.at<double>(0,0), // Fx
K.at<double>(1,1), // Fy
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment