我目前做OPENCV双目立体匹配标定完成但匹配存在问题

[pengge0433]

在stereoRectify函数运行后会出现Formats of input arguments do not match in cvRodrigues2请好心人帮帮忙
代码：
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include <vector>
#include <time.h>
#include <cv.h>
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d//calib3d.hpp"
using namespace cv;
using namespace std;
const cv::Size imageSize = cvSize(640, 480);
struct CameraParams
{
Size imageSize = cvSize(640, 480); // 图像分辨率
Mat cameraMatrix = cvCreateMat(3, 3, CV_32FC1); // 摄像机矩阵
Mat distortionCoefficients = cvCreateMat(5, 1, CV_32FC1); // 摄像机畸变参数
}cameraParams;
struct StereoParams
{
Size imageSize = cvSize(640, 480);// 图像分辨率
CameraParams cameraParamsl; // 左摄像机标定参数
CameraParams cameraParamsr; // 右摄像机标定参数
Mat rotation = cvCreateMat(3, 3, CV_32FC1); // 旋转矩阵
Mat translation = cvCreateMat(3, 1, CV_32FC1); // 平移向量
Mat essential = cvCreateMat(3, 3, CV_32FC1); // 本质矩阵
Mat foundational = cvCreateMat(3, 3, CV_32FC1); // 基础矩阵
}stereoParams;
struct RemapMatrixs
{
Mat mXl = Mat(stereoParams.imageSize, CV_32FC1); // 左视图 X 方向像素映射矩阵
Mat mYl = Mat(stereoParams.imageSize, CV_32FC1); // 左视图 Y 方向像素映射矩阵
Mat mXr = Mat(stereoParams.imageSize, CV_32FC1); // 右视图 X 方向像素映射矩阵
Mat mYr = Mat(stereoParams.imageSize, CV_32FC1); // 右视图 Y 方向像素映射矩阵
Mat Q; // 用于计算三维点云的 Q 矩阵
Rect roil; // 左视图有效区域的矩形
Rect roir; // 右视图有效区域的矩形
}remapMatrixs;
enum RECTIFYMETHOD { RECTIFY_BOUGUET, RECTIFY_HARTLEY };
int remapImage(cv::Mat& imgl, cv::Mat& imgr, cv::Mat& imglr, cv::Mat& imgrr, RemapMatrixs& remapMatrixs)
{
if (!remapMatrixs.mXl.empty() && !remapMatrixs.mYl.empty())
{
cv::remap(imgl, imglr, remapMatrixs.mXl, remapMatrixs.mYl, cv::INTER_LINEAR);
}
if (!remapMatrixs.mXr.empty() && !remapMatrixs.mYr.empty())
{
cv::remap(imgr, imgrr, remapMatrixs.mXr, remapMatrixs.mYr, cv::INTER_LINEAR);
}

return 1;
}
int main(int argc, char*argv[])
{
IplImage*image[3] = { NULL, NULL, NULL };
CvCapture*capture[3] = { NULL, NULL, NULL };
IplImage*current_frame_rgb_l = cvCreateImage(cvSize(imageSize.width, imageSize.height), 8, 3);
IplImage*current_frame_rgb_r = cvCreateImage(cvSize(imageSize.width, imageSize.height), 8, 3);
capture[0] = cvCreateCameraCapture(0);
capture[1] = cvCreateCameraCapture(1);//左
capture[2] = cvCreateCameraCapture(2);//右
remapMatrixs.mXl = Mat(stereoParams.imageSize, CV_32FC1);
remapMatrixs.mYl = Mat(stereoParams.imageSize, CV_32FC1);
remapMatrixs.mXr = Mat(stereoParams.imageSize, CV_32FC1);
remapMatrixs.mYr = Mat(stereoParams.imageSize, CV_32FC1);
Mat Rl, Rr, Pl, Pr, Q;
Rect roil, roir;
double alpha = -1;
cvNamedWindow("Window left", 1);
cvNamedWindow("Window right", 1);
//第一组
stereoParams.cameraParamsl.cameraMatrix = (Mat_<float>(3, 3) << 677.7580, 0, 343.4333, 0, 679.5944, 249.3090, 0, 0, 1);
stereoParams.cameraParamsr.cameraMatrix = (Mat_<float>(3, 3) << 678.4570, 0, 323.0295, 0, 681.8638, 238.1935, 0, 0, 1);
stereoParams.cameraParamsl.distortionCoefficients = (Mat_<float>(5, 1) << 0.0719, -0.2109, 0.0044, 0.0059, -0.4662);
stereoParams.cameraParamsr.distortionCoefficients = (Mat_<float>(5, 1) << 0.1438, -1.2269, 0.0065, 0.0029, 4.2003);
stereoParams.rotation = (Mat_<float>(3, 3) << 0.9945, 0.0179, -0.1033, -0.0135, 0.999, 0.0431, 0.1039, -0.0415, 0.9937);
stereoParams.translation = (Mat_<float>(3, 1) << 97.3820, -0.2773, 3.5643);
//执行双目校正
stereoRectify(
stereoParams.cameraParamsl.cameraMatrix,
stereoParams.cameraParamsl.distortionCoefficients,
stereoParams.cameraParamsr.cameraMatrix,
stereoParams.cameraParamsr.distortionCoefficients,
stereoParams.imageSize,
stereoParams.rotation,
stereoParams.translation,
Rl, Rr, Pl, Pr, Q,
CV_CALIB_ZERO_DISPARITY,
alpha,
stereoParams.imageSize,
&roil, &roir);
//生成图像校正所需的像素映射矩阵
initUndistortRectifyMap(
stereoParams.cameraParamsl.cameraMatrix,
stereoParams.cameraParamsl.distortionCoefficients,
Rl, Pl, stereoParams.imageSize,
CV_16SC2, remapMatrixs.mXl, remapMatrixs.mYl);
initUndistortRectifyMap(
stereoParams.cameraParamsr.cameraMatrix,
stereoParams.cameraParamsr.distortionCoefficients,
Rr, Pr, stereoParams.imageSize,
CV_16SC2, remapMatrixs.mXr, remapMatrixs.mYr);
//输出数据
Q.copyTo(remapMatrixs.Q);
remapMatrixs.roil = roil;
remapMatrixs.roir = roir;
for (;;)
{
Mat imgl = cvQueryFrame(capture[2]);
Mat imgr = cvQueryFrame(capture[1]);
remapImage(imgl, imgr, imgl, imgr, remapMatrixs);
imshow("Window left", imgl);
imshow("Window right", imgr);
char c = cvWaitKey(20);
if (c == 27)
break;
}
}

[pengge0433]