From c971d70337f6981e5561e228de28913f83897575 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Tue, 12 Mar 2019 10:48:19 +0200
Subject: [PATCH] Potentially working calibration code
---
cv-node/CMakeLists.txt | 2 +-
cv-node/include/ftl/calibrate.hpp | 73 +++-
cv-node/include/ftl/local.hpp | 4 +-
cv-node/src/calibrate.cpp | 555 +++++++++++++-----------------
cv-node/src/local.cpp | 63 +++-
cv-node/src/main.cpp | 4 +-
6 files changed, 372 insertions(+), 329 deletions(-)
diff --git a/cv-node/CMakeLists.txt b/cv-node/CMakeLists.txt
index 17f4d3d72..a5dfa5f78 100644
--- a/cv-node/CMakeLists.txt
+++ b/cv-node/CMakeLists.txt
@@ -33,7 +33,7 @@ endif (WIN32)
if (UNIX)
add_definitions(-DUNIX)
- set(FTL_CONFIG_ROOT "$ENV{HOME}/.config/ftl")
+ set(FTL_CONFIG_ROOT "\"$ENV{HOME}/.config/ftl\"")
set(FTL_CACHE_ROOT "$ENV{HOME}/.cache/ftl")
set(FTL_DATA_ROOT "$ENV{HOME}/.local/share/ftl")
endif (UNIX)
diff --git a/cv-node/include/ftl/calibrate.hpp b/cv-node/include/ftl/calibrate.hpp
index fee7647e2..e8d450828 100644
--- a/cv-node/include/ftl/calibrate.hpp
+++ b/cv-node/include/ftl/calibrate.hpp
@@ -4,13 +4,70 @@
#include <opencv2/opencv.hpp>
#include <ftl/local.hpp>
#include <string>
+#include <vector>
+
+namespace cv {
+class FileStorage;
+class FileNode;
+};
namespace ftl {
class Calibrate {
+ public:
+ class Settings {
+ public:
+ Settings() : goodInput(false) {}
+ enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
+ enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST };
+
+ void write(cv::FileStorage& fs) const;
+ void read(const cv::FileNode& node);
+ void validate();
+ //Mat nextImage();
+
+ static bool readStringList( const std::string& filename, std::vector<std::string>& l );
+
+ static bool isListOfImages( const std::string& filename);
+ public:
+ cv::Size boardSize; // The size of the board -> Number of items by width and height
+ Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric circle pattern
+ float squareSize; // The size of a square in your defined unit (point, millimeter,etc).
+ int nrFrames; // The number of frames to use from the input for calibration
+ float aspectRatio; // The aspect ratio
+ int delay; // In case of a video input
+ bool writePoints; // Write detected feature points
+ bool writeExtrinsics; // Write extrinsic parameters
+ bool writeGrid; // Write refined 3D target grid points
+ bool calibZeroTangentDist; // Assume zero tangential distortion
+ bool calibFixPrincipalPoint; // Fix the principal point at the center
+ bool flipVertical; // Flip the captured images around the horizontal axis
+ std::string outputFileName; // The name of the file where to write
+ bool showUndistorsed; // Show undistorted images after calibration
+ std::string input; // The input ->
+ bool useFisheye; // use fisheye camera model for calibration
+ bool fixK1; // fix K1 distortion coefficient
+ bool fixK2; // fix K2 distortion coefficient
+ bool fixK3; // fix K3 distortion coefficient
+ bool fixK4; // fix K4 distortion coefficient
+ bool fixK5; // fix K5 distortion coefficient
+
+ int cameraID;
+ std::vector<std::string> imageList;
+ size_t atImageList;
+ //cv::VideoCapture inputCapture;
+ InputType inputType;
+ bool goodInput;
+ int flag;
+
+ private:
+ std::string patternToUse;
+
+
+ };
public:
Calibrate(ftl::LocalSource *s, const std::string &cal);
- bool recalibrate(const std::string &conf);
+ bool recalibrate();
bool undistort(cv::Mat &l, cv::Mat &r);
bool rectified(cv::Mat &l, cv::Mat &r);
@@ -19,11 +76,25 @@ class Calibrate {
private:
bool runCalibration(cv::Mat &img, cv::Mat &cam);
+ bool _recalibrate(size_t cam);
+ cv::Mat _nextImage(size_t cam);
private:
ftl::LocalSource *local_;
+ Settings settings_;
+ bool calibrated_;
+ std::vector<cv::Mat> map1_;
+ std::vector<cv::Mat> map2_;
};
};
+/*static inline void read(const cv::FileNode& node, ftl::Calibrate::Settings& x, const ftl::Calibrate::Settings& default_value = ftl::Calibrate::Settings())
+{
+ if(node.empty())
+ x = default_value;
+ else
+ x.read(node);
+}*/
+
#endif // _FTL_CALIBRATION_HPP_
diff --git a/cv-node/include/ftl/local.hpp b/cv-node/include/ftl/local.hpp
index 54f3f6916..3ab86a2fb 100644
--- a/cv-node/include/ftl/local.hpp
+++ b/cv-node/include/ftl/local.hpp
@@ -14,8 +14,8 @@ class LocalSource {
LocalSource();
LocalSource(const std::string &vid);
- //bool left(cv::Mat &m);
- //bool right(cv::Mat &m);
+ bool left(cv::Mat &m);
+ bool right(cv::Mat &m);
bool get(cv::Mat &l, cv::Mat &r);
//void setFramerate(float fps);
diff --git a/cv-node/src/calibrate.cpp b/cv-node/src/calibrate.cpp
index 16e88d7db..a659e659f 100644
--- a/cv-node/src/calibrate.cpp
+++ b/cv-node/src/calibrate.cpp
@@ -14,336 +14,238 @@
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
+#include <glog/logging.h>
+
using namespace cv;
using namespace std;
using ftl::Calibrate;
-class Settings
-{
-public:
- Settings() : goodInput(false) {}
- enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
- enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST };
- void write(FileStorage& fs) const //Write serialization for this class
+void Calibrate::Settings::write(FileStorage& fs) const //Write serialization for this class
+{
+ fs << "{"
+ << "BoardSize_Width" << boardSize.width
+ << "BoardSize_Height" << boardSize.height
+ << "Square_Size" << squareSize
+ << "Calibrate_Pattern" << patternToUse
+ << "Calibrate_NrOfFrameToUse" << nrFrames
+ << "Calibrate_FixAspectRatio" << aspectRatio
+ << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
+ << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint
+
+ << "Write_DetectedFeaturePoints" << writePoints
+ << "Write_extrinsicParameters" << writeExtrinsics
+ << "Write_gridPoints" << writeGrid
+ //<< "Write_outputFileName" << outputFileName
+
+ //<< "Show_UndistortedImage" << showUndistorsed
+
+ << "Input_FlipAroundHorizontalAxis" << flipVertical
+ << "Input_Delay" << delay
+ //<< "Input" << input
+ << "}";
+}
+void Calibrate::Settings::read(const FileNode& node) //Read serialization for this class
+{
+ node["BoardSize_Width" ] >> boardSize.width;
+ node["BoardSize_Height"] >> boardSize.height;
+ node["Calibrate_Pattern"] >> patternToUse;
+ node["Square_Size"] >> squareSize;
+ node["Calibrate_NrOfFrameToUse"] >> nrFrames;
+ node["Calibrate_FixAspectRatio"] >> aspectRatio;
+ node["Write_DetectedFeaturePoints"] >> writePoints;
+ node["Write_extrinsicParameters"] >> writeExtrinsics;
+ node["Write_gridPoints"] >> writeGrid;
+ //node["Write_outputFileName"] >> outputFileName;
+ node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist;
+ node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint;
+ node["Calibrate_UseFisheyeModel"] >> useFisheye;
+ node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
+ //node["Show_UndistortedImage"] >> showUndistorsed;
+ //node["Input"] >> input;
+ node["Input_Delay"] >> delay;
+ node["Fix_K1"] >> fixK1;
+ node["Fix_K2"] >> fixK2;
+ node["Fix_K3"] >> fixK3;
+ node["Fix_K4"] >> fixK4;
+ node["Fix_K5"] >> fixK5;
+
+ validate();
+}
+void Calibrate::Settings::validate()
+{
+ goodInput = true;
+ if (boardSize.width <= 0 || boardSize.height <= 0)
{
- fs << "{"
- << "BoardSize_Width" << boardSize.width
- << "BoardSize_Height" << boardSize.height
- << "Square_Size" << squareSize
- << "Calibrate_Pattern" << patternToUse
- << "Calibrate_NrOfFrameToUse" << nrFrames
- << "Calibrate_FixAspectRatio" << aspectRatio
- << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
- << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint
-
- << "Write_DetectedFeaturePoints" << writePoints
- << "Write_extrinsicParameters" << writeExtrinsics
- << "Write_gridPoints" << writeGrid
- << "Write_outputFileName" << outputFileName
-
- << "Show_UndistortedImage" << showUndistorsed
-
- << "Input_FlipAroundHorizontalAxis" << flipVertical
- << "Input_Delay" << delay
- << "Input" << input
- << "}";
+ LOG(ERROR) << "Invalid Board size: " << boardSize.width << " " << boardSize.height;
+ goodInput = false;
}
- void read(const FileNode& node) //Read serialization for this class
+ if (squareSize <= 10e-6)
{
- node["BoardSize_Width" ] >> boardSize.width;
- node["BoardSize_Height"] >> boardSize.height;
- node["Calibrate_Pattern"] >> patternToUse;
- node["Square_Size"] >> squareSize;
- node["Calibrate_NrOfFrameToUse"] >> nrFrames;
- node["Calibrate_FixAspectRatio"] >> aspectRatio;
- node["Write_DetectedFeaturePoints"] >> writePoints;
- node["Write_extrinsicParameters"] >> writeExtrinsics;
- node["Write_gridPoints"] >> writeGrid;
- node["Write_outputFileName"] >> outputFileName;
- node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist;
- node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint;
- node["Calibrate_UseFisheyeModel"] >> useFisheye;
- node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
- node["Show_UndistortedImage"] >> showUndistorsed;
- node["Input"] >> input;
- node["Input_Delay"] >> delay;
- node["Fix_K1"] >> fixK1;
- node["Fix_K2"] >> fixK2;
- node["Fix_K3"] >> fixK3;
- node["Fix_K4"] >> fixK4;
- node["Fix_K5"] >> fixK5;
-
- validate();
+ LOG(ERROR) << "Invalid square size " << squareSize;
+ goodInput = false;
}
- void validate()
+ if (nrFrames <= 0)
{
- goodInput = true;
- if (boardSize.width <= 0 || boardSize.height <= 0)
- {
- cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << endl;
- goodInput = false;
- }
- if (squareSize <= 10e-6)
- {
- cerr << "Invalid square size " << squareSize << endl;
- goodInput = false;
- }
- if (nrFrames <= 0)
- {
- cerr << "Invalid number of frames " << nrFrames << endl;
- goodInput = false;
- }
-
- if (input.empty()) // Check for valid input
- inputType = INVALID;
- else
- {
- if (input[0] >= '0' && input[0] <= '9')
- {
- stringstream ss(input);
- ss >> cameraID;
- inputType = CAMERA;
- }
- else
- {
- if (isListOfImages(input) && readStringList(input, imageList))
- {
- inputType = IMAGE_LIST;
- nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames : (int)imageList.size();
- }
- else
- inputType = VIDEO_FILE;
- }
- if (inputType == CAMERA)
- inputCapture.open(cameraID);
- if (inputType == VIDEO_FILE)
- inputCapture.open(input);
- if (inputType != IMAGE_LIST && !inputCapture.isOpened())
- inputType = INVALID;
- }
- if (inputType == INVALID)
- {
- cerr << " Input does not exist: " << input;
- goodInput = false;
- }
-
- flag = 0;
- if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
- if(calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST;
- if(aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO;
- if(fixK1) flag |= CALIB_FIX_K1;
- if(fixK2) flag |= CALIB_FIX_K2;
- if(fixK3) flag |= CALIB_FIX_K3;
- if(fixK4) flag |= CALIB_FIX_K4;
- if(fixK5) flag |= CALIB_FIX_K5;
-
- if (useFisheye) {
- // the fisheye model has its own enum, so overwrite the flags
- flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC;
- if(fixK1) flag |= fisheye::CALIB_FIX_K1;
- if(fixK2) flag |= fisheye::CALIB_FIX_K2;
- if(fixK3) flag |= fisheye::CALIB_FIX_K3;
- if(fixK4) flag |= fisheye::CALIB_FIX_K4;
- if (calibFixPrincipalPoint) flag |= fisheye::CALIB_FIX_PRINCIPAL_POINT;
- }
-
- calibrationPattern = NOT_EXISTING;
- if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD;
- if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID;
- if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
- if (calibrationPattern == NOT_EXISTING)
- {
- cerr << " Camera calibration mode does not exist: " << patternToUse << endl;
- goodInput = false;
- }
- atImageList = 0;
-
+ LOG(ERROR) << "Invalid number of frames " << nrFrames;
+ goodInput = false;
}
- Mat nextImage()
- {
- Mat result;
- if( inputCapture.isOpened() )
- {
- Mat view0;
- inputCapture >> view0;
- view0.copyTo(result);
- }
- else if( atImageList < imageList.size() )
- result = imread(imageList[atImageList++], IMREAD_COLOR);
- return result;
+ flag = 0;
+ if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
+ if(calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST;
+ if(aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO;
+ if(fixK1) flag |= CALIB_FIX_K1;
+ if(fixK2) flag |= CALIB_FIX_K2;
+ if(fixK3) flag |= CALIB_FIX_K3;
+ if(fixK4) flag |= CALIB_FIX_K4;
+ if(fixK5) flag |= CALIB_FIX_K5;
+
+ if (useFisheye) {
+ // the fisheye model has its own enum, so overwrite the flags
+ flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC;
+ if(fixK1) flag |= fisheye::CALIB_FIX_K1;
+ if(fixK2) flag |= fisheye::CALIB_FIX_K2;
+ if(fixK3) flag |= fisheye::CALIB_FIX_K3;
+ if(fixK4) flag |= fisheye::CALIB_FIX_K4;
+ if (calibFixPrincipalPoint) flag |= fisheye::CALIB_FIX_PRINCIPAL_POINT;
}
- static bool readStringList( const string& filename, vector<string>& l )
+ calibrationPattern = NOT_EXISTING;
+ if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD;
+ if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID;
+ if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
+ if (calibrationPattern == NOT_EXISTING)
{
- l.clear();
- FileStorage fs(filename, FileStorage::READ);
- if( !fs.isOpened() )
- return false;
- FileNode n = fs.getFirstTopLevelNode();
- if( n.type() != FileNode::SEQ )
- return false;
- FileNodeIterator it = n.begin(), it_end = n.end();
- for( ; it != it_end; ++it )
- l.push_back((string)*it);
- return true;
+ LOG(ERROR) << " Camera calibration mode does not exist: " << patternToUse;
+ goodInput = false;
}
+ atImageList = 0;
- static bool isListOfImages( const string& filename)
- {
- string s(filename);
- // Look for file extension
- if( s.find(".xml") == string::npos && s.find(".yaml") == string::npos && s.find(".yml") == string::npos )
- return false;
- else
- return true;
- }
-public:
- Size boardSize; // The size of the board -> Number of items by width and height
- Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric circle pattern
- float squareSize; // The size of a square in your defined unit (point, millimeter,etc).
- int nrFrames; // The number of frames to use from the input for calibration
- float aspectRatio; // The aspect ratio
- int delay; // In case of a video input
- bool writePoints; // Write detected feature points
- bool writeExtrinsics; // Write extrinsic parameters
- bool writeGrid; // Write refined 3D target grid points
- bool calibZeroTangentDist; // Assume zero tangential distortion
- bool calibFixPrincipalPoint; // Fix the principal point at the center
- bool flipVertical; // Flip the captured images around the horizontal axis
- string outputFileName; // The name of the file where to write
- bool showUndistorsed; // Show undistorted images after calibration
- string input; // The input ->
- bool useFisheye; // use fisheye camera model for calibration
- bool fixK1; // fix K1 distortion coefficient
- bool fixK2; // fix K2 distortion coefficient
- bool fixK3; // fix K3 distortion coefficient
- bool fixK4; // fix K4 distortion coefficient
- bool fixK5; // fix K5 distortion coefficient
-
- int cameraID;
- vector<string> imageList;
- size_t atImageList;
- VideoCapture inputCapture;
- InputType inputType;
- bool goodInput;
- int flag;
-
-private:
- string patternToUse;
-
-
-};
-
-static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings())
-{
- if(node.empty())
- x = default_value;
- else
- x.read(node);
-}
-
-Calibrate::Calibrate(ftl::LocalSource *s, const std::string &cal) : local_(s) {
-
-}
-
-bool Calibrate::recalibrate(const std::string &conf) {
- return false;
}
-
-bool Calibrate::undistort(cv::Mat &l, cv::Mat &r) {
- local_->get(l,r);
- return false;
+Mat Calibrate::_nextImage(size_t cam)
+{
+ Mat result;
+ if (cam == 0) {
+ local_->left(result);
+ } else if (cam == 1 && local_->isStereo()) {
+ local_->right(result);
+ }
+ return result;
}
-bool Calibrate::rectified(cv::Mat &l, cv::Mat &r) {
- return undistort(l,r);
+bool Calibrate::Settings::readStringList( const string& filename, vector<string>& l )
+{
+ l.clear();
+ FileStorage fs(filename, FileStorage::READ);
+ if( !fs.isOpened() )
+ return false;
+ FileNode n = fs.getFirstTopLevelNode();
+ if( n.type() != FileNode::SEQ )
+ return false;
+ FileNodeIterator it = n.begin(), it_end = n.end();
+ for( ; it != it_end; ++it )
+ l.push_back((string)*it);
+ return true;
}
-bool Calibrate::isCalibrated() {
- return false;
+bool Calibrate::Settings::isListOfImages( const string& filename)
+{
+ string s(filename);
+ // Look for file extension
+ if( s.find(".xml") == string::npos && s.find(".yaml") == string::npos && s.find(".yml") == string::npos )
+ return false;
+ else
+ return true;
}
enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
-bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
+bool runCalibrationAndSave(Calibrate::Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints, float grid_width, bool release_object);
-int main2(int argc, char* argv[])
-{
-
- //! [file_read]
- Settings s;
- const string inputSettingsFile = parser.get<string>(0);
- FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
+Calibrate::Calibrate(ftl::LocalSource *s, const std::string &cal) : local_(s) {
+ FileStorage fs(cal, FileStorage::READ); // Read the settings
if (!fs.isOpened())
{
- cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
- parser.printMessage();
- return -1;
+ LOG(ERROR) << "Could not open the configuration file: \"" << cal << "\"";
+ return;
}
- fs["Settings"] >> s;
- fs.release(); // close Settings file
- //! [file_read]
-
- //FileStorage fout("settings.yml", FileStorage::WRITE); // write config as YAML
- //fout << "Settings" << s;
-
- if (!s.goodInput)
+ //fs["Settings"] >> settings_;
+ settings_.read(fs["Settings"]);
+ fs.release();
+
+ if (!settings_.goodInput)
{
- cout << "Invalid input detected. Application stopping. " << endl;
- return -1;
+ LOG(ERROR) << "Invalid input detected. Application stopping.";
+ return;
}
+
+ map1_.resize(2);
+ map2_.resize(2);
+
+ calibrated_ = false;
+}
- int winSize = parser.get<int>("winSize");
+bool Calibrate::recalibrate() {
+ bool r = _recalibrate(0);
+ if (local_->isStereo()) r |= _recalibrate(1);
+
+ if (r) calibrated_ = true;
+
+ return r;
+}
- float grid_width = s.squareSize * (s.boardSize.width - 1);
+bool Calibrate::_recalibrate(size_t cam) {
+ // TODO WHAT IS WINSIZE!!
+ int winSize = 11; //parser.get<int>("winSize");
+
+ float grid_width = settings_.squareSize * (settings_.boardSize.width - 1);
bool release_object = false;
- if (parser.has("d")) {
- grid_width = parser.get<float>("d");
- release_object = true;
- }
vector<vector<Point2f> > imagePoints;
Mat cameraMatrix, distCoeffs;
Size imageSize;
- int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
+ int mode = CAPTURING;
clock_t prevTimestamp = 0;
const Scalar RED(0,0,255), GREEN(0,255,0);
- const char ESC_KEY = 27;
+
+ settings_.outputFileName = string(FTL_CONFIG_ROOT "/calib_cam") + std::to_string(cam) + ".yml";
//! [get_input]
for(;;)
{
Mat view;
- bool blinkOutput = false;
+ //bool blinkOutput = false;
- view = s.nextImage();
+ view = _nextImage(cam);
+ LOG(INFO) << "Grabbing calibration image...";
//----- If no more image, or got enough, then stop calibration and show result -------------
- if( mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames )
+ if( mode == CAPTURING && imagePoints.size() >= (size_t)settings_.nrFrames )
{
- if(runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width,
- release_object))
+ if(runCalibrationAndSave(settings_, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width,
+ release_object)) {
+ LOG(INFO) << "Calibration completed";
mode = CALIBRATED;
- else
+ } else
mode = DETECTION;
}
if(view.empty()) // If there are no more images stop the loop
{
+ LOG(INFO) << "More calibration images are required";
// if calibration threshold was not reached yet, calibrate now
if( mode != CALIBRATED && !imagePoints.empty() )
- runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width,
+ runCalibrationAndSave(settings_, imageSize, cameraMatrix, distCoeffs, imagePoints, grid_width,
release_object);
break;
}
//! [get_input]
imageSize = view.size(); // Format input image.
- if( s.flipVertical ) flip( view, view, 0 );
+ if( settings_.flipVertical ) flip( view, view, 0 );
//! [find_pattern]
vector<Point2f> pointBuf;
@@ -352,21 +254,21 @@ int main2(int argc, char* argv[])
int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
- if(!s.useFisheye) {
+ if(!settings_.useFisheye) {
// fast check erroneously fails with high distortions like fisheye
chessBoardFlags |= CALIB_CB_FAST_CHECK;
}
- switch( s.calibrationPattern ) // Find feature points on the input format
+ switch( settings_.calibrationPattern ) // Find feature points on the input format
{
case Settings::CHESSBOARD:
- found = findChessboardCorners( view, s.boardSize, pointBuf, chessBoardFlags);
+ found = findChessboardCorners( view, settings_.boardSize, pointBuf, chessBoardFlags);
break;
case Settings::CIRCLES_GRID:
- found = findCirclesGrid( view, s.boardSize, pointBuf );
+ found = findCirclesGrid( view, settings_.boardSize, pointBuf );
break;
case Settings::ASYMMETRIC_CIRCLES_GRID:
- found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
+ found = findCirclesGrid( view, settings_.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
break;
default:
found = false;
@@ -377,7 +279,7 @@ int main2(int argc, char* argv[])
if ( found) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
- if( s.calibrationPattern == Settings::CHESSBOARD)
+ if( settings_.calibrationPattern == Settings::CHESSBOARD)
{
Mat viewGray;
cvtColor(view, viewGray, COLOR_BGR2GRAY);
@@ -386,20 +288,22 @@ int main2(int argc, char* argv[])
}
if( mode == CAPTURING && // For camera only take new samples after delay time
- (!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) )
+ (clock() - prevTimestamp > settings_.delay*1e-3*CLOCKS_PER_SEC) )
{
imagePoints.push_back(pointBuf);
prevTimestamp = clock();
- blinkOutput = s.inputCapture.isOpened();
+ // blinkOutput = s.inputCapture.isOpened();
}
// Draw the corners.
- drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found );
+ drawChessboardCorners( view, settings_.boardSize, Mat(pointBuf), found );
+ } else {
+ LOG(WARNING) << "No calibration pattern found";
}
//! [pattern_found]
//----------------------------- Output Text ------------------------------------------------
//! [output_text]
- string msg = (mode == CAPTURING) ? "100/100" :
+ /*string msg = (mode == CAPTURING) ? "100/100" :
mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
int baseLine = 0;
Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
@@ -416,75 +320,76 @@ int main2(int argc, char* argv[])
putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED);
if( blinkOutput )
- bitwise_not(view, view);
+ bitwise_not(view, view);*/
//! [output_text]
//------------------------- Video capture output undistorted ------------------------------
//! [output_undistorted]
- if( mode == CALIBRATED && s.showUndistorsed )
+ /*if( mode == CALIBRATED && settings_.showUndistorsed )
{
Mat temp = view.clone();
- if (s.useFisheye)
+ if (settings_.useFisheye)
cv::fisheye::undistortImage(temp, view, cameraMatrix, distCoeffs);
else
- undistort(temp, view, cameraMatrix, distCoeffs);
- }
+ cv::undistort(temp, view, cameraMatrix, distCoeffs);
+ }*/
//! [output_undistorted]
//------------------------------ Show image and check for input commands -------------------
//! [await_input]
- imshow("Image View", view);
+ /*imshow("Image View", view);
char key = (char)waitKey(s.inputCapture.isOpened() ? 50 : s.delay);
if( key == ESC_KEY )
break;
if( key == 'u' && mode == CALIBRATED )
- s.showUndistorsed = !s.showUndistorsed;
+ s.showUndistorsed = !s.showUndistorsed;*/
- if( s.inputCapture.isOpened() && key == 'g' )
+ /*if( s.inputCapture.isOpened() && key == 'g' )
{
mode = CAPTURING;
imagePoints.clear();
- }
+ }*/
//! [await_input]
+ if (mode == CALIBRATED) break;
}
+
+ if (mode != CALIBRATED) return false;
+
+ Mat view, rview;
- // -----------------------Show the undistorted image for the image list ------------------------
- //! [show_results]
- if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed )
+ if (settings_.useFisheye)
+ {
+ Mat newCamMat;
+ fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize,
+ Matx33d::eye(), newCamMat, 1);
+ fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(), newCamMat, imageSize,
+ CV_16SC2, map1_[cam], map2_[cam]);
+ }
+ else
{
- Mat view, rview, map1, map2;
+ initUndistortRectifyMap(
+ cameraMatrix, distCoeffs, Mat(),
+ getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize,
+ CV_16SC2, map1_[cam], map2_[cam]);
+ }
+ return true;
+}
- if (s.useFisheye)
- {
- Mat newCamMat;
- fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize,
- Matx33d::eye(), newCamMat, 1);
- fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(), newCamMat, imageSize,
- CV_16SC2, map1, map2);
- }
- else
- {
- initUndistortRectifyMap(
- cameraMatrix, distCoeffs, Mat(),
- getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize,
- CV_16SC2, map1, map2);
- }
+bool Calibrate::undistort(cv::Mat &l, cv::Mat &r) {
+ local_->get(l,r);
+ if (!calibrated_) return false;
+ if (l.empty()) return false;
+ remap(l, l, map1_[0], map2_[0], INTER_LINEAR);
+ if (local_->isStereo()) remap(r, r, map1_[1], map2_[1], INTER_LINEAR);
+ return true;
+}
- for(size_t i = 0; i < s.imageList.size(); i++ )
- {
- view = imread(s.imageList[i], IMREAD_COLOR);
- if(view.empty())
- continue;
- remap(view, rview, map1, map2, INTER_LINEAR);
- imshow("Image View", rview);
- char c = (char)waitKey();
- if( c == ESC_KEY || c == 'q' || c == 'Q' )
- break;
- }
- }
- //! [show_results]
+bool Calibrate::rectified(cv::Mat &l, cv::Mat &r) {
+ return undistort(l,r);
+}
- return 0;
+bool Calibrate::isCalibrated() {
+ return calibrated_;
}
//! [compute_errors]
@@ -523,20 +428,20 @@ static double computeReprojectionErrors( const vector<vector<Point3f> >& objectP
//! [compute_errors]
//! [board_corners]
static void calcBoardCornerPositions(Size boardSize, float squareSize, vector<Point3f>& corners,
- Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
+ Calibrate::Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
{
corners.clear();
switch(patternType)
{
- case Settings::CHESSBOARD:
- case Settings::CIRCLES_GRID:
+ case Calibrate::Settings::CHESSBOARD:
+ case Calibrate::Settings::CIRCLES_GRID:
for( int i = 0; i < boardSize.height; ++i )
for( int j = 0; j < boardSize.width; ++j )
corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
break;
- case Settings::ASYMMETRIC_CIRCLES_GRID:
+ case Calibrate::Settings::ASYMMETRIC_CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
corners.push_back(Point3f((2*j + i % 2)*squareSize, i*squareSize, 0));
@@ -546,7 +451,7 @@ static void calcBoardCornerPositions(Size boardSize, float squareSize, vector<Po
}
}
//! [board_corners]
-static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
+static bool runCalibration( Calibrate::Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs,
vector<float>& reprojErrs, double& totalAvgErr, vector<Point3f>& newObjPoints,
float grid_width, bool release_object)
@@ -613,12 +518,14 @@ static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat
}
// Print camera parameters to the output file
-static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
+static void saveCameraParams( Calibrate::Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
const vector<Mat>& rvecs, const vector<Mat>& tvecs,
const vector<float>& reprojErrs, const vector<vector<Point2f> >& imagePoints,
double totalAvgErr, const vector<Point3f>& newObjPoints )
{
FileStorage fs( s.outputFileName, FileStorage::WRITE );
+
+ LOG(INFO) << "Saving calibration to " << s.outputFileName;
time_t tm;
time( &tm );
@@ -731,7 +638,7 @@ static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, M
}
//! [run_and_save]
-bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
+bool runCalibrationAndSave(Calibrate::Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints, float grid_width, bool release_object)
{
vector<Mat> rvecs, tvecs;
@@ -743,6 +650,8 @@ bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat&
totalAvgErr, newObjPoints, grid_width, release_object);
cout << (ok ? "Calibration succeeded" : "Calibration failed")
<< ". avg re projection error = " << totalAvgErr << endl;
+
+ //return ok;
if (ok)
saveCameraParams(s, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, imagePoints,
diff --git a/cv-node/src/local.cpp b/cv-node/src/local.cpp
index 1c4ddd0bd..1a4618210 100644
--- a/cv-node/src/local.cpp
+++ b/cv-node/src/local.cpp
@@ -68,6 +68,67 @@ LocalSource::LocalSource(const string &vid): timestamp_(0.0) {
}
}
+bool LocalSource::left(cv::Mat &l) {
+ if (!camera_a_) return false;
+
+ if (!camera_a_->grab()) {
+ LOG(ERROR) << "Unable to grab from camera A";
+ return false;
+ }
+
+ // Record timestamp
+ timestamp_ = duration_cast<duration<double>>(
+ high_resolution_clock::now().time_since_epoch()).count();
+
+ if (camera_b_ || !stereo_) {
+ if (!camera_a_->retrieve(l)) {
+ LOG(ERROR) << "Unable to read frame from camera A";
+ return false;
+ }
+ } else {
+ Mat frame;
+ if (!camera_a_->retrieve(frame)) {
+ LOG(ERROR) << "Unable to read frame from video";
+ return false;
+ }
+
+ int resx = frame.cols / 2;
+ l = Mat(frame, Rect(0,0,resx,frame.rows));
+ }
+
+ return true;
+}
+
+bool LocalSource::right(cv::Mat &r) {
+ if (camera_b_ && !camera_b_->grab()) {
+ LOG(ERROR) << "Unable to grab from camera B";
+ return false;
+ }
+
+ // Record timestamp
+ timestamp_ = duration_cast<duration<double>>(
+ high_resolution_clock::now().time_since_epoch()).count();
+
+ if (camera_b_ || !stereo_) {
+ if (camera_b_ && !camera_b_->retrieve(r)) {
+ LOG(ERROR) << "Unable to read frame from camera B";
+ return false;
+ }
+ } else {
+ Mat frame;
+ if (!camera_a_) return false;
+ if (!camera_a_->retrieve(frame)) {
+ LOG(ERROR) << "Unable to read frame from video";
+ return false;
+ }
+
+ int resx = frame.cols / 2;
+ r = Mat(frame, Rect(resx,0,frame.cols-resx,frame.rows));
+ }
+
+ return true;
+}
+
bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
if (!camera_a_) return false;
@@ -102,7 +163,7 @@ bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
int resx = frame.cols / 2;
l = Mat(frame, Rect(0,0,resx,frame.rows));
- r = Mat(frame, Rect(resx,0,frame.cols,frame.rows));
+ r = Mat(frame, Rect(resx,0,frame.cols-resx,frame.rows));
}
return true;
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index bf4221399..b7d32c6d2 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -13,7 +13,7 @@ using cv::Mat;
static vector<string> OPTION_peers;
static vector<string> OPTION_channels;
-static string OPTION_calibration_config;
+static string OPTION_calibration_config = FTL_CONFIG_ROOT "/calibration.xml";
static string OPTION_config;
static bool OPTION_display = false;
static bool OPTION_calibrate = false;
@@ -73,6 +73,8 @@ int main(int argc, char **argv) {
Calibrate calibrate(lsrc, OPTION_calibration_config);
+ if (!calibrate.isCalibrated()) calibrate.recalibrate();
+
while (true) {
Mat l, r;
--
GitLab