多点透视cvSolvePNP的替代函数
在调试JNI程序时，所有的Shell都已经加载完成，而唯一真正核心的cv::SolvePnP却不能在JNI里面获得通行证，经过反复测试都不能运行，因此只能忍痛舍弃，自行编写一个具有相似功能的函数对其进行替代。
元函数是这样的：
void rec3D::reconstruct3D(const vector<Point2f>& image_points, Mat& rvecs, Mat& tvecs)
{
print_Matf( cam.intrinsic_Mat );
print_Matf( cam.disCoeff );
print_VecPointf(image_points);
print_VecPointf(cam.object_points );
cv::solvePnP(cam.object_points , image_points , cam.intrinsic_Mat , cam.disCoeff , rvecs , tvecs , false , CV_P3P);
print_Matf( cam.intrinsic_Mat );
print_Matf( cam.disCoeff );
print_VecPointf(image_points);
print_VecPointf(cam.object_points );
print_Matf( rvecs );
print_Matf( tvecs );
}
当然，函数后面的测试时没有机会调用的，而函数前面的测试则是准确无误的....
1. 关于多点投射的函数cv::solvePnP
参考链接：http://blog.csdn.net/abc20002929/article/details/8520063
Wiki链接：CV照相机 标定和三维重建 详细资料
计算多点透视问题，有离散的方法和解方程的方法，分别为cvPOSIT 和SolePnP。
应用场景：给定物体3D点集与对应的图像2D点集，之后进行姿态计算(即求旋转与位移矩阵)。
输入输出：
1.输入都是3D点集和对应的2D点集，其中cvPOSIT的3D点包含在posit_object结构中
2.输出均包括旋转矩阵和位移向量
形式不同：
solvePnP有摄像机的一些内参
语义不同：
除了一堆数据类型检查和转化外，其实solvePnP调用的是cvFindExtrinsicCameraParams2通过已知的内参进行未知外参求解，是一个精确解；而cvPOSIT是用仿射投影模型近似透视投影模型下，不断迭代计算出来的估计值(在物体深度变化相对于物体到摄像机的距离比较大的时候,这种算法可能不收敛)。
2. 函数原型
Finds an object pose from 3D-2D point correspondences.
solvePnP(http://docs.opencv.org/modules/calib3d /doc/camera_calibration_and_3d_reconstruction.html#solvepnp)
bool solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )
cvPOSIT(http://www.opencv.org.cn/index.php/Cv%E7%85%A7%E7%9B%B8%E6%9C%BA%E5%AE%9A%E6%A0%87%E5%92%8C%E4%B8%89%E7%BB%B4%E9%87%8D%E5%BB%BA#POSIT)
执行POSIT算法
void cvPOSIT( CvPOSITObject* posit_object, CvPoint2D32f* image_points, >double focal_length, CvTermCriteria criteria, CvMatr32f rotation_matrix, CvVect32f translation_vector );
3.Cv2.410的源代码
OpenCV参考手册第392页
bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
{
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
_rvec.create(3, 1, CV_64F);
_tvec.create(3, 1, CV_64F);
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
if (flags == CV_EPNP)
{
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
epnp PnP(cameraMatrix, opoints, undistortedPoints);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
PnP.compute_pose(R, tvec);
cv::Rodrigues(R, rvec);
return true;
}
else if (flags == CV_P3P)
{
CV_Assert( npoints == 4);
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
p3p P3Psolver(cameraMatrix);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
if (result)
cv::Rodrigues(R, rvec);
return result;
}
else if (flags == CV_ITERATIVE)
{
CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat();
cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
&c_rvec, &c_tvec, useExtrinsicGuess );
return true;
}
else
CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE or CV_EPNP");
return false;
}有效代码：
else if (flags == CV_P3P)
{
CV_Assert( npoints == 4);
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
p3p P3Psolver(cameraMatrix);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
if (result)
cv::Rodrigues(R, rvec);
return result;
}
4. 函数mySolvePnP()
int rec3D::mySolvePnP(
//vector<cv::Point3f> &_opoints , vector<cv::Point2f> &_ipoints ,
InputArray &_opoints , InputArray &_ipoints ,
//cv::Mat &intrinsic_Mat , cv::Mat &disCoeff ,
InputArray &_cameraMatrix , InputArray &_distCoeffs ,
//cv::Mat &rvecs , cv::Mat &tvecs ,
OutputArray &_rvec , OutputArray &_tvec ,
bool useExtrinsicGuess, int flags)
{
cv::Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
_rvec.create(3, 1, CV_64F);
_tvec.create(3, 1, CV_64F);
cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
{
CV_Assert( npoints == 4);
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
p3p P3Psolver(cameraMatrix);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
if (result)
cv::Rodrigues(R, rvec);
return result;
}
return true;
}
暂时未完成，待修改...................
真正可以运行的JNI函数：
int rec3D::mySolvePnP(
vector<cv::Point3f> &_opoints ,const vector<cv::Point2f> &_ipoints ,
//InputArray &_opoints , InputArray &_ipoints ,
cv::Mat &intrinsic_Mat , cv::Mat &disCoeff ,
//InputArray &_cameraMatrix , InputArray &_distCoeffs ,
cv::Mat &_rvecs , cv::Mat &_tvecs ,
//OutputArray &_rvec , OutputArray &_tvec ,
bool useExtrinsicGuess, int flags)
{
cv::Mat opoints = Mat(_opoints, true);// _opoints.getMat();
cv::Mat ipoints = Mat(_ipoints , true);///_ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));//The error source! wishchin !
CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
//_rvecs= new Mat(3, 1, CV_64F);//_rvec.create(3, 1, CV_64F);
//_tvecs= new Mat(3, 1, CV_64F);//.create(3, 1, CV_64F);
//cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
cv::Mat cameraMatrix = intrinsic_Mat.clone();
cv::Mat distCoeffs = disCoeff.clone();
//{
CV_Assert( npoints == 4);
cv::Mat undistortedPoints(ipoints.rows,ipoints.cols, ipoints.type() );
//undistortedPoints 为输出参数！
undistortPointsMy(ipoints, undistortedPoints, cameraMatrix, distCoeffs);//undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
//1.修改函数
//p3pMy P3Psolver= new p3pMy(cameraMatrix);
p3pMy P3Psolver(cameraMatrix);
//cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
//cv::Mat R = _rvecs.clone();
//cv::Mat rvec = _rvecs.clone();
//cv::Mat tvec = _tvecs.clone();
////2.修改函数
//bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
cv::Mat R;// = _rvecs.clone();
//cv::Mat rvec = _rvecs.clone();
//cv::Mat tvec = _tvecs.clone();
//2.修改函数
bool result = P3Psolver.solve(_rvecs, _tvecs, opoints, undistortedPoints);
if (result) cv::Rodrigues(_rvecs, R);//cv::Rodrigues(R, rvec);
//return result;
//}
return true;
}
//void rec3D::undistortPointsMy( InputArray _src, OutputArray _dst,
// InputArray _cameraMatrix,
// InputArray _distCoeffs,
// InputArray _Rmat,
// InputArray _Pmat )
void rec3D::undistortPointsMy(
cv::Mat &_src, cv::Mat &_dst,
cv::Mat &_cameraMatrix,
cv::Mat &_distCoeffs)
{
cv::Mat src =_src;// _src.getMat();
cv::Mat cameraMatrix = _cameraMatrix;//_cameraMatrix.getMat();
cv::Mat distCoeffs = _distCoeffs;//_distCoeffs.getMat();//, R = _Rmat.getMat(), P = _Pmat.getMat();
cv::Mat R;
cv::Mat P;
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;//_dst.getMat();
//Fuck！引用传来传去的！ dst矩阵！
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);
//Fuck！引用传来传去的！ dst矩阵！
//cvUndistortPoints(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP);
cvUndistortPointsMyRf(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP);
}
//逻辑混乱！！！
void rec3D::cvUndistortPointsMy(
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
for( j = 0; j < iters; 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;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
}
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 rec3D::cvUndistortPointsMyRf(
const CvMat* _src, CvMat* _dst,
const CvMat* _cameraMatrix,
const CvMat* _distCoeffs,
const CvMat* matR,
const CvMat* matP )
{
double A[3][3], RR[3][3];
double k[8]={0,0,0,0,0,0,0,0};
double fx, fy, ifx, ify, cx, cy;
CvMat matA=cvMat(3, 3, CV_64F, A);
CvMat _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( src, dst );
//cvConvert( _cameraMatrix, &matA );//这一句关系到画三维坐标系，但看起来毫无用处！
cvConvertMy( _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 );
cvConvertMy(_distCoeffs, &_Dk );
iters = 5;
}
if( matR )
{
CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 );
//cvConvert( matR, &_RR );
cvConvertMy(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 );
cvConvertMy( cvGetCols(matP, &_P3x3, 0, 3), &_PP );
cvMatMul( &_PP, &_RR, &_RR );
}
srcf = (const CvPoint2D32f*)_src->data.ptr;
srcd = (const CvPoint2D64f*)_src->data.ptr;
//dst的指针引用！
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
for( j = 0; j < iters; 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;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
}
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;
}
}
}
//bool rec3D::cvConvertMy(const CvMat* _cameraMatrix,double matA[][3] )
//{
//
// return true;
//}
bool rec3D::cvConvertMy(const CvMat* Mat, CvMat* MatA )
{
int C = Mat->step/ Mat->cols/sizeof(Mat->type);
for (int row = 0; row < Mat->rows && row < MatA->rows; row++){
float* ptr = (float*)(Mat->data.ptr + row * Mat->step);//第row行数据的起始指针
float* ptrD = (float*)(MatA->data.ptr + row * MatA->step);
for (int col = 0; col < Mat->cols && col < MatA->cols; col++)
{
for (int i=0;i<C;++i)
{
*(ptrD+C*col+i) = *(ptr+C*col+i);
}
//*(ptrD+3*col) = *(ptr+3*col);//cout<<*(ptr+3*col)<<endl;
//*(ptrD+3*col+1) = *(ptr+3*col+1);//cout<<*(ptr+3*col+1)<<endl;
//*(ptrD+3*col+2) = *(ptr+3*col+2);//cout<<*(ptr+3*col+2)<<endl;
}
}
return true;
}
完成！