Basis_HGRAD_TRI_Cn_FEM<Scalar,ArrayScalar>::Basis_HGRAD_TRI_Cn_FEM( const int n ,
const EPointType pointType ):
Phis( n ),
V((n+1)*(n+2)/2,(n+1)*(n+2)/2),
Vinv((n+1)*(n+2)/2,(n+1)*(n+2)/2),
latticePts( (n+1)*(n+2)/2 , 2 )
{
TEUCHOS_TEST_FOR_EXCEPTION( n <= 0, std::invalid_argument, "polynomial order must be >= 1");
const int N = (n+1)*(n+2)/2;
this -> basisCardinality_ = N;
this -> basisDegree_ = n;
this -> basisCellTopology_ = shards::CellTopology(shards::getCellTopologyData<shards::Triangle<3> >() );
this -> basisType_ = BASIS_FEM_FIAT;
this -> basisCoordinates_ = COORDINATES_CARTESIAN;
this -> basisTagsAreSet_ = false;
// construct lattice
shards::CellTopology myTri_3( shards::getCellTopologyData< shards::Triangle<3> >() );
PointTools::getLattice<Scalar,FieldContainer<Scalar> >( latticePts ,
myTri_3 ,
n ,
0 ,
pointType );
// form Vandermonde matrix. Actually, this is the transpose of the VDM,
// so we transpose on copy below.
Phis.getValues( V , latticePts , OPERATOR_VALUE );
// now I need to copy V into a Teuchos array to do the inversion
Teuchos::SerialDenseMatrix<int,Scalar> Vsdm(N,N);
for (int i=0;i<N;i++) {
for (int j=0;j<N;j++) {
Vsdm(i,j) = V(i,j);
}
}
// invert the matrix
Teuchos::SerialDenseSolver<int,Scalar> solver;
solver.setMatrix( rcp( &Vsdm , false ) );
solver.invert( );
// now I need to copy the inverse into Vinv
for (int i=0;i<N;i++) {
for (int j=0;j<N;j++) {
Vinv(i,j) = Vsdm(j,i);
}
}
}
Basis_HGRAD_TET_Cn_FEM<Scalar,ArrayScalar>::Basis_HGRAD_TET_Cn_FEM( const int n ,
const EPointType pointType ):
Phis( n ),
V((n+1)*(n+2)*(n+3)/6,(n+1)*(n+2)*(n+3)/6),
Vinv((n+1)*(n+2)*(n+3)/6,(n+1)*(n+2)*(n+3)/6),
latticePts( (n+1)*(n+2)*(n+3)/6 , 3 )
{
const int N = (n+1)*(n+2)*(n+3)/6;
this -> basisCardinality_ = N;
this -> basisDegree_ = n;
this -> basisCellTopology_ = shards::CellTopology(shards::getCellTopologyData<shards::Tetrahedron<4> >() );
this -> basisType_ = BASIS_FEM_FIAT;
this -> basisCoordinates_ = COORDINATES_CARTESIAN;
this -> basisTagsAreSet_ = false;
// construct lattice
PointTools::getLattice<Scalar,ArrayScalar >( latticePts ,
this->getBaseCellTopology() ,
n ,
0 ,
pointType );
// form Vandermonde matrix. Actually, this is the transpose of the VDM,
// so we transpose on copy below.
Phis.getValues( V , latticePts , OPERATOR_VALUE );
// now I need to copy V into a Teuchos array to do the inversion
Teuchos::SerialDenseMatrix<int,Scalar> Vsdm(N,N);
for (int i=0;i<N;i++) {
for (int j=0;j<N;j++) {
Vsdm(i,j) = V(i,j);
}
}
// invert the matrix
Teuchos::SerialDenseSolver<int,Scalar> solver;
solver.setMatrix( rcp( &Vsdm , false ) );
solver.invert( );
// now I need to copy the inverse into Vinv
for (int i=0;i<N;i++) {
for (int j=0;j<N;j++) {
Vinv(i,j) = Vsdm(j,i);
}
}
initializeTags();
this->basisTagsAreSet_ = true;
}
void omxCallGREMLFitFunction(omxFitFunction *oo, int want, FitContext *fc){
if (want & (FF_COMPUTE_PREOPTIMIZE)) return;
//Recompute Expectation:
omxExpectation* expectation = oo->expectation;
omxExpectationCompute(expectation, NULL);
omxGREMLFitState *gff = (omxGREMLFitState*)oo->argStruct; //<--Cast generic omxFitFunction to omxGREMLFitState
//Ensure that the pointer in the GREML fitfunction is directed at the right FreeVarGroup
//(not necessary for most compute plans):
if(fc && gff->varGroup != fc->varGroup){
gff->buildParamMap(fc->varGroup);
}
//Declare local variables used in more than one scope in this function:
const double Scale = fabs(Global->llScale); //<--absolute value of loglikelihood scale
const double NATLOG_2PI = 1.837877066409345483560659472811; //<--log(2*pi)
int i;
Eigen::Map< Eigen::MatrixXd > Eigy(omxMatrixDataColumnMajor(gff->y), gff->y->cols, 1);
Eigen::Map< Eigen::MatrixXd > Vinv(omxMatrixDataColumnMajor(gff->invcov), gff->invcov->rows, gff->invcov->cols);
EigenMatrixAdaptor EigX(gff->X);
Eigen::MatrixXd P, Py;
P.setZero(gff->invcov->rows, gff->invcov->cols);
double logdetV=0, logdetquadX=0;
if(want & (FF_COMPUTE_FIT | FF_COMPUTE_GRADIENT | FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
if(gff->usingGREMLExpectation){
omxGREMLExpectation* oge = (omxGREMLExpectation*)(expectation->argStruct);
//Check that factorizations of V and the quadratic form in X succeeded:
if(oge->cholV_fail_om->data[0]){
oo->matrix->data[0] = NA_REAL;
if (fc) fc->recordIterationError("expected covariance matrix is non-positive-definite");
return;
}
if(oge->cholquadX_fail){
oo->matrix->data[0] = NA_REAL;
if (fc) fc->recordIterationError("Cholesky factorization failed; possibly, the matrix of covariates is rank-deficient");
return;
}
//Log determinant of V:
logdetV = oge->logdetV_om->data[0];
//Log determinant of quadX:
for(i=0; i < gff->X->cols; i++){
logdetquadX += log(oge->cholquadX_vectorD[i]);
}
logdetquadX *= 2;
gff->REMLcorrection = Scale*0.5*logdetquadX;
//Finish computing fit (negative loglikelihood):
P.triangularView<Eigen::Lower>() = Vinv.selfadjointView<Eigen::Lower>() *
(Eigen::MatrixXd::Identity(Vinv.rows(), Vinv.cols()) -
(EigX * oge->quadXinv.selfadjointView<Eigen::Lower>() * oge->XtVinv)); //Vinv * (I-Hatmat)
Py = P.selfadjointView<Eigen::Lower>() * Eigy;
oo->matrix->data[0] = gff->REMLcorrection + Scale*0.5*( (((double)gff->y->cols) * NATLOG_2PI) + logdetV + ( Eigy.transpose() * Py )(0,0));
gff->nll = oo->matrix->data[0];
}
else{ //If not using GREML expectation, deal with means and cov in a general way to compute fit...
//Declare locals:
EigenMatrixAdaptor yhat(gff->means);
EigenMatrixAdaptor EigV(gff->cov);
double logdetV=0, logdetquadX=0;
Eigen::MatrixXd Vinv, quadX;
Eigen::LLT< Eigen::MatrixXd > cholV(gff->cov->rows);
Eigen::LLT< Eigen::MatrixXd > cholquadX(gff->X->cols);
Eigen::VectorXd cholV_vectorD, cholquadX_vectorD;
//Cholesky factorization of V:
cholV.compute(EigV);
if(cholV.info() != Eigen::Success){
omxRaiseErrorf("expected covariance matrix is non-positive-definite");
oo->matrix->data[0] = NA_REAL;
return;
}
//Log determinant of V:
cholV_vectorD = (( Eigen::MatrixXd )(cholV.matrixL())).diagonal();
for(i=0; i < gff->X->rows; i++){
logdetV += log(cholV_vectorD[i]);
}
logdetV *= 2;
Vinv = cholV.solve(Eigen::MatrixXd::Identity( EigV.rows(), EigV.cols() )); //<-- V inverse
quadX = EigX.transpose() * Vinv * EigX; //<--Quadratic form in X
cholquadX.compute(quadX); //<--Cholesky factorization of quadX
if(cholquadX.info() != Eigen::Success){
omxRaiseErrorf("Cholesky factorization failed; possibly, the matrix of covariates is rank-deficient");
oo->matrix->data[0] = NA_REAL;
return;
}
cholquadX_vectorD = (( Eigen::MatrixXd )(cholquadX.matrixL())).diagonal();
for(i=0; i < gff->X->cols; i++){
logdetquadX += log(cholquadX_vectorD[i]);
}
logdetquadX *= 2;
gff->REMLcorrection = Scale*0.5*logdetquadX;
//Finish computing fit:
oo->matrix->data[0] = gff->REMLcorrection + Scale*0.5*( ((double)gff->y->rows * NATLOG_2PI) + logdetV +
( Eigy.transpose() * Vinv * (Eigy - yhat) )(0,0));
gff->nll = oo->matrix->data[0];
return;
}
}
if(want & (FF_COMPUTE_GRADIENT | FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
//This part requires GREML expectation:
omxGREMLExpectation* oge = (omxGREMLExpectation*)(expectation->argStruct);
//Declare local variables for this scope:
int numChildren = fc->childList.size();
int __attribute__((unused)) parallelism = (numChildren == 0) ? 1 : numChildren;
fc->grad.resize(gff->dVlength); //<--Resize gradient in FitContext
//Set up new HessianBlock:
HessianBlock *hb = new HessianBlock;
if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
hb->vars.resize(gff->dVlength);
hb->mat.resize(gff->dVlength, gff->dVlength);
}
//Begin looping thru free parameters:
#pragma omp parallel for num_threads(parallelism)
for(i=0; i < gff->dVlength; i++){
//Declare locals within parallelized region:
int j=0, t1=0, t2=0;
Eigen::MatrixXd PdV_dtheta1;
Eigen::MatrixXd dV_dtheta1(Eigy.rows(), Eigy.rows()); //<--Derivative of V w/r/t parameter i.
Eigen::MatrixXd dV_dtheta2(Eigy.rows(), Eigy.rows()); //<--Derivative of V w/r/t parameter j.
t1 = gff->gradMap[i]; //<--Parameter number for parameter i.
if(t1 < 0){continue;}
if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){hb->vars[i] = t1;}
if( oge->numcases2drop ){
dropCasesAndEigenize(gff->dV[i], dV_dtheta1, oge->numcases2drop, oge->dropcase, 1);
}
else{dV_dtheta1 = Eigen::Map< Eigen::MatrixXd >(omxMatrixDataColumnMajor(gff->dV[i]), gff->dV[i]->rows, gff->dV[i]->cols);}
//PdV_dtheta1 = P.selfadjointView<Eigen::Lower>() * dV_dtheta1.selfadjointView<Eigen::Lower>();
PdV_dtheta1 = P.selfadjointView<Eigen::Lower>();
PdV_dtheta1 = PdV_dtheta1 * dV_dtheta1.selfadjointView<Eigen::Lower>();
for(j=i; j < gff->dVlength; j++){
if(j==i){
gff->gradient(t1) = Scale*0.5*(PdV_dtheta1.trace() - (Eigy.transpose() * PdV_dtheta1 * Py)(0,0));
fc->grad(t1) += gff->gradient(t1);
if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
gff->avgInfo(t1,t1) = Scale*0.5*(Eigy.transpose() * PdV_dtheta1 * PdV_dtheta1 * Py)(0,0);
}
}
else{if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
t2 = gff->gradMap[j]; //<--Parameter number for parameter j.
if(t2 < 0){continue;}
if( oge->numcases2drop ){
dropCasesAndEigenize(gff->dV[j], dV_dtheta2, oge->numcases2drop, oge->dropcase, 1);
}
else{dV_dtheta2 = Eigen::Map< Eigen::MatrixXd >(omxMatrixDataColumnMajor(gff->dV[j]), gff->dV[j]->rows, gff->dV[j]->cols);}
gff->avgInfo(t1,t2) = Scale*0.5*(Eigy.transpose() * PdV_dtheta1 * P.selfadjointView<Eigen::Lower>() * dV_dtheta2.selfadjointView<Eigen::Lower>() * Py)(0,0);
gff->avgInfo(t2,t1) = gff->avgInfo(t1,t2);
}}}}
//Assign upper triangle elements of avgInfo to the HessianBlock:
if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
for (size_t d1=0, h1=0; h1 < gff->dV.size(); ++h1) {
for (size_t d2=0, h2=0; h2 <= h1; ++h2) {
hb->mat(d2,d1) = gff->avgInfo(h2,h1);
++d2;
}
++d1;
}
fc->queue(hb);
}}
// Polar decomponsition of F through left stretch matrix
// F = VR = Q Lambda (QTR)
// The target matrix is assumed to be F
// Function returns V and optionally R = V^-1 F (if pointer is not NULL)
// and optionally (lam1,lam2,lam3) in stretches (if not NULL)
// It does not get Q, but if needed, they are eigenvectors of the
// returned V matrix
Matrix3 Matrix3::LeftDecompose(Matrix3 *R,Vector *stretches) const
{
if(is2D)
{ // 2D has simple formulae for R = ((Fsum,Fdif),(-Fdif,Fsum))
double Fsum = m[0][0]+m[1][1];
double Fdif = m[0][1]-m[1][0];
double denom = sqrt(Fsum*Fsum+Fdif*Fdif);
Fsum /= denom;
Fdif /= denom;
// V is F* R*T
Matrix3 V(m[0][0]*Fsum+m[0][1]*Fdif,-m[0][0]*Fdif+m[0][1]*Fsum,
m[1][0]*Fsum+m[1][1]*Fdif,-m[1][0]*Fdif+m[1][1]*Fsum,m[2][2]);
// if R pointer not NULL, return it too
if(R!=NULL)
{ R->set(Fsum,Fdif,-Fdif,Fsum,1.);
}
// Return Eigenvalues of V if asked
if(stretches!=NULL)
{ *stretches = V.Eigenvalues();
}
return V;
}
// rest is for 3D matrix
// Get B=FF^T and B^2
Matrix3 B = (*this)*Transpose();
Matrix3 B2 = B*B;
// Eigenvalues of B are lamda^2
Vector Eigenvals = B.Eigenvalues();
double lam1 = sqrt(Eigenvals.x);
double lam2 = sqrt(Eigenvals.y);
double lam3 = sqrt(Eigenvals.z);
// invariants of V
double i1 = lam1+lam2+lam3;
double i2 = lam1*lam2+lam1*lam3+lam2*lam3;
double i3 = lam1*lam2*lam3;
// set coefficients
double d1 = 1./(i1*i2-i3);
double c2 = -d1; // coefficient of B2
double c1 = (i1*i1-i2)*d1; // coefficient of B
double cI = i1*i3*d1; // coefficient of I
// Get V = (1/d1)*(-B^2 + (i1*i1-i2)*B + i1*i3*I)
Matrix3 V(c2*B2(0,0)+c1*B(0,0)+cI, c2*B2(0,1)+c1*B(0,1), c2*B2(0,2)+c1*B(0,2),
c2*B2(1,0)+c1*B(1,0), c2*B2(1,1)+c1*B(1,1)+cI, c2*B2(1,2)+c1*B(1,2),
c2*B2(2,0)+c1*B(2,0), c2*B2(2,1)+c1*B(2,1), c2*B2(2,2)+c1*B(2,2)+cI);
// if R pointer not NULL, find R too
if(R!=NULL)
{ c1 = 1/i3; // coefficient of B
double cV = -i1*c1; // coefficient of V
cI = i2*c1; // coefficient of I
// Get Vinv = (1/i3)*(B - i1*V + i2*I)
Matrix3 Vinv(c1*B(0,0)+cV*V(0,0)+cI, c1*B(0,1)+cV*V(0,1), c1*B(0,2)+cV*V(0,2),
c1*B(1,0)+cV*V(1,0), c1*B(1,1)+cV*V(1,1)+cI, c1*B(1,2)+cV*V(1,2),
c1*B(2,0)+cV*V(2,0), c1*B(2,1)+cV*V(2,1), c1*B(2,2)+cV*V(2,2)+cI);
// R = V^-1 F
*R = Vinv*(*this);
}
// Return Eigenvalues of U if asked
if(stretches!=NULL)
{ stretches->x = lam1;
stretches->y = lam2;
stretches->z = lam3;
}
return V;
}
cv::Point3d scan3d::approximate_ray_intersection(const cv::Point3d & v1, const cv::Point3d & q1,
const cv::Point3d & v2, const cv::Point3d & q2,
double * distance, double * out_lambda1, double * out_lambda2)
{
cv::Mat v1mat = cv::Mat(v1);
cv::Mat v2mat = cv::Mat(v2);
double v1tv1 = cv::Mat(v1mat.t()*v1mat).at<double>(0,0);
double v2tv2 = cv::Mat(v2mat.t()*v2mat).at<double>(0,0);
double v1tv2 = cv::Mat(v1mat.t()*v2mat).at<double>(0,0);
double v2tv1 = cv::Mat(v2mat.t()*v1mat).at<double>(0,0);
//cv::Mat V(2, 2, CV_64FC1);
//V.at<double>(0,0) = v1tv1; V.at<double>(0,1) = -v1tv2;
//V.at<double>(1,0) = -v2tv1; V.at<double>(1,1) = v2tv2;
//std::cout << " V: "<< V << std::endl;
cv::Mat Vinv(2, 2, CV_64FC1);
double detV = v1tv1*v2tv2 - v1tv2*v2tv1;
Vinv.at<double>(0,0) = v2tv2/detV; Vinv.at<double>(0,1) = v1tv2/detV;
Vinv.at<double>(1,0) = v2tv1/detV; Vinv.at<double>(1,1) = v1tv1/detV;
//std::cout << " V.inv(): "<< V.inv() << std::endl << " Vinv: " << Vinv << std::endl;
//cv::Mat Q(2, 1, CV_64FC1);
//Q.at<double>(0,0) = cv::Mat(v1mat.t()*(cv::Mat(q2-q1))).at<double>(0,0);
//Q.at<double>(1,0) = cv::Mat(v2mat.t()*(cv::Mat(q1-q2))).at<double>(0,0);
//std::cout << " Q: "<< Q << std::endl;
cv::Point3d q2_q1 = q2 - q1;
double Q1 = v1.x*q2_q1.x + v1.y*q2_q1.y + v1.z*q2_q1.z;
double Q2 = -(v2.x*q2_q1.x + v2.y*q2_q1.y + v2.z*q2_q1.z);
//cv::Mat L = V.inv()*Q;
//cv::Mat L = Vinv*Q;
//std::cout << " L: "<< L << std::endl;
double lambda1 = (v2tv2 * Q1 + v1tv2 * Q2) /detV;
double lambda2 = (v2tv1 * Q1 + v1tv1 * Q2) /detV;
//std::cout << "lambda1: " << lambda1 << " lambda2: " << lambda2 << std::endl;
//cv::Mat p1 = L.at<double>(0,0)*v1mat + cv::Mat(q1); //ray1
//cv::Mat p2 = L.at<double>(1,0)*v2mat + cv::Mat(q2); //ray2
//cv::Point3d p1 = L.at<double>(0,0)*v1 + q1; //ray1
//cv::Point3d p2 = L.at<double>(1,0)*v2 + q2; //ray2
cv::Point3d p1 = lambda1*v1 + q1; //ray1
cv::Point3d p2 = lambda2*v2 + q2; //ray2
//cv::Point3d p = cv::Point3d(cv::Mat((p1+p2)/2.0));
cv::Point3d p = 0.5*(p1+p2);
if (distance!=NULL)
{
*distance = cv::norm(p2-p1);
}
if (out_lambda1)
{
*out_lambda1 = lambda1;
}
if (out_lambda2)
{
*out_lambda2 = lambda2;
}
return p;
}
