bool mrpt::vision::pnp::rpnp::compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_) { // selecting an edge $P_{ i1 }P_{ i2 }$ by n random sampling int i1 = 0, i2 = 1; double lmin = Q(0, i1)*Q(0, i2) + Q(1, i1)*Q(1, i2) + Q(2, i1)*Q(2, i2); Eigen::MatrixXi rij (n,2); R_=Eigen::MatrixXd::Identity(3,3); t_=Eigen::Vector3d::Zero(); for (int i = 0; i < n; i++) for (int j = 0; j < 2; j++) rij(i, j) = rand() % n; for (int ii = 0; ii < n; ii++) { int i = rij(ii, 0), j = rij(ii,1); if (i == j) continue; double l = Q(0, i)*Q(0, j) + Q(1, i)*Q(1, j) + Q(2, i)*Q(2, j); if (l < lmin) { i1 = i; i2 = j; lmin = l; } } // calculating the rotation matrix of $O_aX_aY_aZ_a$. Eigen::Vector3d p1, p2, p0, x, y, z, dum_vec; p1 = P.col(i1); p2 = P.col(i2); p0 = (p1 + p2) / 2; x = p2 - p0; x /= x.norm(); if (abs(x(1)) < abs(x(2)) ) { dum_vec << 0, 1, 0; z = x.cross(dum_vec); z /= z.norm(); y = z.cross(x); y /= y.norm(); } else { dum_vec << 0, 0, 1; y = dum_vec.cross(x); y /= y.norm(); z = x.cross(y); x /= x.norm(); } Eigen::Matrix3d R0; R0.col(0) = x; R0.col(1) =y; R0.col(2) = z; for (int i = 0; i < n; i++) P.col(i) = R0.transpose() * (P.col(i) - p0); // Dividing the n - point set into(n - 2) 3 - point subsets // and setting up the P3P equations Eigen::Vector3d v1 = Q.col(i1), v2 = Q.col(i2); double cg1 = v1.dot(v2); double sg1 = sqrt(1 - cg1*cg1); double D1 = (P.col(i1) - P.col(i2)).norm(); Eigen::MatrixXd D4(n - 2, 5); int j = 0; Eigen::Vector3d vi; Eigen::VectorXd rowvec(5); for (int i = 0; i < n; i++) { if (i == i1 || i == i2) continue; vi = Q.col(i); double cg2 = v1.dot(vi); double cg3 = v2.dot(vi); double sg2 = sqrt(1 - cg2*cg2); double D2 = (P.col(i1) - P.col(i)).norm(); double D3 = (P.col(i) - P.col(i2)).norm(); // get the coefficients of the P3P equation from each subset. rowvec = getp3p(cg1, cg2, cg3, sg1, sg2, D1, D2, D3); D4.row(j) = rowvec; j += 1; if(j>n-3) break; } Eigen::VectorXd D7(8), dumvec(8), dumvec1(5); D7.setZero(); for (int i = 0; i < n-2; i++) { dumvec1 = D4.row(i); dumvec= getpoly7(dumvec1); D7 += dumvec; } Eigen::PolynomialSolver<double, 7> psolve(D7.reverse()); Eigen::VectorXcd comp_roots = psolve.roots().transpose(); Eigen::VectorXd real_comp, imag_comp; real_comp = comp_roots.real(); imag_comp = comp_roots.imag(); Eigen::VectorXd::Index max_index; double max_real= real_comp.cwiseAbs().maxCoeff(&max_index); std::vector<double> act_roots_; int cnt=0; for (int i=0; i<imag_comp.size(); i++ ) { if(abs(imag_comp(i))/max_real<0.001) { act_roots_.push_back(real_comp(i)); cnt++; } } double* ptr = &act_roots_[0]; Eigen::Map<Eigen::VectorXd> act_roots(ptr, cnt); if (cnt==0) { return false; } Eigen::VectorXd act_roots1(cnt); act_roots1 << act_roots.segment(0,cnt); std::vector<Eigen::Matrix3d> R_cum(cnt); std::vector<Eigen::Vector3d> t_cum(cnt); std::vector<double> err_cum(cnt); for(int i=0; i<cnt; i++) { double root = act_roots(i); // Compute the rotation matrix double d2 = cg1 + root; Eigen::Vector3d unitx, unity, unitz; unitx << 1,0,0; unity << 0,1,0; unitz << 0,0,1; x = v2*d2 -v1; x/=x.norm(); if (abs(unity.dot(x)) < abs(unitz.dot(x))) { z = x.cross(unity);z/=z.norm(); y=z.cross(x); y/y.norm(); } else { y=unitz.cross(x); y/=y.norm(); z = x.cross(y); z/=z.norm(); } R.col(0)=x; R.col(1)=y; R.col(2)=z; //calculating c, s, tx, ty, tz Eigen::MatrixXd D(2 * n, 6); D.setZero(); R0 = R.transpose(); Eigen::VectorXd r(Eigen::Map<Eigen::VectorXd>(R0.data(), R0.cols()*R0.rows())); for (int j = 0; j<n; j++) { double ui = img_pts(j, 0), vi = img_pts(j, 1), xi = P(0, j), yi = P(1, j), zi = P(2, j); D.row(2 * j) << -r(1)*yi + ui*(r(7)*yi + r(8)*zi) - r(2)*zi, -r(2)*yi + ui*(r(8)*yi - r(7)*zi) + r(1)*zi, -1, 0, ui, ui*r(6)*xi - r(0)*xi; D.row(2 * j + 1) << -r(4)*yi + vi*(r(7)*yi + r(8)*zi) - r(5)*zi, -r(5)*yi + vi*(r(8)*yi - r(7)*zi) + r(4)*zi, 0, -1, vi, vi*r(6)*xi - r(3)*xi; } Eigen::MatrixXd DTD = D.transpose()*D; Eigen::EigenSolver<Eigen::MatrixXd> es(DTD); Eigen::VectorXd Diag = es.pseudoEigenvalueMatrix().diagonal(); Eigen::MatrixXd V_mat = es.pseudoEigenvectors(); Eigen::MatrixXd::Index min_index; Diag.minCoeff(&min_index); Eigen::VectorXd V = V_mat.col(min_index); V /= V(5); double c = V(0), s = V(1); t << V(2), V(3), V(4); //calculating the camera pose by 3d alignment Eigen::VectorXd xi, yi, zi; xi = P.row(0); yi = P.row(1); zi = P.row(2); Eigen::MatrixXd XXcs(3, n), XXc(3,n); XXc.setZero(); XXcs.row(0) = r(0)*xi + (r(1)*c + r(2)*s)*yi + (-r(1)*s + r(2)*c)*zi + t(0)*Eigen::VectorXd::Ones(n); XXcs.row(1) = r(3)*xi + (r(4)*c + r(5)*s)*yi + (-r(4)*s + r(5)*c)*zi + t(1)*Eigen::VectorXd::Ones(n); XXcs.row(2) = r(6)*xi + (r(7)*c + r(8)*s)*yi + (-r(7)*s + r(8)*c)*zi + t(2)*Eigen::VectorXd::Ones(n); for (int ii = 0; ii < n; ii++) XXc.col(ii) = Q.col(ii)*XXcs.col(ii).norm(); Eigen::Matrix3d R2; Eigen::Vector3d t2; Eigen::MatrixXd XXw = obj_pts.transpose(); calcampose(XXc, XXw, R2, t2); R_cum[i] = R2; t_cum[i] = t2; for (int k = 0; k < n; k++) XXc.col(k) = R2 * XXw.col(k) + t2; Eigen::MatrixXd xxc(2, n); xxc.row(0) = XXc.row(0).array() / XXc.row(2).array(); xxc.row(1) = XXc.row(1).array() / XXc.row(2).array(); double res = ((xxc.row(0) - img_pts.col(0).transpose()).norm() + (xxc.row(1) - img_pts.col(1).transpose()).norm()) / 2; err_cum[i] = res; } int pos_cum = std::min_element(err_cum.begin(), err_cum.end()) - err_cum.begin(); R_ = R_cum[pos_cum]; t_ = t_cum[pos_cum]; return true; }
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points, Eigen::MatrixXf end_points, Eigen::MatrixXf directions, Eigen::MatrixXf points, Eigen::MatrixXf &rot_cw, Eigen::VectorXf &pos_cw) { int n = start_points.cols(); if(n != directions.cols()) { return; } if(n<4) { return; } float condition_err_threshold = 1e-3; Eigen::VectorXf cosAngleThreshold(3); cosAngleThreshold << 1.1, 0.9659, 0.8660; Eigen::MatrixXf optimumrot_cw(3,3); Eigen::VectorXf optimumpos_cw(3); std::vector<float> lineLenVec(n,1); vfloat3 l1; vfloat3 l2; vfloat3 nc1; vfloat3 Vw1; vfloat3 Xm; vfloat3 Ym; vfloat3 Zm; Eigen::MatrixXf Rot(3,3); std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0)); std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0)); std::vector<vfloat3> n_c(n,vfloat3(0,0,0)); Eigen::MatrixXf Rx(3,3); int line_id; for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++) { if(HowToChooseFixedTwoLines==1) { #pragma omp parallel for for(int i = 0; i < n ; i++ ) { // to correct float lineLen = 10; lineLenVec[i] = lineLen; } std::vector<float>::iterator result; result = std::max_element(lineLenVec.begin(), lineLenVec.end()); line_id = std::distance(lineLenVec.begin(), result); vfloat3 temp; temp = start_points.col(0); start_points.col(0) = start_points.col(line_id); start_points.col(line_id) = temp; temp = end_points.col(0); end_points.col(0) = end_points.col(line_id); end_points.col(line_id) = temp; temp = directions.col(line_id); directions.col(0) = directions.col(line_id); directions.col(line_id) = temp; temp = points.col(0); points.col(0) = points.col(line_id); points.col(line_id) = temp; lineLenVec[line_id] = lineLenVec[1]; lineLenVec[1] = 0; l1 = start_points.col(0) - end_points.col(0); l1 = l1/l1.norm(); } for(int i = 1; i < n; i++) { std::vector<float>::iterator result; result = std::max_element(lineLenVec.begin(), lineLenVec.end()); line_id = std::distance(lineLenVec.begin(), result); l2 = start_points.col(line_id) - end_points.col(line_id); l2 = l2/l2.norm(); lineLenVec[line_id] = 0; MatrixXf cosAngle(3,3); cosAngle = (l1.transpose()*l2).cwiseAbs(); if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines]) { break; } } vfloat3 temp; temp = start_points.col(1); start_points.col(1) = start_points.col(line_id); start_points.col(line_id) = temp; temp = end_points.col(1); end_points.col(1) = end_points.col(line_id); end_points.col(line_id) = temp; temp = directions.col(1); directions.col(1) = directions.col(line_id); directions.col(line_id) = temp; temp = points.col(1); points.col(1) = points.col(line_id); points.col(line_id) = temp; lineLenVec[line_id] = lineLenVec[1]; lineLenVec[1] = 0; // The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper, // but the framework is the same. // R_wc = (Rot') * R * Rot = (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot nc1 = x_cross(start_points.col(1),end_points.col(1)); nc1 = nc1/nc1.norm(); Vw1 = directions.col(1); Vw1 = Vw1/Vw1.norm(); //the X axis of Model frame Xm = x_cross(nc1,Vw1); Xm = Xm/Xm.norm(); //the Y axis of Model frame Ym = nc1; //the Z axis of Model frame Zm = x_cross(Xm,Zm); Zm = Zm/Zm.norm(); //Rot * [Xm, Ym, Zm] = I. Rot.col(0) = Xm; Rot.col(1) = Ym; Rot.col(2) = Zm; Rot = Rot.transpose(); //rotate all the vector by Rot. //nc_bar(:,i) = Rot * nc(:,i) //Vw_bar(:,i) = Rot * Vw(:,i) #pragma omp parallel for for(int i = 0 ; i < n ; i++) { vfloat3 nc = x_cross(start_points.col(1),end_points.col(1)); nc = nc/nc.norm(); n_c[i] = nc; nc_bar[i] = Rot * nc; Vw_bar[i] = Rot * directions.col(i); } //Determine the angle psi, it is the angle between z axis and Vw_bar(:,1). //The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);. float sinpsi= sqrt(1 - cospsi*cospsi); Rx.row(0) = vfloat3(1,0,0); Rx.row(1) = vfloat3(0,cospsi,-sinpsi); Rx.row(2) = vfloat3(0,sinpsi,cospsi); vfloat3 Zaxis = Rx * Vw_bar[1]; if(1-fabs(Zaxis[3]) > 1e-5) { Rx = Rx.transpose(); } //estimate the rotation angle phi by least square residual. //i.e the rotation matrix Rz(phi) vfloat3 Vm2 = Rx * Vw_bar[1]; float A2 = Vm2[0]; float B2 = Vm2[1]; float C2 = Vm2[2]; float x2 = (nc_bar[1])[0]; float y2 = (nc_bar[1])[1]; float z2 = (nc_bar[1])[2]; Eigen::PolynomialSolver<double, Eigen::Dynamic> solver; Eigen::VectorXf coeff(9); std::vector<float> coef(9,0); //coefficients of equation (7) Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16); //construct the polynomial F' #pragma omp parallel for for(int i = 3 ; i < n ; i++) { vfloat3 Vm3 = Rx*Vw_bar[i]; float A3 = Vm3[0]; float B3 = Vm3[1]; float C3 = Vm3[2]; float x3 = (nc_bar[i])[0]; float y3 = (nc_bar[i])[1]; float z3 = (nc_bar[i])[2]; float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3; float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3; float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3; float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3; float u15 = x2*C2*y3*A3 - y2*A2*x3*C3; float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3; float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3; float u23 = x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3; float u24 = y2*B2*z3*C3 - z2*C2*y3*B3; float u25 = y2*A2*z3*C3 - z2*C2*y3*A3; float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3; float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3; float u33 = x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3; float u34 = z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3; float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3; float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3; float a4 = u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 + u21*u21 + u22*u22 - u23*u23 -2*u21*u22 - u31*u31 - u32*u32 + u33*u33 + 2*u31*u32; float a3 =2*(u11*u14 - u13*u15 - u12*u14 + u21*u24 - u23*u25 - u22*u24 - u31*u34 + u33*u35 + u32*u34); float a2 =-2*u12*u12 + u13*u13 + u14*u14 - u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23 + u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 - u33*u33 - u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36; float a1 =2*(u12*u14 + u13*u15 + u22*u24 + u23*u25 - u32*u34 - u33*u35 - u34*u36); float a0 = u12*u12 + u15*u15+ u22*u22 + u25*u25 - u32*u32 - u35*u35 - u36*u36 - 2*u32*u36; float b3 =2*(u11*u13 - u12*u13 + u21*u23 - u22*u23 - u31*u33 + u32*u33); float b2 =2*(u11*u15 - u12*u15 + u13*u14 + u21*u25 - u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34); float b1 =2*(u12*u13 + u14*u15 + u22*u23 + u24*u25 - u32*u33 - u34*u35 - u33*u36); float b0 =2*(u12*u15 + u22*u25 - u32*u35 - u35*u36); float d0 = a0*a0 - b0*b0; float d1 = 2*(a0*a1 - b0*b1); float d2 = a1*a1 + 2*a0*a2 + b0*b0 - b1*b1 - 2*b0*b2; float d3 = 2*(a0*a3 + a1*a2 + b0*b1 - b1*b2 - b0*b3); float d4 = a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3; float d5 = 2*(a1*a4 + a2*a3 + b1*b2 + b0*b3 - b2*b3); float d6 = a3*a3 + 2*a2*a4 + b2*b2 - b3*b3 + 2*b1*b3; float d7 = 2*(a3*a4 + b2*b3); float d8 = a4*a4 + b3*b3; std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 }; Eigen::VectorXf vp; vp << a4, a3, a2, a1, a0, b3, b2, b1, b0 ; //coef = coef + v; coeff = coeff + vp; polyDF[0] = polyDF[0] + 8*d8*d8; polyDF[1] = polyDF[1] + 15* d7*d8; polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7; polyDF[3] = polyDF[3] + 13*(d5*d8 + d6*d7); polyDF[4] = polyDF[4] + 12*(d4*d8 + d5*d7) + 6*d6*d6; polyDF[5] = polyDF[5] + 11*(d3*d8 + d4*d7 + d5*d6); polyDF[6] = polyDF[6] + 10*(d2*d8 + d3*d7 + d4*d6) + 5*d5*d5; polyDF[7] = polyDF[7] + 9 *(d1*d8 + d2*d7 + d3*d6 + d4*d5); polyDF[8] = polyDF[8] + 8 *(d1*d7 + d2*d6 + d3*d5) + 4*d4*d4 + 8*d0*d8; polyDF[9] = polyDF[9] + 7 *(d1*d6 + d2*d5 + d3*d4) + 7*d0*d7; polyDF[10] = polyDF[10] + 6 *(d1*d5 + d2*d4) + 3*d3*d3 + 6*d0*d6; polyDF[11] = polyDF[11] + 5 *(d1*d4 + d2*d3)+ 5*d0*d5; polyDF[12] = polyDF[12] + 4 * d1*d3 + 2*d2*d2 + 4*d0*d4; polyDF[13] = polyDF[13] + 3 * d1*d2 + 3*d0*d3; polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2; polyDF[15] = polyDF[15] + d0*d1; } Eigen::VectorXd coefficientPoly = VectorXd::Zero(16); for(int j =0; j < 16 ; j++) { coefficientPoly[j] = polyDF[15-j]; } //solve polyDF solver.compute(coefficientPoly); const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots(); Eigen::VectorXd rs(r.rows()); Eigen::VectorXd is(r.rows()); std::vector<float> min_roots; for(int j =0;j<r.rows();j++) { rs[j] = fabs(r[j].real()); is[j] = fabs(r[j].imag()); } float maxreal = rs.maxCoeff(); for(int j = 0 ; j < rs.rows() ; j++ ) { if(is[j]/maxreal <= 0.001) { min_roots.push_back(rs[j]); } } std::vector<float> temp_v(15); std::vector<float> poly(15); for(int j = 0 ; j < 15 ; j++) { temp_v[j] = j+1; } for(int j = 0 ; j < 15 ; j++) { poly[j] = polyDF[j]*temp_v[j]; } Eigen::Matrix<double,16,1> polynomial; Eigen::VectorXd evaluation(min_roots.size()); for( int j = 0; j < min_roots.size(); j++ ) { evaluation[j] = poly_eval( polynomial, min_roots[j] ); } std::vector<float> minRoots; for( int j = 0; j < min_roots.size(); j++ ) { if(!evaluation[j]<=0) { minRoots.push_back(min_roots[j]); } } if(minRoots.size()==0) { cout << "No solution" << endl; return; } int numOfRoots = minRoots.size(); //for each minimum, we try to find a solution of the camera pose, then //choose the one with the least reprojection residual as the optimum of the solution. float minimalReprojectionError = 100; // In general, there are two solutions which yields small re-projection error // or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the // world scene behind the camera center, the other solution transforms the world // scene in front of camera center. While only the latter one is correct. // This can easily be checked by verifying their Z coordinates in the camera frame. // P_c(Z) must be larger than 0 if it's in front of the camera. for(int rootId = 0 ; rootId < numOfRoots ; rootId++) { float cosphi = minRoots[rootId]; float sign1 = sign_of_number(coeff[0] * pow(cosphi,4) + coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2) + coeff[3] * cosphi + coeff[4]); float sign2 = sign_of_number(coeff[5] * pow(cosphi,3) + coeff[6] * pow(cosphi,2) + coeff[7] * cosphi + coeff[8]); float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi)); Eigen::MatrixXf Rz(3,3); Rz.row(0) = vfloat3(cosphi,-sinphi,0); Rz.row(1) = vfloat3(sinphi,cosphi,0); Rz.row(2) = vfloat3(0,0,1); //now, according to Sec4.3, we estimate the rotation angle theta //and the translation vector at a time. Eigen::MatrixXf RzRxRot(3,3); RzRxRot = Rz*Rx*Rot; //According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we //have: scalarproduct(Vi^c, ni^c) = 0 and scalarproduct(Pi^c, ni^c) = 0. //where Vi^c = Rwc * Vi^w, Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos; //Using the above two constraints to construct linear equation system Mat about //[costheta, sintheta, tx, ty, tz, 1]. Eigen::MatrixXf Matrice(2*n-1,6); #pragma omp parallel for for(int i = 0 ; i < n ; i++) { float nxi = (nc_bar[i])[0]; float nyi = (nc_bar[i])[1]; float nzi = (nc_bar[i])[2]; Eigen::VectorXf Vm(3); Vm = RzRxRot * directions.col(i); float Vxi = Vm[0]; float Vyi = Vm[1]; float Vzi = Vm[2]; Eigen::VectorXf Pm(3); Pm = RzRxRot * points.col(i); float Pxi = Pm(1); float Pyi = Pm(2); float Pzi = Pm(3); //apply the constraint scalarproduct(Vi^c, ni^c) = 0 //if i=1, then scalarproduct(Vi^c, ni^c) always be 0 if(i>1) { Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi; Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi; Matrice(2*i-3, 5) = nyi * Vyi; } //apply the constraint scalarproduct(Pi^c, ni^c) = 0 Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi; Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi; Matrice(2*i-2, 2) = -nxi; Matrice(2*i-2, 3) = -nyi; Matrice(2*i-2, 4) = -nzi; Matrice(2*i-2, 5) = nyi * Pyi; } //solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0 using SVD, JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV); Eigen::MatrixXf VMat = svd.matrixV(); Eigen::VectorXf vec(2*n-1); //the last column of Vmat; vec = VMat.col(5); //the condition that the last element of vec should be 1. vec = vec/vec[5]; //the condition costheta^2+sintheta^2 = 1; float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]); float costheta = vec[0]*normalizeTheta; float sintheta = vec[1]*normalizeTheta; Eigen::MatrixXf Ry(3,3); Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta; //now, we get the rotation matrix rot_wc and translation pos_wc Eigen::MatrixXf rot_wc(3,3); rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot; Eigen::VectorXf pos_wc(3); pos_wc = - Rot.transpose() * vec.segment(2,4); //now normalize the camera pose by 3D alignment. We first translate the points //on line in the world frame Pw to points in the camera frame Pc. Then we project //Pc onto the line interpretation plane as Pc_new. So we could call the point //alignment algorithm to normalize the camera by aligning Pc_new and Pw. //In order to improve the accuracy of the aligment step, we choose two points for each //lines. The first point is Pwi, the second point is the closest point on line i to camera center. //(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.) Eigen::MatrixXf Pw2(3,n); Pw2.setZero(); Eigen::MatrixXf Pc_new(3,n); Pc_new.setZero(); Eigen::MatrixXf Pc2_new(3,n); Pc2_new.setZero(); for(int i = 0 ; i < n ; i++) { vfloat3 nci = n_c[i]; vfloat3 Pwi = points.col(i); vfloat3 Vwi = directions.col(i); //first point on line i vfloat3 Pci; Pci = rot_wc * Pwi + pos_wc; Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci; //second point is the closest point on line i to camera center. vfloat3 Pw2i; Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi; Pw2.col(i) = Pw2i; vfloat3 Pc2i; Pc2i = rot_wc * Pw2i + pos_wc; Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci; } MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols()); XXc << Pc_new, Pc2_new; MatrixXf XXw(points.rows(), points.cols()+Pw2.cols()); XXw << points, Pw2; int nm = points.cols()+Pw2.cols(); cal_campose(XXc,XXw,nm,rot_wc,pos_wc); pos_cw = -rot_wc.transpose() * pos_wc; //check the condition n_c^T * rot_wc * V_w = 0; float conditionErr = 0; for(int i =0 ; i < n ; i++) { float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) ); conditionErr = conditionErr + val*val; } if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3) { //check whether the world scene is in front of the camera. int numLineInFrontofCamera = 0; if(HowToChooseFixedTwoLines<3) { for(int i = 0 ; i < n ; i++) { vfloat3 P_c = rot_wc * (points.col(i) - pos_cw); if(P_c[2]>0) { numLineInFrontofCamera++; } } } else { numLineInFrontofCamera = n; } if(numLineInFrontofCamera > 0.5*n) { //most of the lines are in front of camera, then check the reprojection error. int reprojectionError = 0; for(int i =0; i < n ; i++) { //line projection function vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i)); float h1 = nc.transpose() * start_points.col(i); float h2 = nc.transpose() * end_points.col(i); float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3; reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]); } if(reprojectionError < minimalReprojectionError) { optimumrot_cw = rot_wc.transpose(); optimumpos_cw = pos_cw; minimalReprojectionError = reprojectionError; } } } } if(optimumrot_cw.rows()>0) { break; } } pos_cw = optimumpos_cw; rot_cw = optimumrot_cw; }