double RotationAverage::chordal(Sophus::SO3d *avg) { double max_angle=0; Eigen::Matrix4d Q; Q.setZero(); auto rest = rotations.begin(); rest++; for(auto && t: zip_range(weights, rotations)) { double w=t.get<0>(); Sophus::SO3d& q = t.get<1>(); Q += w * q.unit_quaternion().coeffs() * q.unit_quaternion().coeffs().transpose(); max_angle = std::accumulate(rest, rotations.end(), max_angle, [&q](double max, const Sophus::SO3d& other) { return std::max(max, q.unit_quaternion().angularDistance(other.unit_quaternion())); }); } Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> solver; solver.compute(Q); Eigen::Vector4d::Map(avg->data()) = solver.eigenvectors().col(3); return max_angle; }
void Foam::mosDMDEigenBase::realSymmEigenSolver(const Eigen::MatrixXd& M, Eigen::DiagonalMatrix<scalar, Eigen::Dynamic>& S, Eigen::MatrixXd& U) { // Solve eigenvalues and eigenvectors Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver; eigenSolver.compute(M); // Sort eigenvalues and corresponding eigenvectors // in descending order SortableList<scalar> sortedList(M.rows()); forAll (sortedList, i) { sortedList[i] = eigenSolver.eigenvalues()[i]; } // Do sort sortedList.sort(); label n = 0; forAllReverse(sortedList, i) { S.diagonal()[n] = sortedList[i]; U.col(n) = eigenSolver.eigenvectors().col(sortedList.indices()[i]); n++; }
/// Draw ellipsoid void QGLVisualizer::drawEllipsoid(const Vec3& pos, const Mat33& covariance) const{ Eigen::SelfAdjointEigenSolver<Mat33> es; es.compute(covariance); Mat33 V(es.eigenvectors()); double GLmat[16]={V(0,0), V(1,0), V(2,0), 0, V(0,1), V(1,1), V(2,1), 0, V(0,2), V(1,2), V(2,2), 0, pos.x(), pos.y(), pos.z(), 1}; glPushMatrix(); glMultMatrixd(GLmat); drawEllipsoid(10,10,sqrt(es.eigenvalues()(0))*config.ellipsoidScale, sqrt(es.eigenvalues()(1))*config.ellipsoidScale, sqrt(es.eigenvalues()(2))*config.ellipsoidScale); glPopMatrix(); }
Eigen::Vector3f FitNormal(const std::vector<T>& points, F f) { // return Eigen::Vector3f(0.0f, 0.0f, 1.0f); // compute covariance matrix Eigen::Matrix3f A = PointCovariance(points, f); // compute eigenvalues/-vectors Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver; solver.compute(A); // take eigenvector (first eigenvalue is smallest!) return solver.eigenvectors().col(0).normalized(); }
void computeEigenReferenceSolution ( size_t nr_problems, const Vcl::Core::InterleavedArray<float, 3, 3, -1>& ATA, Vcl::Core::InterleavedArray<float, 3, 3, -1>& U, Vcl::Core::InterleavedArray<float, 3, 1, -1>& S ) { // Compute reference using Eigen for (int i = 0; i < static_cast<int>(nr_problems); i++) { Vcl::Matrix3f A = ATA.at<float>(i); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver; solver.compute(A, Eigen::ComputeEigenvectors); U.at<float>(i) = solver.eigenvectors(); S.at<float>(i) = solver.eigenvalues(); } }
void drawBoundingBox(PointCloudT::Ptr cloud, boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer, int z) { //Eigen::Vector4f centroid; pcl::compute3DCentroid(*cloud, centroid); //Eigen::Matrix3f covariance; computeCovarianceMatrixNormalized(*cloud, centroid, covariance); //Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, //Eigen::ComputeEigenvectors); eigen_solver.compute(covariance,Eigen::ComputeEigenvectors); // eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver> // (covariance,Eigen::ComputeEigenvectors); eigDx = eigen_solver.eigenvectors(); eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1)); //Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity()); p2w.block<3,3>(0,0) = eigDx.transpose(); p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>()); //pcl::PointCloud<PointT> cPoints; pcl::transformPointCloud(*cloud, cPoints, p2w); //PointT min_pt, max_pt; pcl::getMinMax3D(cPoints, min_pt, max_pt); const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap()); const Eigen::Quaternionf qfinal(eigDx); const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>(); //viewer->addPointCloud(cloud); viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200)); }
std::list< LieGroup > confidence_region_contours(const NormalDistributionOn<LieGroup>& N, int dim, typename LieGroup::Scalar confidence) { using namespace std; list< typename LieGroup::Tangent > sphere = sample_sphere< typename LieGroup::Scalar, LieGroup::DoF >(50, dim); boost::math::chi_squared_distribution<typename LieGroup::Scalar> chi2( LieGroup::DoF ); double scale = sqrt( boost::math::quantile(chi2, confidence) ) ; Eigen::SelfAdjointEigenSolver< typename NormalDistributionOn< LieGroup >::Covariance > eigs; eigs.compute(N.covariance()); typename NormalDistributionOn<SE2d>::Covariance sqrt_cov = eigs.eigenvectors() * eigs.eigenvalues().array().sqrt().matrix().asDiagonal(); std::list< LieGroup > out; for( typename list< typename LieGroup::Tangent >::iterator it = sphere.begin(); it != sphere.end(); it++ ) { out.push_back( N.mean() * LieGroup::exp( scale* sqrt_cov * (*it) ) ); } return out; }
typename GaussianProcess<TScalarType>::MatrixType GaussianProcess<TScalarType>::InvertKernelMatrix(const typename GaussianProcess<TScalarType>::MatrixType &K, typename GaussianProcess<TScalarType>::InversionMethod inv_method = GaussianProcess<TScalarType>::FullPivotLU, bool stable) const{ // compute core matrix if(debug){ std::cout << "GaussianProcess::InvertKernelMatrix: inverting kernel matrix... "; std::cout.flush(); } typename GaussianProcess<TScalarType>::MatrixType core; switch(inv_method){ // standard method: fast but not that accurate // Uses the LU decomposition with full pivoting for the inversion case FullPivotLU:{ if(debug) std::cout << " (inversion method: FullPivotLU) " << std::flush; try{ if(stable){ core = K.inverse(); } else{ if(debug) std::cout << " (using lapack) " << std::flush; core = lapack::lu_invert<TScalarType>(K); } } catch(lapack::LAPACKException& e){ core = K.inverse(); } } break; // very accurate and very slow method, use it for small problems // Uses the two-sided Jacobi SVD decomposition case JacobiSVD:{ if(debug) std::cout << " (inversion method: JacobiSVD) " << std::flush; Eigen::JacobiSVD<MatrixType> jacobisvd(K, Eigen::ComputeThinU | Eigen::ComputeThinV); if((jacobisvd.singularValues().real().array() < 0).any() && debug){ std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues."; std::cout.flush(); } core = jacobisvd.matrixV() * VectorType(1/jacobisvd.singularValues().array()).asDiagonal() * jacobisvd.matrixU().transpose(); } break; // accurate method and faster than Jacobi SVD. // Uses the bidiagonal divide and conquer SVD case BDCSVD:{ if(debug) std::cout << " (inversion method: BDCSVD) " << std::flush; #ifdef EIGEN_BDCSVD_H Eigen::BDCSVD<MatrixType> bdcsvd(K, Eigen::ComputeThinU | Eigen::ComputeThinV); if((bdcsvd.singularValues().real().array() < 0).any() && debug){ std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues."; std::cout.flush(); } core = bdcsvd.matrixV() * VectorType(1/bdcsvd.singularValues().array()).asDiagonal() * bdcsvd.matrixU().transpose(); #else // this is checked, since BDCSVD is currently not in the newest release throw std::string("GaussianProcess::InvertKernelMatrix: BDCSVD is not supported by the provided Eigen library."); #endif } break; // faster than the SVD method but less stable // computes the eigenvalues/eigenvectors of selfadjoint matrices case SelfAdjointEigenSolver:{ if(debug) std::cout << " (inversion method: SelfAdjointEigenSolver) " << std::flush; try{ core = lapack::chol_invert<TScalarType>(K); } catch(lapack::LAPACKException& e){ Eigen::SelfAdjointEigenSolver<MatrixType> es; es.compute(K); VectorType eigenValues = es.eigenvalues().reverse(); MatrixType eigenVectors = es.eigenvectors().rowwise().reverse(); if((eigenValues.real().array() < 0).any() && debug){ std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues."; std::cout.flush(); } core = eigenVectors * VectorType(1/eigenValues.array()).asDiagonal() * eigenVectors.transpose(); } } break; } if(debug) std::cout << "[done]" << std::endl; return core; }
int Fit(Vector& res_G, // residual under NULL -- may change when permuting Vector& v_G, // variance under NULL -- may change when permuting Matrix& X_G, // covariance Matrix& G_G, // genotype Vector& w_G) // weight { this->nPeople = X_G.rows; this->nMarker = G_G.cols; this->nCovariate = X_G.cols; // calculation w_sqrt G_to_Eigen(w_G, &this->w_sqrt); w_sqrt = w_sqrt.cwiseSqrt(); // calculate K = G * W * G' Eigen::MatrixXf G; G_to_Eigen(G_G, &G); this->K_sqrt.noalias() = w_sqrt.asDiagonal() * G.transpose(); // calculate Q = ||res * K|| Eigen::VectorXf res; G_to_Eigen(res_G, &res); this->Q = (this->K_sqrt * res).squaredNorm(); // calculate P0 = V - V X (X' V X)^(-1) X' V Eigen::VectorXf v; G_to_Eigen(v_G, &v); if (this->nCovariate == 1) { P0 = -v * v.transpose() / v.sum(); // printf("dim(P0) = %d, %d\n", P0.rows(), P0.cols()); // printf("dim(v) = %d\n", v.size()); P0.diagonal() += v; // printf("dim(v) = %d\n", v.size()); } else { Eigen::MatrixXf X; G_to_Eigen(X_G, &X); Eigen::MatrixXf XtV; // X^t V XtV.noalias() = X.transpose() * v.asDiagonal(); P0 = -XtV.transpose() * ((XtV * X).inverse()) * XtV; P0.diagonal() += v; } // dump(); // Eigen::MatrixXf tmp = K_sqrt * P0 * K_sqrt.transpose(); // dumpToFile(tmp, "out.tmp"); // eigen decomposition Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es; es.compute(K_sqrt * P0 * K_sqrt.transpose()); #ifdef DEBUG std::ofstream k("K"); k << K_sqrt; k.close(); #endif // std::ofstream p("P0"); // p << P0; // p.close(); this->mixChiSq.reset(); int r_ub = std::min(nPeople, nMarker); int r = 0; // es.eigenvalues().size(); int eigen_len = es.eigenvalues().size(); for (int i = eigen_len - 1; i >= 0; i--) { if (es.eigenvalues()[i] > ZBOUND && r < r_ub) { this->mixChiSq.addLambda(es.eigenvalues()[i]); r++; } else break; } // calculate p-value this->pValue = this->mixChiSq.getPvalue(this->Q); if (this->pValue == 0.0 || this->pValue == 1.0) { this->pValue = this->mixChiSq.getLiuPvalue(this->Q); } return 0; };
void PointWithNormalStatistcsGenerator::computeNormalsAndSVD(PointWithNormalVector& points, PointWithNormalSVDVector& svds, const Eigen::MatrixXi& indices, const Eigen::Matrix3f& cameraMatrix, const Eigen::Isometry3f& transform){ _integralImage.compute(indices,points); int q=0; int outerStep = _numThreads * _step; PixelMapper pixelMapper; pixelMapper.setCameraMatrix(cameraMatrix); pixelMapper.setTransform(transform); Eigen::Isometry3f inverseTransform = transform.inverse(); #pragma omp parallel { #ifdef _PWN_USE_OPENMP_ int threadNum = omp_get_thread_num(); #else // _PWN_USE_OPENMP_ int threadNum = 0; Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver; #endif // _PWN_USE_OPENMP_ for (int c=threadNum; c<indices.cols(); c+=outerStep) { #ifdef _PWN_USE_OPENMP_ Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver; #endif // _PWN_USE_OPENMP_ for (int r=0; r<indices.rows(); r+=_step, q++){ int index = indices(r,c); //cerr << "index(" << r <<"," << c << ")=" << index << endl; if (index<0) continue; // determine the region PointWithNormal& point = points[index]; PointWithNormalSVD& originalSVD = svds[index]; PointWithNormalSVD svd; Eigen::Vector3f normal = point.normal(); Eigen::Vector3f coord = pixelMapper.projectPoint(point.point()+Eigen::Vector3f(_worldRadius, _worldRadius, 0)); svd._z=point(2); coord.head<2>()*=(1./coord(2)); int dx = abs(c - coord[0]); int dy = abs(r - coord[1]); if (dx>_imageRadius) dx = _imageRadius; if (dy>_imageRadius) dy = _imageRadius; PointAccumulator acc = _integralImage.getRegion(c-dx, c+dx, r-dy, r+dy); svd._mean=point.point(); if (acc.n()>_minPoints){ Eigen::Vector3f mean = acc.mean(); svd._mean = mean; svd._n = acc.n(); Eigen::Matrix3f cov = acc.covariance(); eigenSolver.compute(cov); svd._U=eigenSolver.eigenvectors(); svd._singularValues=eigenSolver.eigenvalues(); if (svd._singularValues(0) <0) svd._singularValues(0) = 0; /* cerr << "\t svd.singularValues():" << svd.singularValues() << endl; cerr << "\t svd.U():" << endl << svd.U() << endl; //cerr << "\t svd.curvature():" << svd.curvature() << endl; */ normal = eigenSolver.eigenvectors().col(0).normalized(); if (normal.dot(inverseTransform * mean) > 0.0f) normal =-normal; svd.updateCurvature(); //cerr << "n(" << index << ") c:" << svd.curvature() << endl << point.tail<3>() << endl; if (svd.curvature()>_maxCurvature){ //cerr << "region: " << c-dx << " " << c+dx << " " << r-dx << " " << r+dx << " points: " << acc.n() << endl; normal.setZero(); } } else { normal.setZero(); svd = PointWithNormalSVD(); } if (svd.n() > originalSVD.n()){ originalSVD = svd; point.setNormal(normal); } } } } }
bool TraceStoreCol::PcaLossyCompressChunk(int row_start, int row_end, int col_start, int col_end, int num_rows, int num_cols, int num_frames, int frame_stride, int flow_ix, int flow_frame_stride, short *data, bool replace, float *ssq, char *filters, int row_step, int col_step, int num_pca) { Eigen::MatrixXf Ysub, Y, S, Basis; int loc_num_wells = (col_end - col_start) * (row_end - row_start); int loc_num_cols = col_end - col_start; // Take a sample of the data at rate step, avoiding flagged wells // Count the good rows int sample_wells = 0; for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) { char *filt_start = filters + row_ix * num_cols + col_start; char *filt_end = filt_start + loc_num_cols; while (filt_start < filt_end) { if (*filt_start == 0) { sample_wells++; } filt_start += col_step; } } // try backing off to every well rather than just sampled if we didn't get enough if (sample_wells < MIN_SAMPLE_WELL) { row_step = 1; col_step = 1; int sample_wells = 0; for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) { char *filt_start = filters + row_ix * num_cols + col_start; char *filt_end = filt_start + loc_num_cols; while (filt_start < filt_end) { if (*filt_start == 0) { sample_wells++; } filt_start += col_step; } } } if (sample_wells < MIN_SAMPLE_WELL) { return false; // just give up } // Got enough data to work with, zero out the ssq array for accumulation for (int row_ix = row_start; row_ix < row_end; row_ix++) { float *ssq_start = ssq + row_ix * num_cols + col_start; float *ssq_end = ssq_start + loc_num_cols; while (ssq_start != ssq_end) { *ssq_start++ = 0; } } // Copy the sampled data in Matrix, frame major Ysub.resize(sample_wells, num_frames); for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) { int sample_offset = 0; for (int row_ix = row_start; row_ix < row_end; row_ix+=row_step) { size_t store_offset = row_ix * num_cols + col_start; char *filt_start = filters + store_offset; char *filt_end = filt_start + loc_num_cols; int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset; float *ysub_start = Ysub.data() + sample_wells * frame_ix + sample_offset; while (filt_start < filt_end) { if (*filt_start == 0) { *ysub_start = *trace_start; ysub_start++; sample_offset++; } trace_start += col_step; filt_start += col_step; } } } // Copy in all the data into working matrix Y.resize(loc_num_wells, (int)num_frames); for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) { for (int row_ix = row_start; row_ix < row_end; row_ix++) { size_t store_offset = row_ix * num_cols + col_start; int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset; int16_t *trace_end = trace_start + loc_num_cols; float * y_start = Y.data() + loc_num_wells * frame_ix + (row_ix - row_start) * loc_num_cols; while( trace_start != trace_end ) { *y_start++ = *trace_start++; } } } Eigen::VectorXf col_mean = Y.colwise().sum(); col_mean /= Y.rows(); for (int i = 0; i < Y.cols(); i++) { Y.col(i).array() -= col_mean.coeff(i); } // Create scatter matrix S = Ysub.transpose() * Ysub; // Compute the eigenvectors Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es; es.compute(S); Eigen::MatrixXf Pca_Basis = es.eigenvectors(); Eigen::VectorXf Pca_Values = es.eigenvalues(); // Copy top eigen vectors into basis for projection Basis.resize(num_frames, num_pca); for (int i = 0; i < Basis.cols(); i++) { // Basis.col(i) = es.eigenvectors().col(es.eigenvectors().cols() - i -1); Basis.col(i) = Pca_Basis.col(Pca_Basis.cols() - i - 1); } // Create solver matrix, often not a good way of solving things but eigen vectors should be stable and fast Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose(); // Get coefficients to solve Eigen::MatrixXf B = Y * SX.transpose(); // Uncompress data into yhat matrix Eigen::MatrixXf Yhat = B * Basis.transpose(); for (int i = 0; i < Yhat.cols(); i++) { Yhat.col(i).array() += col_mean.coeff(i); Y.col(i).array() += col_mean.coeff(i); } // H5File h5("pca_lossy.h5"); // h5.Open(); // char buff[256]; // snprintf(buff, sizeof(buff), "/Y_%d_%d_%d", flow_ix, row_start, col_start); // H5Eigen::WriteMatrix(h5, buff, Y); // snprintf(buff, sizeof(buff), "/Yhat_%d_%d_%d", flow_ix, row_start, col_start); // H5Eigen::WriteMatrix(h5, buff, Yhat); // snprintf(buff, sizeof(buff), "/Basis_%d_%d_%d", flow_ix, row_start, col_start); // H5Eigen::WriteMatrix(h5, buff, Basis); // h5.Close(); // Copy data out of yhat matrix into original data structure, keeping track of residuals for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) { for (int row_ix = row_start; row_ix < row_end; row_ix++) { size_t store_offset = row_ix * num_cols + col_start; int16_t *trace_start = data + flow_frame_stride * frame_ix + flow_ix * frame_stride + store_offset; int16_t *trace_end = trace_start + loc_num_cols; float * ssq_start = ssq + store_offset; size_t loc_offset = (row_ix - row_start) * loc_num_cols; float * y_start = Y.data() + loc_num_wells * frame_ix + loc_offset; float * yhat_start = Yhat.data() + loc_num_wells * frame_ix + loc_offset; while( trace_start != trace_end ) { if (replace) { *trace_start = (int16_t)(*yhat_start + .5); } float val = *y_start - *yhat_start; *ssq_start += val * val; y_start++; yhat_start++; trace_start++; ssq_start++; } } } // divide ssq data out for per frame avg for (int row_ix = row_start; row_ix < row_end; row_ix++) { size_t store_offset = row_ix * num_cols + col_start; float * ssq_start = ssq + store_offset; float * ssq_end = ssq_start + loc_num_cols; while (ssq_start != ssq_end) { *ssq_start /= num_frames; ssq_start++; } } return true; }
void AbsoluteOrientation::compute( std::vector<Eigen::Vector3d> &left, std::vector<Eigen::Vector3d> &right, Eigen::Quaterniond &result ) { int i, pairNum = left.size(); Eigen::MatrixXd muLmuR = Eigen::MatrixXd::Zero(3,3), M = Eigen::MatrixXd::Zero(3,3), curMat = Eigen::MatrixXd::Zero(3,3), N = Eigen::MatrixXd::Zero(4,4); Eigen::Vector3d meanFirst(0,0,0), meanSecond(0,0,0); //assume points set to zero by constructor //compute the mean of both point sets for (i=0; i<pairNum; i++) { meanFirst[0] += left[i][0]; meanFirst[1] += left[i][1]; meanFirst[2] += left[i][2]; meanSecond[0] += right[i][0]; meanSecond[1] += right[i][1]; meanSecond[2] += right[i][2]; } meanFirst[0]/=pairNum; meanFirst[1]/=pairNum; meanFirst[2]/=pairNum; meanSecond[0]/=pairNum; meanSecond[1]/=pairNum; meanSecond[2]/=pairNum; //compute the matrix muLmuR muLmuR(0,0) = meanFirst[0]*meanSecond[0]; muLmuR(0,1) = meanFirst[0]*meanSecond[1]; muLmuR(0,2) = meanFirst[0]*meanSecond[2]; muLmuR(1,0) = meanFirst[1]*meanSecond[0]; muLmuR(1,1) = meanFirst[1]*meanSecond[1]; muLmuR(1,2) = meanFirst[1]*meanSecond[2]; muLmuR(2,0) = meanFirst[2]*meanSecond[0]; muLmuR(2,1) = meanFirst[2]*meanSecond[1]; muLmuR(2,2) = meanFirst[2]*meanSecond[2]; //compute the matrix M for (i=0; i<pairNum; i++) { Eigen::Vector3d &leftPoint = left[i]; Eigen::Vector3d &rightPoint = right[i]; curMat(0,0) = leftPoint[0]*rightPoint[0]; curMat(0,1) = leftPoint[0]*rightPoint[1]; curMat(0,2) = leftPoint[0]*rightPoint[2]; curMat(1,0) = leftPoint[1]*rightPoint[0]; curMat(1,1) = leftPoint[1]*rightPoint[1]; curMat(1,2) = leftPoint[1]*rightPoint[2]; curMat(2,0) = leftPoint[2]*rightPoint[0]; curMat(2,1) = leftPoint[2]*rightPoint[1]; curMat(2,2) = leftPoint[2]*rightPoint[2]; M+=curMat; } M+= (muLmuR *(-pairNum)); //compute the matrix N Eigen::MatrixXd tmpMat = Eigen::MatrixXd::Zero(3,3); double A12, A20, A01; double traceM = 0.0; for(i=0; i<3; i++) traceM += M(i,i); tmpMat.diagonal() = Eigen::VectorXd::Constant(3, -traceM); //tmpMat.fill_diagonal(-traceM); tmpMat += (M + M.transpose()); A12 = M(1,2) - M(2,1); A20 = M(2,0) - M(0,2); A01 = M(0,1) - M(1,0); N(0,0)=traceM; N(0,1)=A12; N(0,2)=A20; N(0,3)=A01; N(1,0)=A12; N(2,0)=A20; N(3,0)=A01; N.bottomRightCorner(3,3) = tmpMat; //N.update(tmpMat,1,1); ////find the eigenvector that belongs to the maximal ////eigenvalue of N, eigenvalues are sorted from smallest to largest //vnl_symmetric_eigensystem<double> eigenSystem(N); Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es; es.compute(N); Eigen::MatrixXd V = es.eigenvectors(); //std::stringstream ss;ss << V; //qDebug() << qPrintable(ss.str().c_str()); //setRotationQuaternion(eigenSystem.V(0,3),eigenSystem.V(1,3),eigenSystem.V(2,3),eigenSystem.V(3,3), true); result = Eigen::Quaterniond( V(0,3),V(1,3),V(2,3),V(3,3) ).normalized(); }