void approx_relpose_generalized ( const Eigen::Matrix<double,6,6> &w1, const Eigen::Matrix<double,6,6> &w2, const Eigen::Matrix<double,6,6> &w3, const Eigen::Matrix<double,6,6> &w4, const Eigen::Matrix<double,6,6> &w5, const Eigen::Matrix<double,6,6> &w6, std::vector<Eigen::Vector3d> &rsolns ) { const Eigen::Matrix<double,15,35> A = computeA(w1,w2,w3,w4,w5,w6); Eigen::Matrix<double,15,35> gbA; gbA << A.col(0),A.col(1),A.col(2),A.col(3),A.col(4),A.col(5),A.col(7),A.col(9),A.col(11),A.col(15),A.col(18),A.col(21),A.col(24),A.col(28),A.col(13),A.col(6),A.col(8),A.col(10),A.col(12),A.col(16),A.col(19),A.col(22),A.col(25),A.col(29),A.col(14),A.col(17),A.col(20),A.col(23),A.col(26),A.col(30),A.col(32),A.col(27),A.col(31),A.col(33),A.col(34); const Eigen::Matrix<double,15,20> G = gbA.block<15,15>(0,0).lu().solve(gbA.block<15,20>(0,15)); Eigen::Matrix<double,20,20> M = Eigen::Matrix<double,20,20>::Zero(); M.block<10,20>(0,0) = -G.block<10,20>(5,0); M(10,4) = 1; M(11,5) = 1; M(12,6) = 1; M(13,7) = 1; M(14,8) = 1; M(15,9) = 1; M(16,13) = 1; M(17,14) = 1; M(18,15) = 1; M(19,18) = 1; const Eigen::EigenSolver< Eigen::Matrix<double,20,20> > eigensolver(M,true); const Eigen::EigenSolver< Eigen::Matrix<double,20,20> >::EigenvalueType evalues = eigensolver.eigenvalues(); const Eigen::EigenSolver< Eigen::Matrix<double,20,20> >::EigenvectorsType evecs = eigensolver.eigenvectors(); rsolns.clear(); rsolns.reserve(evalues.size()); for ( size_t i = 0; i < evalues.size(); i++ ) { if ( evalues[i].imag() != 0 ) continue; const double zsoln = evalues(i).real(); const double xsoln = evecs(16,i).real()/evecs(19,i).real(); const double ysoln = evecs(17,i).real()/evecs(19,i).real(); Eigen::Vector3d rsoln; rsoln << xsoln, ysoln, zsoln; rsolns.push_back(rsoln); } }