// in global 3D coordinate system
bp::tuple computeShearAndBending() {
PolyVec Vx, Vy, Fz, Mx, My, Tz;
beam->shearAndBending(Vx, Vy, Fz, Mx, My, Tz);
bp::list Vx0, Vy0, Fz0, Mx0, My0, Tz0;
int n = beam->getNumNodes() - 1;
for(int i = 0; i < n; i++)
{
Vx0.append(Vx(i).eval(0.0));
Vy0.append(Vy(i).eval(0.0));
Fz0.append(Fz(i).eval(0.0));
Mx0.append(-My(i).eval(0.0)); // translate back to global coordinates
My0.append(Mx(i).eval(0.0)); // translate back to global coordinates
Tz0.append(Tz(i).eval(0.0));
}
Vx0.append(Vx(n-1).eval(1.0));
Vy0.append(Vy(n-1).eval(1.0));
Fz0.append(Fz(n-1).eval(1.0));
Mx0.append(-My(n-1).eval(1.0)); // translate back to global coordinates
My0.append(Mx(n-1).eval(1.0)); // translate back to global coordinates
Tz0.append(Tz(n-1).eval(1.0));
return bp::make_tuple(bpn::array(Vx0), bpn::array(Vy0), bpn::array(Fz0), bpn::array(Mx0), bpn::array(My0), bpn::array(Tz0));
}
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();
}
