///TODO FARE UNICA FUNZIONE MATNN
cv::Mat matToCvMat6x(float *O, const cv::Size &_sz)
{
cv::Mat_<cv::Vec6f> out(_sz);
int double_area = 2*_sz.area();
cv::Vec6f vec;
for(uint j = 0; j < _sz.width; ++j)
{
for(uint i = 0; i < _sz.height; ++i)
{
vec[0] = O[(j*_sz.height + i)];
vec[1] = O[(j*_sz.height + i) + _sz.area()];
vec[2] = O[(j*_sz.height + i) + double_area];
vec[3] = O[(j*_sz.height + i) + double_area + _sz.area()];
vec[4] = O[(j*_sz.height + i) + double_area + double_area];
vec[5] = O[(j*_sz.height + i) + double_area + double_area + _sz.area()];
out.at<cv::Vec6f>(i, j) = vec;
}
}
return out;
}
inline cv::Mat reformat_image(const std::vector<uint8_t>& vSignal, const cv::Size& oOrigImageSize) {
CV_Assert(!vSignal.empty() && oOrigImageSize.area()>0 && (oOrigImageSize.area()==vSignal.size() || oOrigImageSize.area()*3==vSignal.size())); // output can only be 1-channel (CV_8UC1) or 3-channel (CV_8UC3)
if(oOrigImageSize.area()==vSignal.size()) // if the image is single-channel (CV_8UC1), reassemble inline as cv::Mat data is row-major
return cv::Mat(oOrigImageSize,CV_8UC1,(void*)vSignal.data()).clone();
std::vector<cv::Mat> voChannels(3); // otherwise, use cv::merge to reinterlace individual CV_8UC1 matrices into a CV_8UC3 one
for(size_t c=0; c<3; ++c)
voChannels[c] = cv::Mat(oOrigImageSize,CV_8UC1,(void*)(vSignal.data()+c*oOrigImageSize.area())); // no need for a clone here, we will not modify the original data
cv::Mat oOrigImage; // will be created/sized automatically by cv::merge
cv::merge(voChannels,oOrigImage);
return oOrigImage;
}
// 打开棋盘图像并提取角点
int CameraCalibrator::addChessboardPoints(
const std::vector<std::string> &filelist,
cv::Size &boardSize)
{
// 棋盘上的点的两种坐标
std::vector<cv::Point2f> imageCorners;
std::vector<cv::Point3f> objectCorners;
// 3D场景中的点:
// 在棋盘坐标系中初始化棋盘角点
// 这些点位于 (X,Y,Z)= (i,j,0)
for (int i = 0; i < boardSize.height; i++)
{
for (int j = 0; j < boardSize.width; j++)
{
objectCorners.push_back(cv::Point3f(i, j, 0.0f));
}
}
// 2D图像中的点:
cv::Mat image; // 用来保存棋盘图像
int successes = 0;
// 循环所有图片
for (int i = 0; i < filelist.size(); i++)
{
// 打开图像
image = cv::imread(filelist[i], 0);
// 得到棋盘角点
bool found = cv::findChessboardCorners(image, boardSize, imageCorners);
// 获取亚像素精度
cv::cornerSubPix(image, imageCorners,
cv::Size(5, 5),
cv::Size(-1, -1),
cv::TermCriteria(cv::TermCriteria::MAX_ITER +
cv::TermCriteria::EPS,
30, // 最大迭代数目
0.1)); // 最小精度
// 如果角点数目满足要要求，那么将它加入数据
if (imageCorners.size() == boardSize.area())
{
// 添加一个视角中的图像点及场景点
addPoints(imageCorners, objectCorners);
successes++;
}
// 绘制角点
// found 已经找到角点
cv::drawChessboardCorners(image, boardSize, imageCorners, found);
// 显示
cv::imshow("Corners on Chessboard", image);
cv::waitKey(100);
}
return successes;
}
bool CameraCalibration::findChessboardPoints(const cv::Mat &image, cv::Size &boardSize, std::vector<cv::Point2f> &imageCorners, std::vector<cv::Point3f> &objectCorners)
{
for(int i = 0; i < boardSize.height; i++)
{
for(int j = 0; j < boardSize.width; j++)
{
objectCorners.push_back(cv::Point3f(i * 110, j * 110, 0.0f)); //110 = size of one square on the board
}
}
bool success = cv::findChessboardCorners(image, boardSize, imageCorners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
if(success) {
cv::cornerSubPix(
image,
imageCorners,
cv::Size(5,5),
cv::Size(-1,-1),
cv::TermCriteria(
cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS,
30, // max number of iterations
0.1 // min accuracy
)
);
}
objectCorners.resize(imageCorners.size(), objectCorners[0]);
return imageCorners.size() == boardSize.area();
}
// Open chessboard images and extract corner points
int CameraCalibrator::addChessboardPoints(const std::vector<std::string>& filelist,
cv::Size& boardSize)
{
// the points on the chessboard
std::vector<cv::Point2f> imageCorners;
std::vector<cv::Point3f> objectCorners;
// 3D Scene Points:
// Initialize the chessboard corners
// in the chessboard reference frame
// The corners are at 3D location (X,Y,Z)= (i,j,0)
for (int i = 0; i < boardSize.height; i++)
{
for (int j = 0; j < boardSize.width; j++)
{
objectCorners.push_back(cv::Point3f(i, j, 0.0f));
}
}
// 2D Image points:
cv::Mat image; // to contain chessboard image
int successes = 0;
// for all viewpoints
for (int i = 0; i < filelist.size(); i++)
{
// Open the image
image = cv::imread(filelist[i], 0);
// Get the chessboard corners
bool found = cv::findChessboardCorners(image, boardSize, imageCorners);
// Get subpixel accuracy on the corners
cv::cornerSubPix(image, imageCorners, cv::Size(5, 5), cv::Size(-1, -1),
cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS,
30, // max number of iterations
0.1)); // min accuracy
// If we have a good board, add it to our data
if (imageCorners.size() == boardSize.area())
{
// Add image and scene points from one view
addPoints(imageCorners, objectCorners);
successes++;
}
// Draw the corners
cv::drawChessboardCorners(image, boardSize, imageCorners, found);
cv::imshow("Corners on Chessboard", image);
cv::waitKey(100);
}
return successes;
}
vector<Point3f> CalcChessboardCorners(cv::Size chess_size, float square_size) {
vector<Point3f> corners;
corners.reserve(chess_size.area());
for (int i = 0; i < chess_size.height; i++)
for (int j = 0; j < chess_size.width; j++)
corners.push_back(Point3f(float(j * square_size), float(i
* square_size), 0));
return corners;
}
std::vector<std::vector<cv::Point3f>> calculateWorldPoints(
std::vector<std::vector<cv::Point2f>>& imagePoints,
const cv::Size& patternSize, float checkSize) {
std::vector<std::vector<cv::Point3f>> worldPoints(imagePoints.size());
for (size_t i = 0; i < imagePoints.size(); i++) {
for (size_t j = 0; j < patternSize.area(); j++) {
worldPoints[i].push_back(
cv::Point3f(static_cast<float>(j % patternSize.width * checkSize),
static_cast<float>(j / patternSize.width * checkSize),
0.0));
}
}
return worldPoints;
}
int CameraCalibrator::addChessboardPoints(const std::vector<std::string>& filelist,cv::Size & boardSize)
{
std::vector<cv::Point2f> imageCorners;
std::vector<cv::Point3f> objectCorners;
// Углы в 3D пространстве (X,Y,Z)= (i,j,0)
for (int i=0; i<boardSize.height; i++) {
for (int j=0; j<boardSize.width; j++) {
objectCorners.push_back(cv::Point3f(i, j, 0.0f));
}
}
cv::Mat image,image1;
int successes = 0; //счетчик удачных изображений
for (int i=0; i<filelist.size(); i++) {
//загрузка изображения и перевод в черно-белое
image1 = cv::imread(filelist[i],CV_LOAD_IMAGE_COLOR);
cvtColor(image1,image, CV_BGR2GRAY);
// поиск углов в пикселях
bool found = cv::findChessboardCorners(image, boardSize, imageCorners);
if (found)
{
//уточнение
cv::cornerSubPix(image, imageCorners,
cv::Size(5,5),
cv::Size(-1,-1),
cv::TermCriteria(cv::TermCriteria::MAX_ITER +
cv::TermCriteria::EPS,
30,
0.1));
//добавление особых точек
if (imageCorners.size() == boardSize.area()) {
addPoints(imageCorners, objectCorners);
successes++;
}
}
}
return successes;
}
// Open chessboard images and extract corner points
int CameraCalibrator::addChessboardPoints(const std::vector<std::string>& filelist, cv::Size & boardSize)
{
// the points on the chessboard
std::vector<cv::Point2f> imageCorners;
std::vector<cv::Point3f> objectCorners;
// 3D Scene Points:
// Initialize the chessboard corners
// in the chessboard reference frame
// The corners are at 3D location (X,Y,Z)= (i,j,0)
for (int i=0; i<boardSize.height; i++)
{
for (int j=0; j<boardSize.width; j++)
{
objectCorners.push_back(cv::Point3f(i, j, 0.0f));
}
}
// 2D Image points:
cv::Mat image; // to contain chessboard image
std::cout << "Processing Chessboard Corners" << std::endl << std::endl;
int successes = 0;
// for all viewpoints
for (int i=0; i<filelist.size(); i++)
{
// Open the image
image = cv::imread(filelist[i],0);
// Get the chessboard corners
bool found = cv::findChessboardCorners(image, boardSize, imageCorners);
// Get subpixel accuracy on the corners
// 30- max number of iterations
// 0.1- min accuracy
cv::cornerSubPix(image, imageCorners, cv::Size(5,5), cv::Size(-1,-1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 30, 0.1));
////Printing ChessBoard Object + Image Corners
//for(int count=0;count<imageCorners.size();count++)
//{
// std::cout<<objectCorners[count].x<<","<<objectCorners[count].y<<","<<objectCorners[count].z<<":"<<imageCorners[count].x<<","<<imageCorners[count].y<<std::endl;
//}
// If we have a good board, add it to our data
if (imageCorners.size() == boardSize.area())
{
// Add image and scene points from one view
addPoints(imageCorners, objectCorners);
successes++;
//Store the Images in Folder
image = cv::imread(filelist[i]);
bool found = cv::findChessboardCorners(image, boardSize, imageCorners);
cv::drawChessboardCorners(image, boardSize, imageCorners, found);
std::stringstream fileNameChessBoardOutput;
fileNameChessBoardOutput << IMG_OUT_CHESSBOARD <<"savasci_doshi_" << i << ".jpg";
cv::imwrite(fileNameChessBoardOutput.str(),image);
////Displaying the Chessboard corners
//cv::imshow(filelist[i], image);
//cv::waitKey(100);
}
}
return successes;
}
uchar TemplateMatchCandidates::compareWeakClassifiers(
const cv::Mat_<int> &i,
int x, int y,
cv::Size templSize,
const std::vector< cv::Rect > &blocks,
const int *compareTo,
float templateMean,
float maxMeanDiff, int maxWeakErrors)
{
const int *topRow = i.ptr<int>(y);
const int *bottomRow = i.ptr<int>(y + templSize.height); // +1 required for integrals
// Mean of image under given template position
const float posMean = (bottomRow[x + templSize.width] - bottomRow[x] - topRow[x + templSize.width] + topRow[x]) / (1.f * templSize.area());
if (std::abs(posMean - templateMean) > maxMeanDiff)
return 0;
// Evaluate means of sub-blocks
int sumErrors = 0;
for (size_t r = 0; r < blocks.size(); ++r)
{
const cv::Rect &b = blocks[r];
int ox = x + b.x;
int oy = y + b.y;
const int *topRow = i.ptr<int>(oy);
const int *bottomRow = i.ptr<int>(oy + b.height);
const float blockMean = (bottomRow[ox + b.width] - bottomRow[ox] - topRow[ox + b.width] + topRow[ox]) / (1.f * b.width * b.height);
const int c = blockMean > posMean ? 1 : -1;
sumErrors += (c != compareTo[r]) ? 1 : 0;
if (sumErrors > maxWeakErrors)
return 0;
}
return 255;
}
void cv::DisplayHelper::display(const std::vector<std::vector<std::pair<cv::Mat,std::string>>>& vvImageNamePairs, const cv::Size& oSuggestedTileSize) {
lvAssert_(!vvImageNamePairs.empty(),"must provide at least one row to display");
lvAssert_(oSuggestedTileSize.area()>0,"must provide non-null tile size");
const size_t nRowCount = vvImageNamePairs.size();
size_t nColCount = SIZE_MAX;
for(size_t nRowIdx=0; nRowIdx<nRowCount; ++nRowIdx) {
lvAssert_(!vvImageNamePairs[nRowIdx].empty(),"must provide at least one column to display");
lvAssert_(nColCount==SIZE_MAX || vvImageNamePairs[nRowIdx].size()==nColCount,"image map column count mismatch");
nColCount = vvImageNamePairs[nRowIdx].size();
for(size_t nColIdx=0; nColIdx<nColCount; ++nColIdx) {
const cv::Mat& oImage = vvImageNamePairs[nRowIdx][nColIdx].first;
lvAssert_(!oImage.empty(),"all images must be non-null");
lvAssert_(oImage.channels()==1 || oImage.channels()==3 || oImage.channels()==4,"all images must be 1/3/4 channels");
lvAssert_(oImage.depth()==CV_8U || oImage.depth()==CV_16U || oImage.depth()==CV_32F,"all images must be 8u/16u/32f depth");
}
}
cv::Size oCurrDisplaySize(int(oSuggestedTileSize.width*nColCount),int(oSuggestedTileSize.height*nRowCount));
if(m_oMaxDisplaySize.area()>0 && (oCurrDisplaySize.width>m_oMaxDisplaySize.width || oCurrDisplaySize.height>m_oMaxDisplaySize.height)) {
if(oCurrDisplaySize.width>m_oMaxDisplaySize.width && oCurrDisplaySize.width>oCurrDisplaySize.height)
oCurrDisplaySize = cv::Size(m_oMaxDisplaySize.width,int(m_oMaxDisplaySize.width*float(oCurrDisplaySize.height)/oCurrDisplaySize.width));
else
oCurrDisplaySize = cv::Size(int(m_oMaxDisplaySize.height*(float(oCurrDisplaySize.width)/oCurrDisplaySize.height)),m_oMaxDisplaySize.height);
}
const cv::Size oNewTileSize(int(oCurrDisplaySize.width/nColCount),int(oCurrDisplaySize.height/nRowCount));
const cv::Size oFinalDisplaySize(int(oNewTileSize.width*nColCount),int(oNewTileSize.height*nRowCount));
const cv::Point2i& oDisplayPt = m_oLatestMouseEvent.oInternalPosition;
for(size_t nRowIdx=0; nRowIdx<nRowCount; ++nRowIdx) {
cv::Mat oOutputRow;
for(size_t nColIdx=0; nColIdx<nColCount; ++nColIdx) {
const cv::Mat& oImage = vvImageNamePairs[nRowIdx][nColIdx].first;
cv::Mat oImageBYTE3;
if(oImage.depth()==CV_16U)
oImage.convertTo(oImageBYTE3,CV_8U,double(UCHAR_MAX)/(USHRT_MAX));
else if(oImage.depth()==CV_32F)
oImage.convertTo(oImageBYTE3,CV_8U,double(UCHAR_MAX));
else
oImageBYTE3 = oImage.clone();
if(oImageBYTE3.channels()==1)
cv::cvtColor(oImageBYTE3,oImageBYTE3,cv::COLOR_GRAY2BGR);
else if(oImageBYTE3.channels()==4)
cv::cvtColor(oImageBYTE3,oImageBYTE3,cv::COLOR_BGRA2BGR);
if(oImageBYTE3.size()!=oNewTileSize)
cv::resize(oImageBYTE3,oImageBYTE3,oNewTileSize);
if(!vvImageNamePairs[nRowIdx][nColIdx].second.empty())
putText(oImageBYTE3,vvImageNamePairs[nRowIdx][nColIdx].second,cv::Scalar_<uchar>(0,0,255));
if(oDisplayPt.x>=0 && oDisplayPt.y>=0 && oDisplayPt.x<oNewTileSize.width && oDisplayPt.y<oNewTileSize.height && m_oLatestMouseEvent.oTileSize==oNewTileSize)
cv::circle(oImageBYTE3,oDisplayPt,5,cv::Scalar(255,255,255));
if(oOutputRow.empty())
oOutputRow = oImageBYTE3;
else
cv::hconcat(oOutputRow,oImageBYTE3,oOutputRow);
}
if(nRowIdx==0)
m_oLastDisplay = oOutputRow;
else
cv::vconcat(m_oLastDisplay,oOutputRow,m_oLastDisplay);
}
if(m_bFirstDisplay && !m_bContinuousUpdates) {
putText(m_oLastDisplay,"[Press space to continue]",cv::Scalar_<uchar>(0,0,255),true,cv::Point2i(m_oLastDisplay.cols/2-100,15),1,1.0);
m_bFirstDisplay = false;
}
lvAssert(m_oLastDisplay.size()==oFinalDisplaySize);
cv::imshow(m_sDisplayName,m_oLastDisplay);
m_oLastDisplaySize = m_oLastDisplay.size();
m_oLastTileSize = oNewTileSize;
}
void calibrateProcess(Configure& conf, cv::Mat& cameraMatrix, cv::Mat& distCoeffs)
{
const cv::Size frameSize(conf.getConfInt("universal.frameWidth"),
conf.getConfInt("universal.frameHeight"));
const cv::string dataPath(conf.getConfString("universal.dataPath"));
const std::string checkerPrefix
(conf.getConfString("calibration.checkerPrefix"));
const std::string checkerSuffix
(conf.getConfString("calibration.checkerSuffix"));
const int checkerNum = conf.getConfInt("calibration.checkerNum");
const cv::Size checkerSize(conf.getConfInt("calibration.checkerWidth"),
conf.getConfInt("calibration.checkerHeight"));
cv::vector<cv::Mat> checkerImgs;
cv::vector<cv::vector<cv::Point3f>> worldPoints(checkerNum);
cv::vector<cv::vector<cv::Point2f>> imagePoints(checkerNum);
cv::TermCriteria criteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.001);
cv::vector<cv::Mat> rotationVectors;
cv::vector<cv::Mat> translationVectors;
for(int i=0; i<checkerNum; i++){
std::stringstream stream;
stream << checkerPrefix << i+1 << checkerSuffix;
std::string fileName = stream.str();
cv::Mat tmp = cv::imread(dataPath + fileName, 0);
cv::resize(tmp, tmp, frameSize);
checkerImgs.push_back(tmp);
std::cout << "load checker image: " << fileName << std::endl;
}
cv::namedWindow("Source", CV_WINDOW_AUTOSIZE|CV_WINDOW_FREERATIO);
for(int i=0; i<checkerNum; i++){
std::cout << "find corners from image " << i+1;
if(cv::findChessboardCorners(checkerImgs[i], checkerSize,
imagePoints[i])){
std::cout << " ... all corners found." << std::endl;
cv::cornerSubPix(checkerImgs[i], imagePoints[i], cv::Size(5, 5),
cv::Size(-1, -1), criteria);
cv::drawChessboardCorners(checkerImgs[i], checkerSize,
(cv::Mat)(imagePoints[i]), true);
cv::imshow("Source", checkerImgs[i]);
cv::waitKey(200);
}else{
std::cout << " ... at least 1 corner not found." << std::endl;
cv::waitKey(0);
exit(-1);
}
}
cv::destroyWindow("Source");
for(int i=0; i<checkerNum; i++){
for(int j=0; j<checkerSize.area(); j++){
worldPoints[i].
push_back(cv::Point3f(static_cast<float>(j%checkerSize.width*10),
static_cast<float>(j/checkerSize.width*10),
0.0));
}
}
cv::calibrateCamera(worldPoints, imagePoints, frameSize, cameraMatrix,
distCoeffs, rotationVectors, translationVectors);
std::cout << "camera matrix" << std::endl;
std::cout << cameraMatrix << std::endl;
std::cout << "dist coeffs" << std::endl;
std::cout << distCoeffs << std::endl;
}
