原函数是这样的：

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;}elseCV_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函数：

真正可以运行的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 );}elsecvSetIdentity(&_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 iterativelyfor( 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 );}elsecvSetIdentity(&_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 iterativelyfor( 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;
}

完成！