inline typename Base<F>::type
SymmetricSchattenNorm
( UpperOrLower uplo, const DistMatrix<F,U,V>& A, typename Base<F>::type p )
{
#ifndef RELEASE
PushCallStack("SymmetricSchattenNorm");
#endif
typedef typename Base<F>::type R;
DistMatrix<F> B( A );
DistMatrix<R,VR,STAR> s( A.Grid() );
MakeSymmetric( uplo, B );
SVD( B, s );
// TODO: Think of how to make this more stable
const int kLocal = s.LocalHeight();
R localSum = 0;
for( int j=0; j<kLocal; ++j )
localSum += Pow( s.GetLocal(j,0), p );
R sum;
mpi::AllReduce( &localSum, &sum, 1, mpi::SUM, A.Grid().VRComm() );
const R norm = Pow( sum, 1/p );
#ifndef RELEASE
PopCallStack();
#endif
return norm;
}
//------------------------------------------------------------------------------
void BasisHarmonicOscillator::createInitalDiscretization()
{
C = zeros<cx_mat>(nGrid, nSpatialOrbitals);
mat Ctmp(nGrid, nSpatialOrbitals);
Wavefunction *wf;
for(int j=0; j<nSpatialOrbitals; j++){
wf = new HarmonicOscillator(cfg, states[2*j]);
for(int i=0; i<nGrid; i++){
Ctmp(i,j) = wf->evaluate(grid->at(i));
}
delete wf;
}
C.set_real(Ctmp);
// Forcing orthogonality by performing a SVD decomposition
SVD(C);
#ifdef DEBUG
cout << "BasisHarmonicOscillator::discretization1d()" << endl;
for(int i=0; i<nSpatialOrbitals; i++){
for(int j=0; j<nSpatialOrbitals; j++){
cout << "|C(" << i << ", " << j << ")| = " << sqrt(cdot(C.col(i),C.col(j))) << endl ;
}
}
#endif
}
void Pseudoinverse( Matrix<F>& A, Base<F> tolerance )
{
DEBUG_CSE
typedef Base<F> Real;
const Int m = A.Height();
const Int n = A.Width();
const Real eps = limits::Epsilon<Real>();
// Get the SVD of A
Matrix<Real> s;
Matrix<F> U, V;
SVDCtrl<Real> ctrl;
ctrl.overwrite = true;
ctrl.bidiagSVDCtrl.approach = COMPACT_SVD;
// TODO(poulson): Let the user change these defaults
ctrl.bidiagSVDCtrl.tolType = RELATIVE_TO_MAX_SING_VAL_TOL;
ctrl.bidiagSVDCtrl.tol =
( tolerance == Real(0) ? Max(m,n)*eps : tolerance );
SVD( A, U, s, V, ctrl );
// Scale U with the inverted (nonzero) singular values, U := U / Sigma
DiagonalSolve( RIGHT, NORMAL, s, U );
// Form pinvA = (U Sigma V^H)^H = V (U Sigma)^H
Gemm( NORMAL, ADJOINT, F(1), V, U, A );
}
void SBVAR::OLSReducedFormEstimate(TDenseMatrix &B, TDenseMatrix &Sigma)
{
// Y = Data()
// X = PredeterminedData()
TDenseMatrix U, V;
TDenseVector d;
// X = U*D*V'
SVD(U,d,V,PredeterminedData(),1);
// Compute the generalize inverse of D.
d.UniqueMemory();
for (int i=d.dim-1; i > 0; i--)
d.vector[i]=(d.vector[i] > d.vector[0]*MACHINE_EPSILON) ? 1.0/d.vector[i] : 0.0;
d.vector[0]=(d.vector[0] > 0.0) ? 1.0/d.vector[0] : 0.0;
// B = inv(X'*X)*X'*Y = V*D^(-2)*V'*V*D*U'*Y' = V*D^(-1)*U'*Y.
TDenseMatrix Z=Transpose(U)*Data();
B=Transpose(V*(DiagonalMatrix(d)*Z));
// Sigma = (Y'*Y - Y'*X*inv(X'*X)*X'*Y)/NumberObservatons()
// = (Y'*Y - Y'*U*U*Y)/NumberObservations()
Sigma=(1.0/(double)NumberObservations())*(YY - Transpose(Z)*Z);
}
void Pseudoinverse( ElementalMatrix<F>& APre, Base<F> tolerance )
{
DEBUG_CSE
typedef Base<F> Real;
DistMatrixReadWriteProxy<F,F,MC,MR> AProx( APre );
auto& A = AProx.Get();
const Int m = A.Height();
const Int n = A.Width();
const Grid& g = A.Grid();
const Real eps = limits::Epsilon<Real>();
// Get the SVD of A
DistMatrix<Real,VR,STAR> s(g);
DistMatrix<F> U(g), V(g);
SVDCtrl<Real> ctrl;
ctrl.overwrite = true;
ctrl.bidiagSVDCtrl.approach = COMPACT_SVD;
// TODO(poulson): Let the user change these defaults
ctrl.bidiagSVDCtrl.tolType = RELATIVE_TO_MAX_SING_VAL_TOL;
ctrl.bidiagSVDCtrl.tol =
( tolerance == Real(0) ? Max(m,n)*eps : tolerance );
SVD( A, U, s, V, ctrl );
// Scale U with the inverted (nonzero) singular values, U := U / Sigma
DiagonalSolve( RIGHT, NORMAL, s, U );
// Form pinvA = (U Sigma V^H)^H = V (U Sigma)^H
Gemm( NORMAL, ADJOINT, F(1), V, U, A );
}
Base<F> TwoNorm( const Matrix<F>& A )
{
DEBUG_CSE
Matrix<Base<F>> s;
SVD( A, s );
return InfinityNorm( s );
}
Base<F> TwoNorm( const ElementalMatrix<F>& A )
{
DEBUG_ONLY(CSE cse("TwoNorm"))
DistMatrix<Base<F>,VR,STAR> s( A.Grid() );
SVD( A, s );
return InfinityNorm( s );
}
Base<F> TwoNorm( const Matrix<F>& A )
{
DEBUG_ONLY(CSE cse("TwoNorm"))
Matrix<Base<F>> s;
SVD( A, s );
return InfinityNorm( s );
}
Base<F> TwoNorm( const AbstractDistMatrix<F>& A )
{
DEBUG_ONLY(CSE cse("TwoNorm"))
DistMatrix<F> B( A );
DistMatrix<Base<F>,VR,STAR> s( A.Grid() );
SVD( B, s );
return InfinityNorm( s );
}
Base<F> SymmetricTwoNorm( UpperOrLower uplo, const Matrix<F>& A )
{
DEBUG_ONLY(CSE cse("SymmetricTwoNorm"))
Matrix<F> B( A );
Matrix<Base<F>> s;
MakeSymmetric( uplo, B );
SVD( B, s );
return MaxNorm( s );
}
Base<F> SymmetricTwoNorm( UpperOrLower uplo, const AbstractDistMatrix<F>& A )
{
DEBUG_ONLY(CSE cse("SymmetricTwoNorm"))
DistMatrix<F> B( A );
DistMatrix<Base<F>,VR,STAR> s( A.Grid() );
MakeSymmetric( uplo, B );
SVD( B, s );
return MaxNorm( s );
}
bool q_rigidaffine_compute_affinematrix_3D(const vector<Point3D64f> &vec_A,const vector<Point3D64f> &vec_B,Matrix &x4x4_affinematrix)
{
if(vec_A.size()<4 || vec_A.size()!=vec_B.size())
{
fprintf(stderr,"ERROR: Invalid input parameters! \n");
return false;
}
if(x4x4_affinematrix.nrows()!=4 || x4x4_affinematrix.ncols()!=4)
{
x4x4_affinematrix.ReSize(4,4);
}
vector<Point3D64f> vec_A_norm,vec_B_norm;
Matrix x4x4_normalize_A(4,4),x4x4_normalize_B(4,4);
vec_A_norm=vec_A; vec_B_norm=vec_B;
q_normalize_points_3D(vec_A,vec_A_norm,x4x4_normalize_A);
q_normalize_points_3D(vec_B,vec_B_norm,x4x4_normalize_B);
int n_point=vec_A.size();
Matrix A(3*n_point,13);
int row=1;
for(int i=0;i<n_point;i++)
{
A(row,1)=vec_A_norm[i].x; A(row,2)=vec_A_norm[i].y; A(row,3)=vec_A_norm[i].z; A(row,4)=1.0;
A(row,5)=0.0; A(row,6)=0.0; A(row,7)=0.0; A(row,8)=0.0;
A(row,9)=0.0; A(row,10)=0.0; A(row,11)=0.0; A(row,12)=0.0;
A(row,13)=-vec_B_norm[i].x;
A(row+1,1)=0.0; A(row+1,2)=0.0; A(row+1,3)=0.0; A(row+1,4)=0.0;
A(row+1,5)=vec_A_norm[i].x; A(row+1,6)=vec_A_norm[i].y; A(row+1,7)=vec_A_norm[i].z; A(row+1,8)=1.0;
A(row+1,9)=0.0; A(row+1,10)=0.0; A(row+1,11)=0.0; A(row+1,12)=0.0;
A(row+1,13)=-vec_B_norm[i].y;
A(row+2,1)=0.0; A(row+2,2)=0.0; A(row+2,3)=0.0; A(row+2,4)=0.0;
A(row+2,5)=0.0; A(row+2,6)=0.0; A(row+2,7)=0.0; A(row+2,8)=0.0;
A(row+2,9)=vec_A_norm[i].x; A(row+2,10)=vec_A_norm[i].y;A(row+2,11)=vec_A_norm[i].z;A(row+2,12)=1.0;
A(row+2,13)=-vec_B_norm[i].z;
row+=3;
}
DiagonalMatrix D(13);
Matrix U(3*n_point,13),V(13,13);
SVD(A,D,U,V);
Matrix h=V.column(13);
for(int i=1;i<=13;i++)
h(i,1) /= h(13,1);
x4x4_affinematrix(1,1)=h(1,1); x4x4_affinematrix(1,2)=h(2,1); x4x4_affinematrix(1,3)=h(3,1); x4x4_affinematrix(1,4)=h(4,1);
x4x4_affinematrix(2,1)=h(5,1); x4x4_affinematrix(2,2)=h(6,1); x4x4_affinematrix(2,3)=h(7,1); x4x4_affinematrix(2,4)=h(8,1);
x4x4_affinematrix(3,1)=h(9,1); x4x4_affinematrix(3,2)=h(10,1); x4x4_affinematrix(3,3)=h(11,1); x4x4_affinematrix(3,4)=h(12,1);
x4x4_affinematrix(4,1)=0.0; x4x4_affinematrix(4,2)=0.0; x4x4_affinematrix(4,3)=0.0; x4x4_affinematrix(4,4)=1.0;
x4x4_affinematrix=x4x4_normalize_B.i()*x4x4_affinematrix*x4x4_normalize_A;
return true;
}
TwoNorm( const DistMatrix<F,U,V>& A )
{
#ifndef RELEASE
CallStackEntry entry("TwoNorm");
#endif
typedef BASE(F) R;
DistMatrix<F> B( A );
DistMatrix<R,VR,STAR> s( A.Grid() );
SVD( B, s );
return InfinityNorm( s );
}
TwoNorm( const Matrix<F>& A )
{
#ifndef RELEASE
CallStackEntry entry("TwoNorm");
#endif
typedef BASE(F) R;
Matrix<F> B( A );
Matrix<R> s;
SVD( B, s );
return InfinityNorm( s );
}
Base<F> SymmetricTwoNorm( UpperOrLower uplo, const ElementalMatrix<F>& A )
{
DEBUG_ONLY(CSE cse("SymmetricTwoNorm"))
DistMatrix<F> B( A );
DistMatrix<Base<F>,VR,STAR> s( A.Grid() );
MakeSymmetric( uplo, B );
SVDCtrl<Base<F>> ctrl;
ctrl.overwrite = true;
SVD( B, s, ctrl );
return MaxNorm( s );
}
Base<F> SymmetricTwoNorm( UpperOrLower uplo, const Matrix<F>& A )
{
DEBUG_ONLY(CSE cse("SymmetricTwoNorm"))
Matrix<F> B( A );
Matrix<Base<F>> s;
MakeSymmetric( uplo, B );
SVDCtrl<Base<F>> ctrl;
ctrl.overwrite = true;
SVD( B, s, ctrl );
return MaxNorm( s );
}
SymmetricTwoNorm( UpperOrLower uplo, const DistMatrix<F,U,V>& A )
{
#ifndef RELEASE
CallStackEntry entry("SymmetricTwoNorm");
#endif
typedef BASE(F) R;
DistMatrix<F,U,V> B( A );
DistMatrix<R,VR,STAR> s( A.Grid() );
MakeSymmetric( uplo, B );
SVD( B, s );
return MaxNorm( s );
}
// compute optimal pose and scale using a procedure described by Umeyame,
// Least-squares estimation of transformation parameters between two point patterns (IEEE Pami 1991)
double computeScalingFactor(MeshType* mesh1, MeshType* mesh2)
{
if (mesh1->GetNumberOfPoints() != mesh2->GetNumberOfPoints()) {
throw std::runtime_error("meshes should be in correspondence when computing specificity");
}
PointListType points1;
PointListType points2;
for (unsigned i = 0; i < mesh1->GetNumberOfPoints(); i++) {
MeshType::PointType pt1;
mesh1->GetPoint(i, &pt1);
points1.push_back(pt1);
MeshType::PointType pt2;
mesh2->GetPoint(i, &pt2);
points2.push_back(pt2);
}
vnlMatrixType X = createMatrixFromPoints(points1);
vnlMatrixType Y = createMatrixFromPoints(points2);
vnlVectorType mu_x = calcMean(X);
vnlVectorType mu_y = calcMean(Y);
ElementType sigma2_x = calcVar(X, mu_x);
ElementType sigma2_y = calcVar(Y, mu_y);
vnlMatrixType Sigma_xy = calcCov(X, Y, mu_x, mu_y);
vnl_svd<ElementType> SVD(Sigma_xy);
unsigned m = X.rows();
vnlMatrixType S(m,m);
S.set_identity();
ElementType detU = vnl_qr<ElementType>(SVD.U()).determinant();
ElementType detV = vnl_qr<ElementType>(SVD.V()).determinant();
if ( detU * detV == -1) {
S[m-1][m-1] = -1;
}
// the procedure actually computes the optimal rotation, translation and scale. We only
// use the scaling factor
vnlMatrixType R = SVD.U() * S * SVD.V().transpose();
ElementType c = 1/sigma2_x * vnl_trace(SVD.W()*S);
vnlVectorType t = mu_y - c * R * mu_x;
ElementType epsilon2 = sigma2_y - pow(vnl_trace(SVD.W()*S), 2) / sigma2_x;
return c;
}
SymmetricTwoNorm( UpperOrLower uplo, const Matrix<F>& A )
{
#ifndef RELEASE
CallStackEntry entry("SymmetricTwoNorm");
#endif
typedef BASE(F) R;
Matrix<F> B( A );
Matrix<R> s;
MakeSymmetric( uplo, B );
SVD( B, s );
return MaxNorm( s );
}
inline void HermitianSVD
( UpperOrLower uplo, DistMatrix<F>& A,
DistMatrix<BASE(F),VR,STAR>& s, DistMatrix<F>& U, DistMatrix<F>& V )
{
#ifndef RELEASE
CallStackEntry entry("HermitianSVD");
#endif
#ifdef HAVE_PMRRR
typedef BASE(F) R;
// Grab an eigenvalue decomposition of A
HermitianEig( uplo, A, s, V );
// Redistribute the singular values into an [MR,* ] distribution
const Grid& grid = A.Grid();
DistMatrix<R,MR,STAR> s_MR_STAR( grid );
s_MR_STAR.AlignWith( V.DistData() );
s_MR_STAR = s;
// Set the singular values to the absolute value of the eigenvalues
const Int numLocalVals = s.LocalHeight();
for( Int iLoc=0; iLoc<numLocalVals; ++iLoc )
{
const R sigma = s.GetLocal(iLoc,0);
s.SetLocal(iLoc,0,Abs(sigma));
}
// Copy V into U (flipping the sign as necessary)
U.AlignWith( V );
U.ResizeTo( V.Height(), V.Width() );
const Int localHeight = V.LocalHeight();
const Int localWidth = V.LocalWidth();
for( Int jLoc=0; jLoc<localWidth; ++jLoc )
{
const R sigma = s_MR_STAR.GetLocal( jLoc, 0 );
F* UCol = U.Buffer( 0, jLoc );
const F* VCol = V.LockedBuffer( 0, jLoc );
if( sigma >= 0 )
for( Int iLoc=0; iLoc<localHeight; ++iLoc )
UCol[iLoc] = VCol[iLoc];
else
for( Int iLoc=0; iLoc<localHeight; ++iLoc )
UCol[iLoc] = -VCol[iLoc];
}
#else
U = A;
MakeHermitian( uplo, U );
SVD( U, s, V );
#endif // ifdef HAVE_PMRRR
}
void getGeneralizedInverse(Matrix& G, Matrix& Gi) {
#ifdef DEBUG
cout << "\n\ngetGeneralizedInverse - Singular Value\n";
#endif
// Singular value decomposition method
// do SVD
Matrix U, V;
DiagonalMatrix D;
SVD(G,D,U,V); // X = U * D * V.t()
#ifdef DEBUG
cout << "D:\n";
cout << setw(9) << setprecision(6) << (D);
cout << "\n\n";
#endif
DiagonalMatrix Di;
Di << D.i();
#ifdef DEBUG
cout << "Di:\n";
cout << setw(9) << setprecision(6) << (Di);
cout << "\n\n";
#endif
int i=Di.Nrows();
for (; i>=1; i--) {
if (Di(i) > 1000.0) {
Di(i) = 0.0;
}
}
#ifdef DEBUG
cout << "Di with biggies zeroed out:\n";
cout << setw(9) << setprecision(6) << (Di);
cout << "\n\n";
#endif
//Matrix Gi;
Gi << (U * (Di * V.t()));
return;
}
ConditionNumber( const DistMatrix<F,U,V>& A )
{
#ifndef RELEASE
CallStackEntry entry("ConditionNumber");
#endif
typedef BASE(F) R;
DistMatrix<F> B( A );
DistMatrix<R,VR,STAR> s( A.Grid() );
SVD( B, s );
R cond = 1;
const int numVals = s.Height();
if( numVals > 0 )
cond = s.Get(0,0) / s.Get(numVals-1,0);
return cond;
}
inline void
Pseudoinverse( DistMatrix<F>& A )
{
#ifndef RELEASE
PushCallStack("Pseudoinverse");
#endif
typedef typename Base<F>::type R;
const Grid& g = A.Grid();
const int m = A.Height();
const int n = A.Width();
const int k = std::max(m,n);
// Get the SVD of A
DistMatrix<R,VR,STAR> s(g);
DistMatrix<F> U(g), V(g);
U = A;
SVD( U, s, V );
// Compute the two-norm of A as the maximum singular value
const R twoNorm = Norm( s, INFINITY_NORM );
// Set the tolerance equal to k ||A||_2 eps and invert above tolerance
const R eps = lapack::MachineEpsilon<R>();
const R tolerance = k*twoNorm*eps;
const int numLocalVals = s.LocalHeight();
for( int iLocal=0; iLocal<numLocalVals; ++iLocal )
{
const R sigma = s.GetLocal(iLocal,0);
if( sigma < tolerance )
s.SetLocal(iLocal,0,0);
else
s.SetLocal(iLocal,0,1/sigma);
}
// Scale U with the singular values, U := U Sigma
DiagonalScale( RIGHT, NORMAL, s, U );
// Form pinvA = (U Sigma V^H)^H = V (U Sigma)^H
Zeros( n, m, A );
Gemm( NORMAL, ADJOINT, F(1), V, U, F(0), A );
#ifndef RELEASE
PopCallStack();
#endif
}
inline void HermitianSVD
( UpperOrLower uplo, Matrix<F>& A, Matrix<BASE(F)>& s )
{
#ifndef RELEASE
CallStackEntry entry("HermitianSVD");
#endif
#if 1
// Grab the eigenvalues of A
HermitianEig( uplo, A, s );
// Set the singular values to the absolute value of the eigenvalues
for( Int i=0; i<s.Height(); ++i )
s.Set(i,0,Abs(s.Get(i,0)));
#else
MakeHermitian( uplo, A );
SVD( A, s );
#endif
}
void XEMSymmetricMatrix::computeSVD(XEMDiagMatrix* & S, XEMGeneralMatrix* & O){
int64_t dim = O->getPbDimension();
DiagonalMatrix * tabShape_k = new DiagonalMatrix(dim);
Matrix * tabOrientation_k = new Matrix(dim,dim);
SVD((*_value),(*tabShape_k),(*tabOrientation_k));
double * storeS = S->getStore();
double * storeO = O->getStore();
double * storeTabShape_k = (*tabShape_k).Store();
double * storeTabOrientation_k = (*tabOrientation_k).Store();
recopyTab(storeTabShape_k, storeS, dim);
recopyTab(storeTabOrientation_k, storeO, dim*dim);
delete tabShape_k;
delete tabOrientation_k;
}
Eigen::Matrix4f CUDACameraTrackingMultiResRGBD::computeBestRigidAlignment(CameraTrackingInput cameraTrackingInput, Eigen::Matrix3f& intrinsics, Eigen::Matrix4f& globalDeltaTransform, unsigned int level, CameraTrackingParameters cameraTrackingParameters, unsigned int maxInnerIter, float condThres, float angleThres, LinearSystemConfidence& conf)
{
Eigen::Matrix4f deltaTransform = globalDeltaTransform;
conf.reset();
Matrix6x7f system;
Eigen::Matrix3f ROld = deltaTransform.block(0, 0, 3, 3);
Eigen::Vector3f eulerAngles = ROld.eulerAngles(2, 1, 0);
float3 anglesOld; anglesOld.x = eulerAngles.x(); anglesOld.y = eulerAngles.y(); anglesOld.z = eulerAngles.z();
float3 translationOld; translationOld.x = deltaTransform(0, 3); translationOld.y = deltaTransform(1, 3); translationOld.z = deltaTransform(2, 3);
m_CUDABuildLinearSystem->applyBL(cameraTrackingInput, intrinsics, cameraTrackingParameters, anglesOld, translationOld, m_imageWidth[level], m_imageHeight[level], level, system, conf);
Matrix6x6f ATA = system.block(0, 0, 6, 6);
Vector6f ATb = system.block(0, 6, 6, 1);
if (ATA.isZero()) {
return m_matrixTrackingLost;
}
Eigen::JacobiSVD<Matrix6x6f> SVD(ATA, Eigen::ComputeFullU | Eigen::ComputeFullV);
Vector6f x = SVD.solve(ATb);
//computing the matrix condition
Vector6f evs = SVD.singularValues();
conf.matrixCondition = evs[0]/evs[5];
Vector6f xNew; xNew.block(0, 0, 3, 1) = eulerAngles; xNew.block(3, 0, 3, 1) = deltaTransform.block(0, 3, 3, 1);
xNew += x;
deltaTransform = delinearizeTransformation(xNew, Eigen::Vector3f(0.0f, 0.0f, 0.0f), 1.0f, level);
if(deltaTransform(0, 0) == -std::numeric_limits<float>::infinity())
{
conf.trackingLostTresh = true;
return m_matrixTrackingLost;
}
return deltaTransform;
}
size_t CMatrixFactorization<double>::Rank(const CDenseArray<double>& A, double tol) {
CDenseArray<double> U(A.NRows(),A.NRows());
CDenseArray<double> S(min(A.NRows(),A.NCols()),1);
CDenseArray<double> Vt(A.NCols(),A.NCols());
SVD(A,U,S,Vt);
size_t rank = 0;
for(size_t i=0; i<S.NRows(); i++) {
if(S(i,0)>tol)
rank++;
}
return rank;
}
void ImageViewer_ex2::adjustimageAffineSimilarity()
{
Vector3f l(3,1);
Vector3f m(3,1);
Vector3f r1(3,1);
Vector3f r2(3,1);
MatrixXf A(2,3);
l = pinmanager->getLine(0);
m = pinmanager->getLine(1);
r2 << l(0) * m(0), l(0) * m(1) + l(1) * m(0), l(1) * m(1);
l = pinmanager->getLine(2);
m = pinmanager->getLine(3);
r1 << l(0) * m(0), l(0) * m(1) + l(1) * m(0), l(1) * m(1);
A << r1.transpose(), r2.transpose();
JacobiSVD<MatrixXf> SVD(A, ComputeFullV);
VectorXf S = SVD.matrixV().col(SVD.matrixV().cols() - 1);
//S /= S(2);
S(2) = 1;
MatrixXf kkt(2,2);
kkt << S(0), S(1), S(1), S(2);
LLT<MatrixXf> lltOfA(kkt);
MatrixXf L = lltOfA.matrixU();
H << L(0), L(1), 0, L(2), L(3), 0, 0, 0, 1;
//std::cout << H << std::endl;
H = H.inverse();
QSize imgSize(this->width(), this->height());
QVector<QPoint> areaRender;
areaRender << QPoint(0,0) << QPoint(0, imgSize.height()) << QPoint(imgSize.width(), imgSize.height()) << QPoint(imgSize.width(), 0);
showResult(imgSize, areaRender);
}
bool q_rigidaffine_compute_rigidmatrix_3D(const vector<Point3D64f> &vec_A,const vector<Point3D64f> &vec_B,Matrix &x4x4_rigidmatrix)
{
if(vec_A.size()<4 || vec_A.size()!=vec_B.size())
{
fprintf(stderr,"ERROR: Invalid input parameters! \n");
return false;
}
if(x4x4_rigidmatrix.nrows()!=4 || x4x4_rigidmatrix.ncols()!=4)
{
x4x4_rigidmatrix.ReSize(4,4);
}
int n_point=vec_A.size();
vector<Point3D64f> vec_A_norm,vec_B_norm;
Matrix x4x4_normalize_A(4,4),x4x4_normalize_B(4,4);
vec_A_norm=vec_A; vec_B_norm=vec_B;
q_normalize_points_3D(vec_A,vec_A_norm,x4x4_normalize_A);
q_normalize_points_3D(vec_B,vec_B_norm,x4x4_normalize_B);
Matrix x3xn_A(3,n_point),x3xn_B(3,n_point);
for(long i=0;i<n_point;i++)
{
x3xn_A(1,i+1)=vec_A_norm[i].x; x3xn_A(2,i+1)=vec_A_norm[i].y; x3xn_A(3,i+1)=vec_A_norm[i].z;
x3xn_B(1,i+1)=vec_B_norm[i].x; x3xn_B(2,i+1)=vec_B_norm[i].y; x3xn_B(3,i+1)=vec_B_norm[i].z;
}
DiagonalMatrix D;
Matrix U,V;
SVD(x3xn_A*x3xn_B.t(),D,U,V);
Matrix R=V*U.t();
x4x4_rigidmatrix(1,1)=R(1,1); x4x4_rigidmatrix(1,2)=R(1,2); x4x4_rigidmatrix(1,3)=R(1,3); x4x4_rigidmatrix(1,4)=0.0;
x4x4_rigidmatrix(2,1)=R(2,1); x4x4_rigidmatrix(2,2)=R(2,2); x4x4_rigidmatrix(2,3)=R(2,3); x4x4_rigidmatrix(2,4)=0.0;
x4x4_rigidmatrix(3,1)=R(3,1); x4x4_rigidmatrix(3,2)=R(3,2); x4x4_rigidmatrix(3,3)=R(3,3); x4x4_rigidmatrix(3,4)=0.0;
x4x4_rigidmatrix(4,1)=0.0; x4x4_rigidmatrix(4,2)=0.0; x4x4_rigidmatrix(4,3)=0.0; x4x4_rigidmatrix(4,4)=1.0;
x4x4_rigidmatrix=x4x4_normalize_B.i()*x4x4_rigidmatrix*x4x4_normalize_A;
return true;
}
inline typename Base<F>::type
SchattenNorm( const Matrix<F>& A, typename Base<F>::type p )
{
#ifndef RELEASE
PushCallStack("SchattenNorm");
#endif
typedef typename Base<F>::type R;
Matrix<F> B( A );
Matrix<R> s;
SVD( B, s );
// TODO: Think of how to make this more stable
const int k = s.Height();
R sum = 0;
for( int j=k-1; j>=0; --j )
sum += Pow( s.Get(j,0), p );
const R norm = Pow( sum, 1/p );
#ifndef RELEASE
PopCallStack();
#endif
return norm;
}