I was doing some tests using the distortion model of OpenCV. Basically what I did is, implement the distortion equations and see if the cv::undistortPoints function gives me the inverse of these equations. I realized that cv::undistortPoints does not exactly give you the inverse of the distortion equations. When I saw this, I went to the implementation of cv::undistortPoints and realized that in the end condition of the iterative process of computing the inverse of the distortion model, OpenCV always does 5 iterations (if there are no distortion coefficients provided to the function it actually does 0 iterations) and does not use any error metric on the undistorted point to see if it is precisely undistorted. Haveing this in mind, I copied and modified the termination condition of the iteration process to take and error metrics into account. This gave me the exact inverse of the distortion model. The code showing this is attached at the end of this post. My question is:
Does this happen because OpenCV prefers performance (spending a bit less time) over accuracy (spending a bit more time) or is this just a "bug"? (it is obvious that with the termination condition that I propose the function will take more time to undistort each point)
Thank you very much!
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <iostream>
using namespace cv;
// This is a copy of the opencv implementation
void cvUndistortPoints_copy( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
const CvMat* _distCoeffs,
const CvMat* matR, const CvMat* matP )
{
double A[3][3], RR[3][3], k[8]={0,0,0,0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
CvMat _RR=cvMat(3, 3, CV_64F, RR);
const CvPoint2D32f* srcf;
const CvPoint2D64f* srcd;
CvPoint2D32f* dstf;
CvPoint2D64f* dstd;
int stype, dtype;
int sstep, dstep;
int i, j, n, iters = 1;
CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
(_src->rows == 1 || _src->cols == 1) &&
(_dst->rows == 1 || _dst->cols == 1) &&
_src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 &&
(CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
(CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
CV_Assert( CV_IS_MAT(_cameraMatrix) &&
_cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 );
cvConvert( _cameraMatrix, &matA );
if( _distCoeffs )
{
CV_Assert( CV_IS_MAT(_distCoeffs) &&
(_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
(_distCoeffs->rows*_distCoeffs->cols == 4 ||
_distCoeffs->rows*_distCoeffs->cols == 5 ||
_distCoeffs->rows*_distCoeffs->cols == 8));
_Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
cvConvert( _distCoeffs, &_Dk );
iters = 5;
}
if( matR )
{
CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 );
cvConvert( matR, &_RR );
}
else
cvSetIdentity(&_RR);
if( matP )
{
double PP[3][3];
CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
CV_Assert( CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4));
cvConvert( cvGetCols(matP, &_P3x3, 0, 3), &_PP );
cvMatMul( &_PP, &_RR, &_RR );
}
srcf = (const CvPoint2D32f*)_src->data.ptr;
srcd = (const CvPoint2D64f*)_src->data.ptr;
dstf = (CvPoint2D32f*)_dst->data.ptr;
dstd = (CvPoint2D64f*)_dst->data.ptr;
stype = CV_MAT_TYPE(_src->type);
dtype = CV_MAT_TYPE(_dst->type);
sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
n = _src->rows + _src->cols - 1;
fx = A[0][0];
fy = A[1][1];
ifx = 1./fx;
ify = 1./fy;
cx = A[0][2];
cy = A[1][2];
for( i = 0; i < n; i++ )
{
double x, y, x0, y0;
if( stype == CV_32FC2 )
{
x = srcf[i*sstep].x;
y = srcf[i*sstep].y;
}
else
{
x = srcd[i*sstep].x;
y = srcd[i*sstep].y;
}
x0 = x = (x - cx)*ifx;
y0 = y = (y - cy)*ify;
// compensate distortion iteratively
int max_iters(500);
double e(1);
for( j = 0; j < max_iters && e>0; j++ )
{
double r2 = x*x + y*y;
double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
double xant = x;
double yant = y;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
e = pow(xant - x,2)+ pow(yant - y,2);
}
double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
x = xx*ww;
y = yy*ww;
if( dtype == CV_32FC2 )
{
dstf[i*dstep].x = (float)x;
dstf[i*dstep].y = (float)y;
}
else
{
dstd[i*dstep].x = x;
dstd[i*dstep].y = y;
}
}
}
void undistortPoints_copy( InputArray _src, OutputArray _dst,
InputArray _cameraMatrix,
InputArray _distCoeffs,
InputArray _Rmat=noArray(),
InputArray _Pmat=noArray() )
{
Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat();
Mat distCoeffs = _distCoeffs.getMat(), R = _Rmat.getMat(), P = _Pmat.getMat();
CV_Assert( src.isContinuous() && (src.depth() == CV_32F || src.depth() == CV_64F) &&
((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
_dst.create(src.size(), src.type(), -1, true);
Mat dst = _dst.getMat();
CvMat _csrc = src, _cdst = dst, _ccameraMatrix = cameraMatrix;
CvMat matR, matP, _cdistCoeffs, *pR=0, *pP=0, *pD=0;
if( R.data )
pR = &(matR = R);
if( P.data )
pP = &(matP = P);
if( distCoeffs.data )
pD = &(_cdistCoeffs = distCoeffs);
cvUndistortPoints_copy(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP);
}
// Distortion model implementation
cv::Point2d distortPoint(cv::Point2d undistorted_point,cv::Mat camera_matrix, std::vector<double> distort_coefficients){
// Check that camera matrix is double
if (!(camera_matrix.type() == CV_64F || camera_matrix.type() == CV_64FC1)){
std::ostringstream oss;
oss<<"distortPoint(): Camera matrix type is wrong. It has to be a double matrix (CV_64)";
throw std::runtime_error(oss.str());
}
// Create distorted point
cv::Point2d distortedPoint;
distortedPoint.x = (undistorted_point.x - camera_matrix.at<double>(0,2))/camera_matrix.at<double>(0,0);
distortedPoint.y = (undistorted_point.y - camera_matrix.at<double>(1,2))/camera_matrix.at<double>(1,1);
// Get model
if (distort_coefficients.size() < 4 || distort_coefficients.size() > 8 ){
throw std::runtime_error("distortPoint(): Invalid numbrer of distortion coefficitnes.");
}
double k1(distort_coefficients[0]);
double k2(distort_coefficients[1]);
double p1(distort_coefficients[2]);// tangent distortion first coeficinet
double p2(distort_coefficients[3]);// tangent distortion second coeficinet
double k3(0);
double k4(0);
double k5(0);
double k6(0);
if (distort_coefficients.size() > 4)
k3 = distort_coefficients[4];
if (distort_coefficients.size() > 5)
k4 = distort_coefficients[5];
if (distort_coefficients.size() > 6)
k5 = distort_coefficients[6];
if (distort_coefficients.size() > 7)
k6 = distort_coefficients[7];
// Distort
double xcx = distortedPoint.x;
double ycy = distortedPoint.y;
double r2 = pow(xcx, 2) + pow(ycy, 2);
double r4 = pow(r2,2);
double r6 = pow(r2,3);
double k = (1+k1*r2+k2*r4+k3*r6)/(1+k4*r2+k5*r4+k5*r6);
distortedPoint.x = xcx*k + 2*p1*xcx*ycy + p2*(r2+2*pow(xcx,2));
distortedPoint.y = ycy*k + p1*(r2+2*pow(ycy,2)) + 2*p2*xcx*ycy;
distortedPoint.x = distortedPoint.x*camera_matrix.at<double>(0,0)+camera_matrix.at<double>(0,2);
distortedPoint.y = distortedPoint.y*camera_matrix.at<double>(1,1)+camera_matrix.at<double>(1,2);
// Exit
return distortedPoint;
}
int main(int argc, char** argv){
// Camera matrix
double cam_mat_da[] = {1486.58092,0,1046.72507,0,1489.8659,545.374244,0,0,1};
cv::Mat cam_mat(3,3,CV_64FC1,cam_mat_da);
// Distortion coefficients
double dist_coefs_da[] ={-0.13827409,0.29240721,-0.00088197,-0.00090189,0};
std::vector<double> dist_coefs(dist_coefs_da,dist_coefs_da+5);
// Distorted Point
cv::Point2d p0(0,0);
std::vector<cv::Point2d> p0_v;
p0_v.push_back(p0);
// Undistort Point
std::vector<cv::Point2d> ud_p_v;
cv::undistortPoints(p0_v,ud_p_v,cam_mat,dist_coefs);
cv::Point2d ud_p = ud_p_v[0];
ud_p.x = ud_p.x*cam_mat.at<double>(0,0)+cam_mat.at<double>(0,2);
ud_p.y = ud_p.y*cam_mat.at<double>(1,1)+cam_mat.at<double>(1,2);
// Redistort Point
cv::Point2d p = distortPoint(ud_p, cam_mat,dist_coefs);
// Undistort Point using own termination of iterative process
std::vector<cv::Point2d> ud_p_v_local;
undistortPoints_copy(p0_v,ud_p_v_local,cam_mat,dist_coefs);
cv::Point2d ud_p_local = ud_p_v_local[0];
ud_p_local.x = ud_p_local.x*cam_mat.at<double>(0,0)+cam_mat.at<double>(0,2);
ud_p_local.y = ud_p_local.y*cam_mat.at<double>(1,1)+cam_mat.at<double>(1,2);
// Redistort Point
cv::Point2d p_local = distortPoint(ud_p_local, cam_mat,dist_coefs);
// Display results
std::cout<<"Distorted original point: "<<p0<<std::endl;
std::cout<<"Undistorted point (CV): "<<ud_p<<std::endl;
std::cout<<"Distorted point (CV): "<<p<<std::endl;
std::cout<<"Erorr in the distorted point (CV): "<<sqrt(pow(p.x-p0.x,2)+pow(p.y-p0.y,2))<<std::endl;
std::cout<<"Undistorted point (Local): "<<ud_p_local<<std::endl;
std::cout<<"Distorted point (Local): "<<p_local<<std::endl;
std::cout<<"Erorr in the distorted point (Local): "<<sqrt(pow(p_local.x-p0.x,2)+pow(p_local.y-p0.y,2))<<std::endl;
// Exit
return 0;
}
As suggested, you could get actual motivation from the OpenCV forums. Note however that historically OpenCV has been developed with real-time or near-real-time applications in mind (for example, the Darpa Grand Challenge), hence you'll find easily code that optimizes for speed over accuracy.
In most cases 5 iterations are good enough. What is "enough" can be argued about, but for cases such as finding the optimal camera matrix one can argue that 0.1 pixel does not change much for many applications.
An important thing to note is that in some cases the function does not converge in 5 iterations. I don't know if there can be a case where it will not converge at all. This happens, for example, when the distortion parameters do not fit well the distortion, and therefore, there is no exact solution for some coordinates.
See Jensenb's answer here for a discussion.
so I am struggling to understand why I am getting this assertion error from opencv when accessing a pointer in the next col/row of an image. Let me tell you what is happening and provide some code.
I am taking a ROI from the image, which is a cv::Mat or a header to a section of a bigger cv::Mat.
I constructed some pointers to access the value of my ROI. So lets say my ROI is filled with values of pixels and is a 3x3 Mat
with the following Dimensions (index starting at 0,0)
---------
| 1 | 2 | 3 |
| 4 | 5 | 6 |
| 7 | 8 | 9 |
first of all I need to initialize my pointers to point to their positions respectively. I took the ptr function of the cv::Mat and their location in the grid via cv::Point.
Problem faced:
When I try to access the pixel of the next neighbor, I get an assertion error.
Diagnostics by me:
I thought it might be the range, but I made sure that wouldn't be the case by defining the for loop conditions according to my dimensions.
The item I am trying to access doesn't exist, but how I understand it when I go through the ROI, i already have the values in a new Matrix and I should be able to access all values around my desired pixels.
PART OF THE CODE:
cv::Mat ROI =disTrafo(cv::Rect(cv::Point(x,y),cv::Size(3,3)));
cv::minMaxLoc(ROI,&minVal,&maxVal,&minCoord,&maxCoord);
auto* maxPtr_x = &maxCoord.x;
auto* maxPtr_y = &maxCoord.y;
auto* maxPtr_value = &maxVal;
uchar diff1 = 0;
uchar diff2= 0;
uchar diff3 = 0;
uchar diff4 = 0;
uchar max_diff = 0;
for(int j = 1; j < ROI.rows ; j++){
auto current = ROI.ptr<uchar>(maxCoord.y);
auto neighbor_down = ROI.ptr<uchar>(maxCoord.y+1); //THE PROB IS HERE according to debugging
auto neighbor_up = ROI.ptr<uchar>(maxCoord.y-1);
cv::Point poi ; //point of interest
for(int i= 0; i < ROI.cols; i++){
switch(maxCoord.x){ //PROOF FOR LOGIC
case 0:
if(maxCoord.y == 0){ //another switch statement maybe ??
diff1 = std::abs(current[maxCoord.x+1] - current[maxCoord.x]);
diff2 = std::abs(neighbor_down[maxCoord.x] - current[maxCoord.x]);
if(diff2 > diff1){
cv::Point(maxCoord.x,maxCoord.y+1) = poi;
} else {
cv::Point(maxCoord.x+1,maxCoord.y) = poi;
}
};
ASSERTION FAILED when running it: OpenCV Error: Assertion failed(y == 0|| < data && dims >= 1 && (unsigned)y < (unsigned)size.p[0])) in cv::Mat::ptr, file //... indicates path to header file mat.hpp// line 428
I can't put my finger on the problem, could you please be of assistance. And please give me some knowledge when working with pointers and pixels in case I misunderstood something.
Thank you
try this
for(int j = 1; j < ROI.rows ; j++){
auto current = ROI.ptr<uchar>(maxCoord.y);
auto neighbor_down = ROI.ptr<uchar>(maxCoord.y+1); //THE PROB IS HERE according to debugging
auto neighbor_up = ROI.ptr<uchar>(maxCoord.y-1);
cv::Point poi ; //point of interest
cv::Point bordess(Point(0,0));
for(int i= 0; i < ROI.cols; i++){
switch(maxCoord.x){ //PROOF FOR LOGIC
case 0:
if(maxCoord.y == 0){ //another switch statement maybe ??
diff1 = std::abs(current[maxCoord.x+1] - current[maxCoord.x]);
diff2 = std::abs(neighbor_down[maxCoord.x] - current[maxCoord.x]);
if(diff2 > diff1){
cv::Point(maxCoord.x,maxCoord.y+1) = poi & bordess;
} else {
cv::Point(maxCoord.x+1,maxCoord.y) = poi & bordess;
}
};
Ok so basically I figured out that my pointer definition was wrong due to the nature of the input image. I have done some preprocessing with the image and the ranges of the values inside changed from ucharto some other value. When I changed the auto neighbor_down = ROI.ptr<uchar>(maxCoord.y+1);for example to auto neighbor_down = ROI.ptr<float>(maxCoord.y+1); everything ran normal.
I need to construct a gaussian filter using C++. I am successfully getting my pixels from the image and constructing both grids; the one with 1 in the center and the one with one in the corner, my problem is I don't quit understand the gaussian equation! what should I put in the pixel that I should put in the output Image?? What should I put in else? Should I sum all the pixels from the modified grid (The one with 1 in the corner) or what?
for ( int r = 0; r < rowCt; r++ )
{
for ( int c = 0; c < colCt; c++ )
{
getPixel ( r, c, onePix );// geting pixel from the Img
int index = getIndex(r,c);// change colomns and rows to one dimentional array
if(c <= (f/2) || c >= colCt-((f/2)-1)-1 || r >= rowCt-((r/2)-1)-1 || r <= ((r/2)-1))
onePix = 0;// black if in the border
else
{
.... // ??
}
outIm.setPixel ( r, c, outPut_Pixel);
}
}
}
Been chasing this bug all night, so please forgive any incoherence.
I'm attempting to use the OpenCV's calibrateCamera() to extract intrinsic and extrinsic parameters from a set of fifteen pictures whose object points and world points are given. From what I can tell from debugging, I'm grabbing valid points from the input files and placing them in a vector<Point3f>, which is itself placed into another vector.
I pass the whole shebang to calibrateCamera(),
double rms = calibrateCamera(worldPoints, pixelPoints, src.size(), intrinsic, distCoeffs, rvecs, tvecs);
which throws Assertion failed (ni >= 0) in unknown function, file ...\calibration.cpp, line 3173
Pulling up this file gives us
static void collectCalibrationData( InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints1,
InputArrayOfArrays imagePoints2,
Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2,
Mat& npoints )
{
int nimages = (int)objectPoints.total();
int i, j = 0, ni = 0, total = 0;
CV_Assert(nimages > 0 && nimages == (int)imagePoints1.total() &&
(!imgPtMat2 || nimages == (int)imagePoints2.total()));
for( i = 0; i < nimages; i++ )
{
ni = objectPoints.getMat(i).checkVector(3, CV_32F);
CV_Assert( ni >= 0 );
total += ni;
}
...
So far as I know, a Point3f is of CV_32F depth, and I can see good data in the double vector just before calibrateCamera is called.
Any ideas what might be happening here? calibrateCamera() requires a vector<vector<Point3f>>, as said by http://aishack.in/tutorials/calibrating-undistorting-with-opencv-in-c-oh-yeah/ and the documentation; hopefully getMat(i) isn't failing due to that.
Could it possibly have been called on the vector<vector<Point2f>> of pixel points just after it? I have been over so many errors I am willing to believe anything.
Edit:
Consequently, checkVector()'s documentation was not really helpful
int cv::Mat::checkVector (int elemChannels, int depth = -1, bool RequireContinuous = true) const
returns N if the matrix is 1-channel (N x ptdim) or ptdim-channel (1 x N) or (N x 1); negative number otherwise
The problem is possibly in one of your InputArrayOfArrays arguments (in worldPoints precisely, if the assertion is thrown from the line pasted in your question). Mat:s should work just fine here.
I solved the same assertion error in my code by making all the 3 InputArrayOfArrays (or vector > and vector > in my case) same length vectors with fully populated entries. So my problem was in my architecture: my objectPoints vector was containing empty entries (even though the existing data was valid), and calibrate.cpp requires that no empty entries are present in any of the 3 InputArrayOfArrays. Btw I am using greyscale images for calibration so single channel data.
In calib3d source the most probable reason for throwing error is a null value if you have checked that data types match. You might try double-checking your valid input data:
1) count the # of valid calibration images from your chosen structure
validCalibImages = (int)goodCalibrationImages.size()
2) define worldPoints as vector<vector<Point3f> > worldPoints
3) IMPORTANT: resize to accommodate for data for each calibration entry
worldPoints.resize(validCalibImages)
4) populate with data e.g.
for(int k = 0; k < (int)goodCalibImages.size(); k++){
for(int i = 0; i < chessboardSize.height; i++){
for(int j = 0; j < chessboardSize.width; j++){
objectPoints[k].push_back(Point3f(i*squareSize, j*squareSize, 0));
}
}
}
'
Hope it helps!
I agree with FSaccilotto - call checkVector and make sure you are passing a vector of size n of Mat 1x1:{3 channel} and not vector of Mat 1 x n:{3 channel} or worse Mat 1 x n:{2 channel} which is what MatOfPoint spits out. That usually fixes 90% of assert failed issues. Explicitly declare the Mat yourself.
The object pattern is somewhat strange in that the x y z coords are in the channels not in the Mat dimensions.
I'm working with OpenCV SURF implementation.
I have found that keypoints, that are obtained by using if the SurfFeatureDetector with default parameters never found at octave 0 independently of the image.
Can somebody explain this result?
It looks like a bug in the following function (it called from SURF::operator() in surf.cpp):
static int getPointOctave(const CvSURFPoint& kpt, const CvSURFParams& params)
{
int octave = 0, layer = 0, best_octave = 0;
float min_diff = FLT_MAX;
for( octave = 1; octave < params.nOctaves; octave++ ) <---- octave counter starts from 1
for( layer = 0; layer < params.nOctaveLayers; layer++ )
{
float diff = std::abs(kpt.size - (float)((HAAR_SIZE0 + HAAR_SIZE_INC*layer) << octave));
if( min_diff > diff )
{
min_diff = diff;
best_octave = octave;
if( min_diff == 0 )
return best_octave;
}
}
return best_octave;
}
At the code that is written above, we can see that octave counter starts from 1. In accordance to the initialization of min_diff = FLT_MAX, the diff variable is obviously less than min_diff.
So, in case of nOctaves > 1, result best_octave will never be 0 even if we actually found keypoint at octave 0.
EDIT: This issue is fixed in the next release of the OpenCV (2.4)