getting primal form from CvSVM trained file - opencv

I am trying to train my own detector based on HOG features and i trained a detector with CvSVM utility of opencv. Now to use this detector in HOGDescriptor.SetSVM(myDetector), i need to get trained detector in row-vector (primal) form to feed. For this i am using this code. my implementation is like given below:
vector<float>primal;
void LinearSVM::getSupportVector(std::vector<float>& support_vector) {
CvSVM svm;
svm.load("Classifier.xml");
cin.get();
int sv_count = svm.get_support_vector_count();
const CvSVMDecisionFunc* df = decision_func;
const double* alphas = df[0].alpha;
double rho = df[0].rho;
int var_count = svm.get_var_count();
support_vector.resize(var_count, 0);
for (unsigned int r = 0; r < (unsigned)sv_count; r++) {
float myalpha = alphas[r];
const float* v = svm.get_support_vector(r);
for (int j = 0; j < var_count; j++,v++) {
support_vector[j] += (-myalpha) * (*v);
}
}
support_vector.push_back(rho);
}
int main()
{
LinearSVM s;
s.getSupportVector(primal);
return 0;
}
When i use built-in CvSVM, it shows me SV as 3 bec i have only 3 SV in my saved file but since the decision_func is in protected mode, hence i can not access it. That's why i tried to use that wrapper but still of no use. Perhaps you guys can help me out here... Thanks alot!

Answer with a test harness. I put in new answer as it would add allot of clutter to the original answer, possibly making it a bit confusing.
//dummy features
std:: vector<float>
dummyDerReaderForOneDer(const vector<float> &pattern)
{
int i = std::rand() % pattern.size();
int j = std::rand() % pattern.size();
vector<float> patternPulNoise(pattern);
std::random_shuffle(patternPulNoise.begin()+std::min(i,j),patternPulNoise.begin()+std::max(i,j));
return patternPulNoise;
};
//extend CvSVM to get access to weights
class mySVM : public CvSVM
{
public:
vector<float>
getWeightVector(const int descriptorSize);
};
//get the weights
vector<float>
mySVM::getWeightVector(const int descriptorSize)
{
vector<float> svmWeightsVec(descriptorSize+1);
int numSupportVectors = get_support_vector_count();
//this is protected, but can access due to inheritance rules
const CvSVMDecisionFunc *dec = CvSVM::decision_func;
const float *supportVector;
float* svmWeight = &svmWeightsVec[0];
for (int i = 0; i < numSupportVectors; ++i)
{
float alpha = *(dec[0].alpha + i);
supportVector = get_support_vector(i);
for(int j=0;j<descriptorSize;j++)
{
*(svmWeight + j) += alpha * *(supportVector+j);
}
}
*(svmWeight + descriptorSize) = - dec[0].rho;
return svmWeightsVec;
}
// main harness entry point for detector test
int main (int argc, const char * argv[])
{
//dummy variables for example
int posFiles = 10;
int negFiles = 10;
int dims = 1000;
int randomFactor = 4;
//setup some dummy data
vector<float> dummyPosPattern;
dummyPosPattern.assign(int(dims/randomFactor),1.f);
dummyPosPattern.resize(dims );
random_shuffle(dummyPosPattern.begin(),dummyPosPattern.end());
vector<float> dummyNegPattern;
dummyNegPattern.assign(int(dims/randomFactor),1.f);
dummyNegPattern.resize(dims );
random_shuffle(dummyNegPattern.begin(),dummyNegPattern.end());
// the labels and lables mat
float posLabel = 1.f;
float negLabel = 2.f;
cv::Mat cSvmLabels;
//the data mat
cv::Mat cSvmTrainingData;
//dummy linear svm parmas
SVMParams cSvmParams;
cSvmParams.svm_type = cv::SVM::C_SVC;
cSvmParams.C = 0.0100;
cSvmParams.kernel_type = cv::SVM::LINEAR;
cSvmParams.term_crit = cv::TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 1000000, FLT_EPSILON);
cout << "creating training data. please wait" << endl;
int i;
for(i=0;i<posFiles;i++)
{
//your feature for one box from file
vector<float> d = dummyDerReaderForOneDer(dummyPosPattern);
//push back a new mat made from the vectors data, with copy data flag on
//this shows the format of the mat for a single example, (1 (row) X dims(col) ), as training mat has each **row** as an example;
//the push_back works like vector add adds each example to the bottom of the matrix
cSvmTrainingData.push_back(cv::Mat(1,dims,CV_32FC1,d.data(),true));
//push back a pos label to the labels mat
cSvmLabels.push_back(posLabel);
}
//do same with neg files;
for(i=0;i<negFiles;i++)
{
float a = rand();
vector<float> d = dummyDerReaderForOneDer(dummyNegPattern);
cSvmTrainingData.push_back(cv::Mat(1,dims,CV_32FC1,d.data(),true));
cSvmLabels.push_back(negLabel);
}
//have a look
cv::Mat viz;
cSvmTrainingData.convertTo(viz,CV_8UC3);
viz = viz*255;
cv::imshow("svmData", viz);
cv::waitKey(10);
cout << "press any key to continue" << endl;
getchar();
viz.release();
//create the svm;
cout << "training, please wait" << endl;
mySVM svm;
svm.train(cSvmTrainingData,cSvmLabels,cv::Mat(),cv::Mat(),cSvmParams);
cout << "get weights" << endl;
vector<float> svmWeights = svm.getWeightVector(dims);
for(i=0; i<dims+1; i++)
{
cout << svmWeights[i] << ", ";
if(i==dims)
{
cout << endl << "bias: " << svmWeights[i] << endl;
}
}
cout << "press any key to continue" << endl;
getchar();
cout << "testing, please wait" << endl;
//test the svm with a large amount of new unseen fake one at a time
int totExamples = 10;
int k;
for(i=0;i<totExamples; i++)
{
cout << endl << endl;
vector<float> dPos = dummyDerReaderForOneDer(dummyPosPattern);
cv::Mat dMatPos(1,dims,CV_32FC1,dPos.data(),true);
float predScoreFromDual = svm.predict(dMatPos,true);
float predScoreBFromPrimal = svmWeights[dims];
for( k = 0; k <= dims - 4; k += 4 )
predScoreBFromPrimal += dPos[k]*svmWeights[k] + dPos[k+1]*svmWeights[k+1] +
dPos[k+2]*svmWeights[k+2] + dPos[k+3]*svmWeights[k+3];
for( ; k < dims; k++ )
predScoreBFromPrimal += dPos[k]*svmWeights[k];
cout << "Dual Score:\t" << predScoreFromDual << "\tPrimal Score:\t" << predScoreBFromPrimal << endl;
}
cout << "press any key to continue" << endl;
getchar();
return(0);
}

Hello again :) please extend the cvsm class rather than encapsulating it, as you need access to protected member.
//header
class mySVM : public CvSVM
{
public:
vector<float>
getWeightVector(const int descriptorSize);
};
//cpp
vector<float>
mySVM::getWeightVector(const int descriptorSize)
{
vector<float> svmWeightsVec(descriptorSize+1);
int numSupportVectors = get_support_vector_count();
//this is protected, but can access due to inheritance rules
const CvSVMDecisionFunc *dec = CvSVM::decision_func;
const float *supportVector;
float* svmWeight = &svmWeightsVec[0];
for (int i = 0; i < numSupportVectors; ++i)
{
float alpha = *(dec[0].alpha + i);
supportVector = get_support_vector(i);
for(int j=0;j<descriptorSize;j++)
{
*(svmWeight + j) += alpha * *(supportVector+j);
}
}
*(svmWeight + descriptorSize) = - dec[0].rho;
return svmWeightsVec;
}
something like that.
credits:
Obtaining weights in CvSVM, the SVM implementation of OpenCV

Related

How to increase BatchSize with Tensorflow's C++ API?

I took the code in https://gist.github.com/kyrs/9adf86366e9e4f04addb (which takes an opencv cv::Mat image as input and converts it to tensor) and I use it to label images with the model inception_v3_2016_08_28_frozen.pb stated in the Tensorflow tutorial (https://www.tensorflow.org/tutorials/image_recognition#usage_with_the_c_api). Everything worked fine when using a batchsize of 1. However, when I increase the batchsize to 2 (or greater), the size of
finalOutput (which is of type std::vector) is zero.
Here's the code to reproduce the error:
// Only for VisualStudio
#define COMPILER_MSVC
#define NOMINMAX
#include <string>
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "tensorflow/core/public/session.h"
#include "tensorflow/core/platform/env.h"
#include "tensorflow/core/framework/tensor.h"
int batchSize = 2;
int height = 299;
int width = 299;
int depth = 3;
int mean = 0;
int stdev = 255;
// Set image paths
cv::String pathFilenameImg1 = "D:/IMGS/grace_hopper.jpg";
cv::String pathFilenameImg2 = "D:/IMGS/lenna.jpg";
// Set model paths
std::string graphFile = "D:/Tensorflow/models/inception_v3_2016_08_28_frozen.pb";
std::string labelfile = "D:/Tensorflow/models/imagenet_slim_labels.txt";
std::string InputName = "input";
std::string OutputName = "InceptionV3/Predictions/Reshape_1";
void read_prepare_image(cv::String pathImg, cv::Mat &imgPrepared) {
// Read Color image:
cv::Mat imgBGR = cv::imread(pathImg);
// Now we resize the image to fit Model's expected sizes:
cv::Size s(height, width);
cv::Mat imgResized;
cv::resize(imgBGR, imgResized, s, 0, 0, cv::INTER_CUBIC);
// Convert the image to float and normalize data:
imgResized.convertTo(imgPrepared, CV_32FC1);
imgPrepared = imgPrepared - mean;
imgPrepared = imgPrepared / stdev;
}
int main()
{
// Read and prepare images using OpenCV:
cv::Mat img1, img2;
read_prepare_image(pathFilenameImg1, img1);
read_prepare_image(pathFilenameImg2, img2);
// creating a Tensor for storing the data
tensorflow::Tensor input_tensor(tensorflow::DT_FLOAT, tensorflow::TensorShape({ batchSize, height, width, depth }));
auto input_tensor_mapped = input_tensor.tensor<float, 4>();
// Copy images data into the tensor:
for (int b = 0; b < batchSize; ++b) {
const float * source_data;
if (b == 0)
source_data = (float*)img1.data;
else
source_data = (float*)img2.data;
for (int y = 0; y < height; ++y) {
const float* source_row = source_data + (y * width * depth);
for (int x = 0; x < width; ++x) {
const float* source_pixel = source_row + (x * depth);
const float* source_B = source_pixel + 0;
const float* source_G = source_pixel + 1;
const float* source_R = source_pixel + 2;
input_tensor_mapped(b, y, x, 0) = *source_R;
input_tensor_mapped(b, y, x, 1) = *source_G;
input_tensor_mapped(b, y, x, 2) = *source_B;
}
}
}
// Load the graph:
tensorflow::GraphDef graph_def;
ReadBinaryProto(tensorflow::Env::Default(), graphFile, &graph_def);
// create a session with the graph
std::unique_ptr<tensorflow::Session> session_inception(tensorflow::NewSession(tensorflow::SessionOptions()));
session_inception->Create(graph_def);
// run the loaded graph
std::vector<tensorflow::Tensor> finalOutput;
session_inception->Run({ { InputName,input_tensor } }, { OutputName }, {}, &finalOutput);
// Get Top 5 classes:
std::cerr << "final output size = " << finalOutput.size() << std::endl;
tensorflow::Tensor output = std::move(finalOutput.at(0));
auto scores = output.flat<float>();
std::cerr << "scores size=" << scores.size() << std::endl;
std::ifstream label(labelfile);
std::string line;
std::vector<std::pair<float, std::string>> sorted;
for (unsigned int i = 0; i <= 1000; ++i) {
std::getline(label, line);
sorted.emplace_back(scores(i), line);
}
std::sort(sorted.begin(), sorted.end());
std::reverse(sorted.begin(), sorted.end());
std::cout << "size of the sorted file is " << sorted.size() << std::endl;
for (unsigned int i = 0; i< 5; ++i)
std::cout << "The output of the current graph has category " << sorted[i].second << " with probability " << sorted[i].first << std::endl;
}
Do I miss anything? Any ideas?
Thanks in advance!
I had the same problem. When I changed to the model used in https://github.com/tensorflow/tensorflow/tree/master/tensorflow/tools/benchmark (differente version of inception) bigger batch sizes work correctly.
Notice you need to change the input size from 299,299,3 to 224,224,3 and the input and output layer names to: input:0 and output:0
Probably the graph in the protobuf file had a fixed batch size of 1 and I was only changing the shape of the input, not the graph. The graph has to accept a variable batch size by setting the shape to (None, width, heihgt, channels). This is done when you freeze the graph. Since the graph we have is already frozen, there is no way to change the batch size at this point.

image binarization using newer Algorithms in opencv

I wanted to binarize low quality images and found that the existing solutions or programs which are implementations of global and local binarization techniques such as Sauvola’s method, NiBlack's method etc are not off much use.
I did find a few good papers regarding much better methods like the ones given in the papers:
1) http://www.ski.org/sites/default/files/publications/wacv11-display-reader.pdf#cite.adap-binar
2) https://www.jstage.jst.go.jp/article/elex/1/16/1_16_501/_pdf
But I haven't worked on image processing much before and so I wanted to know how I could proceed to implement it and what knowledge I need to implement these algorithms
I implemented the binarization of the first paper in like 10 minutes (less time than processing the 2nd image) - no guarantee that it's correct, better have a look at the formulas yourself:
int main()
{
//cv::Mat input = cv::imread("../inputData/Lenna.png");
cv::Mat input = cv::imread("../inputData/LongLineColor.jpg");
cv::Mat gray;
cv::cvtColor(input,gray,CV_BGR2GRAY);
cv::Mat binaryImage = cv::Mat::zeros(gray.rows, gray.cols, CV_8UC1);
// binarization:
// TODO: adjust to your application:
int smallWindowSize = 17; // suggested by the paper
int bigWindowSize = 35; // suggested by the paper
// TODO: adjust to your application
double minTau = 10 ;
// create roi relative to (0,0)
cv::Rect roiTemplate1 = cv::Rect(-smallWindowSize/2,-smallWindowSize/2, smallWindowSize, smallWindowSize);
cv::Rect roiTemplate2 = cv::Rect(-bigWindowSize/2,-bigWindowSize/2, bigWindowSize, bigWindowSize);
cv::Rect imgROI = cv::Rect(0,0, gray.cols, gray.rows);
for(int y=0; y<gray.rows; ++y)
{
std::cout << y << std::endl;
for(int x=0; x<gray.cols; ++x)
{
double pixelThreshold = 255;
// small roi
cv::Rect cROIs = roiTemplate1 + cv::Point(x,y);
// test whether ROI is inside the image. Reduce otherwise:
cROIs = cROIs & imgROI;
if(cROIs.width == 0 || cROIs.height == 0)
continue; // ignore this pixel
// large roi
cv::Rect cROIl = roiTemplate2 + cv::Point(x,y);
cROIl = cROIl & imgROI;
if(cROIl.width == 0 || cROIl.height == 0)
continue; // ignore this pixel
cv::Mat subSmall = gray(cROIs);
cv::Mat subLarge = gray(cROIl);
// evaluate subimages:
// standard deviations
double stdDevS =0;
double stdDevL =0;
// mean value
double meanS =0;
double minL =DBL_MAX;
double meanL =0;
// mean of small region
for(int j=0; j<subSmall.rows; ++j)
for(int i=0; i<subSmall.cols; ++i)
{
meanS += subSmall.at<unsigned char>(j,i);
}
meanS = meanS/ (double)(subSmall.cols*subSmall.rows);
// stddev of small region
for(int j=0; j<subSmall.rows; ++j)
for(int i=0; i<subSmall.cols; ++i)
{
double diff = subSmall.at<unsigned char>(j,i) - meanS;
stdDevS += diff*diff;
}
stdDevS = sqrt(stdDevS/(double)(subSmall.cols*subSmall.rows));
// mean and min of large region
for(int j=0; j<subLarge.rows; ++j)
for(int i=0; i<subLarge.cols; ++i)
{
if(subLarge.at<unsigned char>(j,i) < minL)
{
minL = subLarge.at<unsigned char>(j,i);
meanL += subLarge.at<unsigned char>(j,i);
}
}
meanL = meanL/ (double)(subLarge.cols*subLarge.rows);
// stddef of large region
for(int j=0; j<subLarge.rows; ++j)
for(int i=0; i<subLarge.cols; ++i)
{
double diff = subLarge.at<unsigned char>(j,i) - meanL;
stdDevL += diff*diff;
}
stdDevL = sqrt(stdDevL/(double)(subLarge.cols*subLarge.rows));
// formula (2)
double tau = ((meanS - minL) * (1-stdDevS/stdDevL))/2.0;
// minimum
if(tau < minTau) tau = minTau;
// formula (1)
double Threshold = meanS - tau;
// for debugging:
/*
std::cout << " meanS:" << meanS << std::endl;
std::cout << " std S:" << stdDevS << std::endl;
std::cout << " min L:" << minL << std::endl;
std::cout << " meanL:" << meanL << std::endl;
std::cout << " std L:" << stdDevL << std::endl;
std::cout << " threshold: " << Threshold << std::endl;
*/
unsigned char pixelVal = gray.at<unsigned char>(y,x);
if(pixelVal >= Threshold)
binaryImage.at<unsigned char>(y,x) = 255;
else
binaryImage.at<unsigned char>(y,x) = 0;
}
}
cv::imshow("input", input);
cv::imshow("binary", binaryImage);
//cv::imwrite("../outputData/binaryCustom.png", binaryImage);
cv::waitKey(0);
return 0;
}
giving me these results:
and
It is very slow but not optimized or encapsulated at all ;)
And the results aren't sooo good imho. Probably you have to adjust the windowSizes to your application/task/objectSize

Failed Assertion Using HOGDescriptor

Ok, so I've decided that using a histogram of oriented gradients is a better method for image fingerprinting vs. creating a histogram of sobel derivatives. I think I finally have it mostly figured out but when I test my code I get the following:
OpenCV Error: Assertion failed ((winSize.width - blockSize.width) % blockStride.width == 0 && (winSize.height - blockSize.height) % blockStride.height == 0).
As of now I'm just trying to figure out how to compute the HOG correctly and see the results; but not visually, I just want some very basic output to see if the HOG was created. Then I'll figure out how to use it in image comparison.
Here is my sample code:
using namespace cv;
using namespace std;
int main(int argc, const char * argv[])
{
// Initialize string variables.
string thePath, img, hogSaveFile;
thePath = "/Users/Mikie/Documents/Xcode/images/";
img = thePath + "HDimage.jpg";
hogSaveFile = thePath + "HDimage.yml";
// Create mats.
Mat src;
// Load image as grayscale.
src = imread(img, CV_LOAD_IMAGE_GRAYSCALE);
// Verify source loaded.
if(src.empty()){
cout << "No image data. \n ";
return -1;
}else{
cout << "Image loaded. \n" << "Size: " << src.cols << " X " << src.rows << "." << "\n";
}
// Initialize float variables.
float imgWidth, imgHeight, newWidth, newHeight;
imgWidth = src.cols;
imgHeight = src.rows;
newWidth = 320;
newHeight = (imgHeight/imgWidth)*newWidth;
Mat dst = Mat::zeros(newHeight, newWidth, CV_8UC3);
resize(src, dst, Size(newWidth, newHeight), CV_INTER_LINEAR);
// Was resize successful?
if (dst.rows < src.rows && dst.cols < src.cols) {
cout << "Resize successful. \n" << "New size: " << dst.cols << " X " << dst.rows << "." << "\n";
} else {
cout << "Resize failed. \n";
return -1;
}
vector<float>theHOG(Mat dst);{
if (dst.empty()) {
cout << "Image lost. \n";
} else {
cout << "Setting up HOG. \n";
}
imshow("Image", dst);
bool gammaC = true;
int nlevels = HOGDescriptor::DEFAULT_NLEVELS;
Size winS(newWidth, newHeight);
// int block_size = 16;
// int block_stride= 8;
// int cell_size = 8;
int gbins = 9;
vector<float> descriptorsValues;
vector<Point> locations;
HOGDescriptor hog(Size(320, 412), Size(16, 16), Size(8, 8), Size(8, 8), gbins, -1, HOGDescriptor::L2Hys, 0.2, gammaC, nlevels);
hog.compute(dst, descriptorsValues, Size(0,0), Size(0,0), locations);
printf("descriptorsValues.size() = %ld \n", descriptorsValues.size()); //prints 960
for (int i = 0; i <descriptorsValues.size(); i++) {
cout << descriptorsValues[i] << endl;
}
}
cvWaitKey(0);
return 0;
}
As you can see, I messed around with different variables to define the sizes but to no avail so, I commented them out and tried manually setting them. Still nothing. What am I doing wrong? Any help will be greatly appreciated.
Thank you!
You are initializing the HOGDescriptor incorrectly.
The assertion states that each of the first three input parameters must satisfy the constraint:
(winSize - blockSize) % blockStride == 0
in both height and width dimensions.
The problem is that winSize.height does not satisfy this constraint, considering the other parameters you initialize hog with:
(412 - 16) % 8 = 4 //Problem!!
Probably the simplest fix is to increase your window dimensions from cv::Size(320,412) to something divisible by 8, perhaps cv::Size(320,416), but the specific size will depend on your specific requirements. Just pay attention to what the assertion is saying!

read 2d array into opencv mat

i need to read dynamic two dimensional array in opencv mat
int main()
{
Mat matrix;
double **theArray;
int numOfRows,numOfCols;
cin >> numOfRows ;
cin >> numOfCols ;
theArray = AllocateDynamicArray<double>(numOfRows,numOfCols);
matrix = Mat(numOfRows,numOfCols,CV_64FC1,&theArray);
string filename = "IN.xml";
FileStorage fs1(filename, FileStorage::WRITE);
fs1.open(filename, FileStorage::WRITE);
fs1 << "locsINMat" << matrix;
fs1 << "descriptorsINMat" << matrix;
fs1.release();
cout << "---------------------" << endl;
FreeDynamicArray(theArray);
}
template <typename T>
T **AllocateDynamicArray( int nRows, int nCols)
{
T **dynamicArray;
dynamicArray = new T*[nRows];
for( int i = 0 ; i < nRows ; i++ )
dynamicArray[i] = new T [nCols];
return dynamicArray;
}
template <typename T>
void FreeDynamicArray(T** dArray)
{
delete [] *dArray;
delete [] dArray;
}
i get this exception:
Unhandled exception at 0x5d08f1aa in GP.exe: 0xC0000005: Access violation reading location 0x003f4000.
float data[2][5] = {{1,2,3,4,5},{7,8,9,10,11}};
A = Mat(2, 5, CV_32FC1, &data);
doesn't throw any error. are your rows and columns correct?
For dynamic memory allocation:
I think the memory allocation should be done like this:
double **theArray;
theArray= (double**)malloc(numOfRows* sizeof(double*));
for(i = 0; i < numOfRows; i++)
theArray[i] = (double*)malloc(numOfCols* sizeof(double));

Re-distort points with camera intrinsics/extrinsics

Given a set of 2D points, how can I apply the opposite of undistortPoints?
I have the camera intrinsics and distCoeffs and would like to (for example) create a square, and distort it as if the camera had viewed it through the lens.
I have found a 'distort' patch here : http://code.opencv.org/issues/1387 but it would seem this is only good for images, I want to work on sparse points.
This question is rather old but since I ended up here from a google search without seeing a neat answer I decided to answer it anyway.
There is a function called projectPoints that does exactly this. The C version is used internally by OpenCV when estimating camera parameters with functions like calibrateCamera and stereoCalibrate
EDIT:
To use 2D points as input, we can set all z-coordinates to 1 with convertPointsToHomogeneous and use projectPoints with no rotation and no translation.
cv::Mat points2d = ...;
cv::Mat points3d;
cv::Mat distorted_points2d;
convertPointsToHomogeneous(points2d, points3d);
projectPoints(points3d, cv::Vec3f(0,0,0), cv::Vec3f(0,0,0), camera_matrix, dist_coeffs, distorted_points2d);
A simple solution is to use initUndistortRectifyMap to obtain a map from undistorted coordinates to distorted ones:
cv::Mat K = ...; // 3x3 intrinsic parameters
cv::Mat D = ...; // 4x1 or similar distortion parameters
int W = 640; // image width
int H = 480; // image height
cv::Mat mapx, mapy;
cv::initUndistortRectifyMap(K, D, cv::Mat(), K, cv::Size(W, H),
CV_32F, mapx, mapy);
float distorted_x = mapx.at<float>(y, x);
float distorted_y = mapy.at<float>(y, x);
I edit to clarify the code is correct:
I cite the documentation of initUndistortRectifyMap:
for each pixel (u, v) in the destination (corrected and rectified)
image, the function computes the corresponding coordinates in the
source image (that is, in the original image from camera.
map_x(u,v) = x''f_x + c_x
map_y(u,v) = y''f_y + c_y
undistortPoint is a simple reverse version of project points
In my case I would like to do the following:
Undistort points:
int undisortPoints(const vector<cv::Point2f> &uv, vector<cv::Point2f> &xy, const cv::Mat &M, const cv::Mat &d)
{
cv::undistortPoints(uv, xy, M, d, cv::Mat(), M);
return 0;
}
This will undistort the points to the very similar coordinate to the origin of the image, but without distortion. This is the default behavior for the cv::undistort() function.
Redistort points:
int distortPoints(const vector<cv::Point2f> &xy, vector<cv::Point2f> &uv, const cv::Mat &M, const cv::Mat &d)
{
vector<cv::Point2f> xy2;
vector<cv::Point3f> xyz;
cv::undistortPoints(xy, xy2, M, cv::Mat());
for (cv::Point2f p : xy2)xyz.push_back(cv::Point3f(p.x, p.y, 1));
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::projectPoints(xyz, rvec, tvec, M, d, uv);
return 0;
}
The little tricky thing here is to first project the points to the z=1 plane with a linear camera model. After that, you must project them with the original camera model.
I found these useful, I hope it also works for you.
I have had exactly the same need.
Here is a possible solution :
void MyDistortPoints(const std::vector<cv::Point2d> & src, std::vector<cv::Point2d> & dst,
const cv::Mat & cameraMatrix, const cv::Mat & distorsionMatrix)
{
dst.clear();
double fx = cameraMatrix.at<double>(0,0);
double fy = cameraMatrix.at<double>(1,1);
double ux = cameraMatrix.at<double>(0,2);
double uy = cameraMatrix.at<double>(1,2);
double k1 = distorsionMatrix.at<double>(0, 0);
double k2 = distorsionMatrix.at<double>(0, 1);
double p1 = distorsionMatrix.at<double>(0, 2);
double p2 = distorsionMatrix.at<double>(0, 3);
double k3 = distorsionMatrix.at<double>(0, 4);
//BOOST_FOREACH(const cv::Point2d &p, src)
for (unsigned int i = 0; i < src.size(); i++)
{
const cv::Point2d &p = src[i];
double x = p.x;
double y = p.y;
double xCorrected, yCorrected;
//Step 1 : correct distorsion
{
double r2 = x*x + y*y;
//radial distorsion
xCorrected = x * (1. + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
yCorrected = y * (1. + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
//tangential distorsion
//The "Learning OpenCV" book is wrong here !!!
//False equations from the "Learning OpenCv" book
//xCorrected = xCorrected + (2. * p1 * y + p2 * (r2 + 2. * x * x));
//yCorrected = yCorrected + (p1 * (r2 + 2. * y * y) + 2. * p2 * x);
//Correct formulae found at : http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
xCorrected = xCorrected + (2. * p1 * x * y + p2 * (r2 + 2. * x * x));
yCorrected = yCorrected + (p1 * (r2 + 2. * y * y) + 2. * p2 * x * y);
}
//Step 2 : ideal coordinates => actual coordinates
{
xCorrected = xCorrected * fx + ux;
yCorrected = yCorrected * fy + uy;
}
dst.push_back(cv::Point2d(xCorrected, yCorrected));
}
}
void MyDistortPoints(const std::vector<cv::Point2d> & src, std::vector<cv::Point2d> & dst,
const cv::Matx33d & cameraMatrix, const cv::Matx<double, 1, 5> & distorsionMatrix)
{
cv::Mat cameraMatrix2(cameraMatrix);
cv::Mat distorsionMatrix2(distorsionMatrix);
return MyDistortPoints(src, dst, cameraMatrix2, distorsionMatrix2);
}
void TestDistort()
{
cv::Matx33d cameraMatrix = 0.;
{
//cameraMatrix Init
double fx = 1000., fy = 950.;
double ux = 324., uy = 249.;
cameraMatrix(0, 0) = fx;
cameraMatrix(1, 1) = fy;
cameraMatrix(0, 2) = ux;
cameraMatrix(1, 2) = uy;
cameraMatrix(2, 2) = 1.;
}
cv::Matx<double, 1, 5> distorsionMatrix;
{
//distorsion Init
const double k1 = 0.5, k2 = -0.5, k3 = 0.000005, p1 = 0.07, p2 = -0.05;
distorsionMatrix(0, 0) = k1;
distorsionMatrix(0, 1) = k2;
distorsionMatrix(0, 2) = p1;
distorsionMatrix(0, 3) = p2;
distorsionMatrix(0, 4) = k3;
}
std::vector<cv::Point2d> distortedPoints;
std::vector<cv::Point2d> undistortedPoints;
std::vector<cv::Point2d> redistortedPoints;
distortedPoints.push_back(cv::Point2d(324., 249.));// equals to optical center
distortedPoints.push_back(cv::Point2d(340., 200));
distortedPoints.push_back(cv::Point2d(785., 345.));
distortedPoints.push_back(cv::Point2d(0., 0.));
cv::undistortPoints(distortedPoints, undistortedPoints, cameraMatrix, distorsionMatrix);
MyDistortPoints(undistortedPoints, redistortedPoints, cameraMatrix, distorsionMatrix);
cv::undistortPoints(redistortedPoints, undistortedPoints, cameraMatrix, distorsionMatrix);
//Poor man's unit test ensuring we have an accuracy that is better than 0.001 pixel
for (unsigned int i = 0; i < undistortedPoints.size(); i++)
{
cv::Point2d dist = redistortedPoints[i] - distortedPoints[i];
double norm = sqrt(dist.dot(dist));
std::cout << "norm = " << norm << std::endl;
assert(norm < 1E-3);
}
}
For those still searching, here is a simple python function that will distort points back:
def distortPoints(undistortedPoints, k, d):
undistorted = np.float32(undistortedPoints[:, np.newaxis, :])
kInv = np.linalg.inv(k)
for i in range(len(undistorted)):
srcv = np.array([undistorted[i][0][0], undistorted[i][0][1], 1])
dstv = kInv.dot(srcv)
undistorted[i][0][0] = dstv[0]
undistorted[i][0][1] = dstv[1]
distorted = cv2.fisheye.distortPoints(undistorted, k, d)
return distorted
Example:
undistorted = np.array([(639.64, 362.09), (234, 567)])
distorted = distortPoints(undistorted, camK, camD)
print(distorted)
This question and it's related questions on SO have been around for nearly a decade, but there still isn't an answer that satisfies the criteria below so I'm proposing a new answer that
uses methods readily available in OpenCV,
works for points, not images, (and also points at subpixel locations),
can be used beyond fisheye distortion models,
does not involve manual interpolation or maps and
can be used in the context of rectification
Preliminaries
It is important to distinquish between ideal coordinates (also called 'normalized' or 'sensor' coordinates) which are the input variables to the distortion model or 'x' and 'y' in the OpenCV docs vs. observed coordinates (also called 'image' coordinates) or 'u' and 'v' in OpenCV docs. Ideal coordinates have been normalized by the intrinsic parameters so that they have been scaled by the focal length and are relative to the image centroid at (cx,cy). This is important to point out because the undistortPoints() method can return either ideal or observed coordinates depending on the input arguments.
undistortPoints() can essentially do any combination of two things: remove distortions and apply a rotational transformation with the output either being in ideal or observed coordinates, depending on if a projection mat (InputArray P) is provided in the input. The input coordinates (InputArray src) for undistortPoints() is always in observed or image coordinates.
At a high level undistortPoints() converts the input coordinates from observed to ideal coordinates and uses an iterative process to remove distortions from the ideal or normalized points. The reason the process is iterative is because the OpenCV distortion model is not easy to invert analytically.
In the example below, we use undistortPoints() twice. First, we apply a reverse rotational transformation to undo image rectification. This step can be skipped if you are not working with rectified images. The output of this first step is in observed coordinates so we use undistortPoints() again to convert these to ideal coordinates. The conversion to ideal coordinates makes setting up the input for projectPoints() easier (which we use to apply the distortions). With the ideal coordinates, we can simply convert them to homogeneous by appending a 1 to each point. This is equivalent to projecting the points to a plane in 3D world coordinates with a linear camera model.
As of currently, there isn't a method in OpenCV to apply distortions to a set of ideal coordinates (with the exception of fisheye distortions using distort()) so we employ the projectPoints() method which can apply distortions as well as transformations as part of its projection algorithm. The tricky part about using projectPoints() is that the input is in terms of world or model coordinates in 3D, which is why we homogenized the output of the second use of undistortPoints(). By using projectPoints() with a dummy, zero-valued rotation vector (InputArray rvec) and translation vector (Input Array tvec) the result is simply a distorted set of coordinates which is conveniently output in observed or image coordinates.
Some helpful links
Difference between undistortPoints() and projectPoints() in OpenCV
https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga55c716492470bfe86b0ee9bf3a1f0f7e
Re-distort points with camera intrinsics/extrinsics
https://stackoverflow.com/questions/28678985/exact-definition-of-the-matrices-in-opencv-stereorectify#:~:text=Normally%20the%20definition%20of%20a,matrix%20with%20the%20extrinsic%20parameters
https://docs.opencv.org/4.x/db/d58/group__calib3d__fisheye.html#ga75d8877a98e38d0b29b6892c5f8d7765
https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga617b1685d4059c6040827800e72ad2b6
Does OpenCV's undistortPoints also rectify them?
Removing distortions in rectified image coordinates
Before providing the solution to recovering the original image coordinates with distortions we provide a short snippet to convert from the original distorted image coordinates to the corresponding rectified, undistorted coordinates that can be used for testing the reverse solution below.
The rotation matrix R1 and the projection matrix P1 come from stereoRectify(). The intrinsic parameters M1 and distortion parameters D1 come from stereoCalibrate().
const size_t img_w = 2448;
const size_t img_h = 2048;
const size_t num_rand_pts = 100;
// observed coordinates of the points in the original
// distorted image (used as a benchmark for testing)
std::vector<cv::Point2f> benchmark_obs_dist_points;
// undistorted and rectified obnserved coordinates
std::vector<cv::Point2f> obs_rect_undist_points;
// initialize with uniform random numbers
cv::RNG rng( 0xFFFFFFFF );
for(size_t i =0;i<num_rand_pts;++i)
benchmark_obs_dist_points.push_back(
cv::Point2f(rng.uniform(0.0,(double)img_w),
rng.uniform(0.0,(double)img_h))
);
// undistort and rectify
cv::undistortPoints(benchmark_obs_dist_points,obs_rect_undist_points,
M1,D1,R1,P1);
Re-distorting and unrectifying points to recover the original image coordinates
We will need three mats to reverse the rectification: the inverse of the rectification rotation matrix from stereoRectify R1, and two others to 'swap' the P1 and M1 projections that happen in undistortPoints(). P1_prime is the rotation matrix sub-portion of the projection matrix and M1_prime converts the rectification rotation matrix into a projection matrix with no translation. Note this only works if the output of stereoRectify has no translation, i.e. the last column of P1 is zeros which can be easily verified.
assert(cv::norm(P1(cv::Rect(3,0,1,3))==0.0));
// create a 3x3 shallow copy of the rotation matrix portion of the projection P1
cv::Mat P1_prime = P1(cv::Rect(0,0,3,3));
// create a 3x4 projection matrix with the rotation portion of
// the rectification rotation matrix R1
cv::Mat M1_prime = cv::Mat::zeros(3,4,CV_64F);
M1.copyTo(M1_prime(cv::Rect(0,0,3,3)));
With these mats, the reversal can proceed as follows
// reverse the image rectification transformation
// (result will still be undistorted)
std::vector<cv::Point2f> obs_undist_points;
cv::undistortPoints(obs_rect_undist_points,obs_undist_points,
P1_prime,cv::Mat(),R1.inv(),M1_prime);
// convert the image coordinates into sensor or normalized or ideal coordinates
// (again, still undistorted)
std::vector<cv::Point2f> ideal_undist_points;
cv::undistortPoints(obs_undist_points,ideal_undist_points,M1,cv::Mat());
// artificially project the ideal 2d points to a plane in world coordinates
// using a linear camera model (z=1)
std::vector<cv::Point3f> world_undist_points;
for (cv::Point2f pt : ideal_undist_points)
world_undist_points.push_back(cv::Point3f(pt.x,pt.y,1));
// add the distortions back in to get the original coordinates
cv::Mat rvec = cv::Mat::zeros(3,1,CV_64FC1); // dummy zero rotation vec
cv::Mat tvec = cv::Mat::zeros(3,1,CV_64FC1); // dummy zero translation vec
std::vector<cv::Point2f> obs_dist_points;
cv::projectPoints(world_undist_points,rvec,tvec,M1,D1,obs_dist_points);
To test the results, we can compare them to the benchmark values
for(size_t i=0;i<num_rand_pts;++i)
std::cout << "benchmark_x: " << benchmark_obs_dist_points[i].x
<< " benchmark_y: " << benchmark_obs_dist_points[i].y
<< " computed_x: " << obs_dist_points[i].x
<< " computed_y: " << obs_dist_points[i].y
<< " diff_x: "
<< std::abs(benchmark_obs_dist_points[i].x-obs_dist_points[i].x)
<< " diff_y: "
<< std::abs(benchmark_obs_dist_points[i].y-obs_dist_points[i].y)
<< std::endl;
This is main.cpp. It is self-sufficient and does not need anything else but opencv. I don't remember where I found this, it works, I used it in my project. The program eats the set of standard chessboard images and generates json/xml files with all the distortions of the camera.
#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif
using namespace cv;
using namespace std;
static void help()
{
cout << "This is a camera calibration sample." << endl
<< "Usage: calibration configurationFile" << endl
<< "Near the sample file you'll find the configuration file, which has detailed help of "
"how to edit it. It may be any OpenCV supported file format XML/YAML." << endl;
}
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
{
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" << bwritePoints
<< "Write_extrinsicParameters" << bwriteExtrinsics
<< "Write_outputFileName" << outputFileName
<< "Show_UndistortedImage" << showUndistorsed
<< "Input_FlipAroundHorizontalAxis" << flipVertical
<< "Input_Delay" << delay
<< "Input" << input
<< "}";
}
void 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"] >> bwritePoints;
node["Write_extrinsicParameters"] >> bwriteExtrinsics;
node["Write_outputFileName"] >> outputFileName;
node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist;
node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint;
node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
node["Show_UndistortedImage"] >> showUndistorsed;
node["Input"] >> input;
node["Input_Delay"] >> delay;
interprate();
}
void interprate()
{
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 (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 << " Inexistent input: " << input << endl;
goodInput = false;
}
flag = 0;
if(calibFixPrincipalPoint) flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
if(calibZeroTangentDist) flag |= CV_CALIB_ZERO_TANGENT_DIST;
if(aspectRatio) flag |= CV_CALIB_FIX_ASPECT_RATIO;
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 << " Inexistent camera calibration mode: " << patternToUse << endl;
goodInput = false;
}
atImageList = 0;
}
Mat nextImage()
{
Mat result;
if( inputCapture.isOpened() )
{
Mat view0;
inputCapture >> view0;
view0.copyTo(result);
}
else if( atImageList < (int)imageList.size() )
result = imread(imageList[atImageList++], CV_LOAD_IMAGE_COLOR);
return result;
}
static bool 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;
}
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 bwritePoints; // Write detected feature points
bool bwriteExtrinsics; // Write extrinsic parameters
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 ->
int cameraID;
vector<string> imageList;
int atImageList;
VideoCapture inputCapture;
InputType inputType;
bool goodInput;
int flag;
private:
string patternToUse;
};
static void read(const FileNode& node, Settings& x, const Settings& default_value = Settings())
{
if(node.empty())
x = default_value;
else
x.read(node);
}
enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints );
int main(int argc, char* argv[])
{
// help();
Settings s;
const string inputSettingsFile = argc > 1 ? argv[1] : "default.xml";
FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
if (!fs.isOpened())
{
cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
return -1;
}
fs["Settings"] >> s;
fs.release(); // close Settings file
if (!s.goodInput)
{
cout << "Invalid input detected. Application stopping. " << endl;
return -1;
}
vector<vector<Point2f> > imagePoints;
Mat cameraMatrix, distCoeffs;
Size imageSize;
int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
clock_t prevTimestamp = 0;
const Scalar RED(0,0,255), GREEN(0,255,0);
const char ESC_KEY = 27;
for(int i = 0;;++i)
{
Mat view;
bool blinkOutput = false;
view = s.nextImage();
//----- If no more image, or got enough, then stop calibration and show result -------------
if( mode == CAPTURING && imagePoints.size() >= (unsigned)s.nrFrames )
{
if( runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints))
mode = CALIBRATED;
else
mode = DETECTION;
}
if(view.empty()) // If no more images then run calibration, save and stop loop.
{
if( imagePoints.size() > 0 )
runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints);
break;
}
imageSize = view.size(); // Format input image.
if( s.flipVertical ) flip( view, view, 0 );
vector<Point2f> pointBuf;
bool found;
switch( s.calibrationPattern ) // Find feature points on the input format
{
case Settings::CHESSBOARD:
found = findChessboardCorners( view, s.boardSize, pointBuf,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
break;
case Settings::CIRCLES_GRID:
found = findCirclesGrid( view, s.boardSize, pointBuf );
break;
case Settings::ASYMMETRIC_CIRCLES_GRID:
found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
break;
default:
found = false;
break;
}
if ( found) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
if( s.calibrationPattern == Settings::CHESSBOARD)
{
Mat viewGray;
cvtColor(view, viewGray, COLOR_BGR2GRAY);
cornerSubPix( viewGray, pointBuf, Size(11,11),
Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
}
if( mode == CAPTURING && // For camera only take new samples after delay time
(!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) )
{
imagePoints.push_back(pointBuf);
prevTimestamp = clock();
blinkOutput = s.inputCapture.isOpened();
}
// Draw the corners.
drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found );
}
//----------------------------- Output Text ------------------------------------------------
string msg = (mode == CAPTURING) ? "100/100" :
mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
int baseLine = 0;
Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10);
if( mode == CAPTURING )
{
if(s.showUndistorsed)
msg = format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames );
else
msg = format( "%d/%d", (int)imagePoints.size(), s.nrFrames );
}
putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED);
if( blinkOutput )
bitwise_not(view, view);
//------------------------- Video capture output undistorted ------------------------------
if( mode == CALIBRATED && s.showUndistorsed )
{
Mat temp = view.clone();
undistort(temp, view, cameraMatrix, distCoeffs);
}
//------------------------------ Show image and check for input commands -------------------
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;
if( s.inputCapture.isOpened() && key == 'g' )
{
mode = CAPTURING;
imagePoints.clear();
}
}
// -----------------------Show the undistorted image for the image list ------------------------
if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed )
{
Mat view, rview, map1, map2;
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
imageSize, CV_16SC2, map1, map2);
for(int i = 0; i < (int)s.imageList.size(); i++ )
{
view = imread(s.imageList[i], 1);
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;
}
}
return 0;
}
static double computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
const vector<vector<Point2f> >& imagePoints,
const vector<Mat>& rvecs, const vector<Mat>& tvecs,
const Mat& cameraMatrix , const Mat& distCoeffs,
vector<float>& perViewErrors)
{
vector<Point2f> imagePoints2;
int i, totalPoints = 0;
double totalErr = 0, err;
perViewErrors.resize(objectPoints.size());
for( i = 0; i < (int)objectPoints.size(); ++i )
{
projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
distCoeffs, imagePoints2);
err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);
int n = (int)objectPoints[i].size();
perViewErrors[i] = (float) std::sqrt(err*err/n);
totalErr += err*err;
totalPoints += n;
}
return std::sqrt(totalErr/totalPoints);
}
static void calcBoardCornerPositions(Size boardSize, float squareSize, vector<Point3f>& corners,
Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
{
corners.clear();
switch(patternType)
{
case Settings::CHESSBOARD:
case Settings::CIRCLES_GRID:
for( int i = 0; i < boardSize.height; ++i )
for( int j = 0; j < boardSize.width; ++j )
corners.push_back(Point3f(float( j*squareSize ), float( i*squareSize ), 0));
break;
case Settings::ASYMMETRIC_CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0));
break;
default:
break;
}
}
static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs,
vector<float>& reprojErrs, double& totalAvgErr)
{
cameraMatrix = Mat::eye(3, 3, CV_64F);
if( s.flag & CV_CALIB_FIX_ASPECT_RATIO )
cameraMatrix.at<double>(0,0) = 1.0;
distCoeffs = Mat::zeros(8, 1, CV_64F);
vector<vector<Point3f> > objectPoints(1);
calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern);
objectPoints.resize(imagePoints.size(),objectPoints[0]);
//Find intrinsic and extrinsic camera parameters
double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
distCoeffs, rvecs, tvecs, s.flag|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
cout << "Re-projection error reported by calibrateCamera: "<< rms << endl;
bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
return ok;
}
// Print camera parameters to the output file
static void saveCameraParams( 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 )
{
FileStorage fs( s.outputFileName, FileStorage::WRITE );
time_t tm;
time( &tm );
struct tm *t2 = localtime( &tm );
char buf[1024];
strftime( buf, sizeof(buf)-1, "%c", t2 );
fs << "calibration_Time" << buf;
if( !rvecs.empty() || !reprojErrs.empty() )
fs << "nrOfFrames" << (int)std::max(rvecs.size(), reprojErrs.size());
fs << "image_Width" << imageSize.width;
fs << "image_Height" << imageSize.height;
fs << "board_Width" << s.boardSize.width;
fs << "board_Height" << s.boardSize.height;
fs << "square_Size" << s.squareSize;
if( s.flag & CV_CALIB_FIX_ASPECT_RATIO )
fs << "FixAspectRatio" << s.aspectRatio;
if( s.flag )
{
sprintf( buf, "flags: %s%s%s%s",
s.flag & CV_CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "",
s.flag & CV_CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "",
s.flag & CV_CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "",
s.flag & CV_CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : "" );
cvWriteComment( *fs, buf, 0 );
}
fs << "flagValue" << s.flag;
fs << "Camera_Matrix" << cameraMatrix;
fs << "Distortion_Coefficients" << distCoeffs;
fs << "Avg_Reprojection_Error" << totalAvgErr;
if( !reprojErrs.empty() )
fs << "Per_View_Reprojection_Errors" << Mat(reprojErrs);
if( !rvecs.empty() && !tvecs.empty() )
{
CV_Assert(rvecs[0].type() == tvecs[0].type());
Mat bigmat((int)rvecs.size(), 6, rvecs[0].type());
for( int i = 0; i < (int)rvecs.size(); i++ )
{
Mat r = bigmat(Range(i, i+1), Range(0,3));
Mat t = bigmat(Range(i, i+1), Range(3,6));
CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
//*.t() is MatExpr (not Mat) so we can use assignment operator
r = rvecs[i].t();
t = tvecs[i].t();
}
cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
fs << "Extrinsic_Parameters" << bigmat;
}
if( !imagePoints.empty() )
{
Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2);
for( int i = 0; i < (int)imagePoints.size(); i++ )
{
Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols);
Mat imgpti(imagePoints[i]);
imgpti.copyTo(r);
}
fs << "Image_points" << imagePtMat;
}
}
bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,vector<vector<Point2f> > imagePoints )
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
bool ok = runCalibration(s,imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs,
reprojErrs, totalAvgErr);
cout << (ok ? "Calibration succeeded" : "Calibration failed")
<< ". avg re projection error = " << totalAvgErr ;
if( ok )
saveCameraParams( s, imageSize, cameraMatrix, distCoeffs, rvecs ,tvecs, reprojErrs,
imagePoints, totalAvgErr);
return ok;
}

Resources