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();
}
