void StereoReconstructor::computeRTRandom(std::string cam1Folder, std::string cam2Folder) {
bool load1 = loadMatrixAndCoe(cam1Folder, camMatrix1, distCoeffs1);
bool load2 = loadMatrixAndCoe(cam2Folder, camMatrix2, distCoeffs2);
if (load1 == false || load2 == false) {
std::cout << "Load matrix and distortion failed!" << std::endl;
return;
}
std::vector<std::string> imgFiles1 = Utilities::folderImagesScan(cam1Folder.c_str());
std::vector<std::string> imgFiles2 = Utilities::folderImagesScan(cam2Folder.c_str());
cv::Mat img = cv::imread(cam1Folder + imgFiles1[0].c_str(), 1);
camImageSize = img.size();
int totalN = imgFiles1.size();
std::ofstream Tx("辅助_T_x.txt"); //x方向位移
std::ofstream Ty("辅助_T_y.txt"); //y方向位移
std::ofstream Tz("辅助_T_z.txt"); //z方向位移
std::ofstream Rx("辅助_R_x.txt"); //旋转轴x方向
std::ofstream Ry("辅助_R_y.txt"); //旋转轴y方向
std::ofstream Rz("辅助_R_z.txt"); //旋转轴z方向
std::ofstream Ra("辅助_R_a.txt"); //旋转角度
std::ofstream All("辅助_All.txt"); //所有数据
All << "Tx " << "Ty " << "Tz " << "T " << "Rx " << "Ry " << "Rz " << "Ra " << std::endl;
for (int k = 1; k <= totalN; k++) {
std::cout << k;
double tbegin = cv::getTickCount();
std::vector<int> nums;
while (nums.size() < k) {
srand(time(NULL));
int random = rand() % totalN;
if (nums.empty() || std::find(nums.begin(), nums.end(), random) == nums.end()) {
nums.push_back(random);
}
}
for (size_t i = 0; i < nums.size(); ++i) {
imgPoints1.push_back(load2DPoints(cam1Folder + imgFiles1[nums[i]].substr(0, imgFiles1[nums[i]].size() - 4) + "_imgCorners.txt"));
imgPoints2.push_back(load2DPoints(cam2Folder + imgFiles2[nums[i]].substr(0, imgFiles2[nums[i]].size() - 4) + "_imgCorners.txt"));
objPoints.push_back(load3DPoints(cam1Folder + "ObjCorners.txt"));
}
cv::Mat E, F;
cv::stereoCalibrate(objPoints, imgPoints1, imgPoints2, camMatrix1, distCoeffs1, camMatrix2, distCoeffs2, camImageSize, R, T, E, F,
cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_INTRINSIC);
//罗德里格斯（Rodrigues）变换
cv::Mat R2T(3, 1, CV_64F);
cv::Rodrigues(R, R2T);
float tx = Utilities::matGet2D(T, 0, 0);
float ty = Utilities::matGet2D(T, 0, 1);
float tz = Utilities::matGet2D(T, 0, 2);
float T = std::sqrt(tx*tx + ty*ty + tz*tz);
Tx << tx << std::endl;
Ty << ty << std::endl;
Tz << tz << std::endl;
double dx = Utilities::matGet2D(R2T, 0, 0);
double dy = Utilities::matGet2D(R2T, 0, 1);
double dz = Utilities::matGet2D(R2T, 0, 2);
double dl = std::sqrt(dx*dx + dy*dy + dz*dz);
float angle = dl * 180 / 3.1415926535897932;
float fdx = dx / dl;
float fdy = dy / dl;
float fdz = dz / dl;
Rx << fdx << std::endl;
Ry << fdy << std::endl;
Rz << fdz << std::endl;
Ra << angle << std::endl;
All << tx << " " << ty << " " << tz << " " << T << " " << fdx << " " << fdy << " " << fdz << " " << angle << std::endl;
nums.clear();
imgPoints1.clear();
imgPoints2.clear();
objPoints.clear();
double tend = cv::getTickCount();
double frequency = cv::getTickFrequency();
int span = (tend - tbegin) / frequency;
std::cout << "副图像,标定时间 " << span << ",基线长度" << T << std::endl;
}
Tx.close();
Ty.close();
Tz.close();
Rx.close();
Ry.close();
Rz.close();
Ra.close();
All.close();
}
vector<Point> Find_refer_point(vector<Point> contour_point)
{
int size = contour_point.size(); Point temp;
int i=0,j=0,k=0;
float x_sum=0,y_sum=0; float x_mean,y_mean;
float xx=0,xy=0,yx=0,yy=0; float xd,yd;
float A[2][2];
float maj1,maj2,min1,min2;
for(i=0;i<size;i++)
{
temp = contour_point[i];
x_sum = temp.x + x_sum;
y_sum = temp.y + x_sum;
}
x_mean = x_sum/(float)size; y_mean = y_sum/(float)size;
for(i=0;i<size;i++)
{
temp = contour_point[i];
xd = (float)temp.y - x_mean;
yd = (float)temp.x - y_mean;
xx = xx + (xd*xd)/(float)size;
xy = xy + (xd*yd)/(float)size;
yx = yx + (yd*xd)/(float)size;
yy = yy + (yd*yd)/(float)size;
}
A[0][0] = xx; A[0][1] = xy; A[1][0] = yx; A[1][1] = yy;
Mat CM(2,2,CV_32FC1,A);
Mat eival(2,1,CV_32FC1);
Mat eivec(2,2,CV_32FC1);
eigen(CM,eival,eivec);
maj1 = eivec.at<float>(0,0);
maj2 = eivec.at<float>(0,1);
min1 = eivec.at<float>(1,0);
min2 = eivec.at<float>(1,1);
float Head[2]={0,0}, Lfoot[2]={0,0}, Rfoot[2]={0,0};
float dummy_x, dummy_y;
float Rmin[2], Rmaj[2], Gmin[2], Gmaj[2];
Rmin[0] = (-1)*maj1; Rmin[1] = (-1)*maj2; Rmaj[0] = maj1; Rmaj[1] = maj2;
Gmin[0] = (-1)*min1; Gmin[1] = (-1)*min2; Gmaj[0] = min1; Gmaj[1] = min2;
Mat Ra(1,2,CV_32FC1,Rmin);
Mat Rb(1,2,CV_32FC1,Rmaj);
Mat Ga(1,2,CV_32FC1,Gmin);
Mat Gb(1,2,CV_32FC1,Gmaj);
Mat dumvec(1,2,CV_32FC1);
float Hmax=0,Lmax=0,Rmax=0,d_dum=0;
float dum1,dum2,dum3;
for (k = 0; k < size; k++)
{
temp = contour_point[k];
i = temp.y; j = temp.x;
dummy_x = (float)temp.y - x_mean; dummy_y = (float)temp.x - y_mean;
dumvec.at<float>(0, 0) = dummy_x;
dumvec.at<float>(0, 1) = dummy_y;
dum1 = Ra.dot(dumvec);
if (Hmax < dum1)
{
Hmax = dum1;
Head[0] = j; Head[1] = i;
}
dum2 = Ga.dot(dumvec);
if (dum2 > 0)
{
d_dum = Rb.dot(dumvec);
if (d_dum > 0)
{
if (Lmax < (d_dum + dum2))
{
Lmax = d_dum + dum2;
Lfoot[0] = j, Lfoot[1] = i;
}
}
}
dum3 = Gb.dot(dumvec);
if (dum3 > 0)
{
d_dum = Rb.dot(dumvec);
if (d_dum > 0)
{
if (Rmax < (d_dum + dum3))
{
Rmax = d_dum + dum3;
Rfoot[0] = j, Rfoot[1] = i;
}
}
}
}
vector<Point> Result(3);
Result[0].x = Head[0]; Result[0].y = Head[1];
Result[1].x = Lfoot[0]; Result[1].y = Lfoot[1];
Result[2].x = Rfoot[0]; Result[2].y = Rfoot[1];
return Result;
}
