Vector2d leastSquareSolverCalib::pointAlignerLoop( Vector2d &x, MatrixXd &Z, int iterations, Vector2d &result)
{
result=x;
MatrixXd H(2,2);
H.setZero();
MatrixXd b(2,1);
b.setZero();
Vector2d X;
std::cout << Z.transpose()<< std::endl;
for(int i = 0; i < iterations; i++){
std::cout<<"iteration "<<i <<std::endl;
X=x;
for(int j=0;j<Z.cols();j++){
Matrix2d J = computeJacobian(j,Z);
Vector2d e=computeError(j,Z);
std:: cout << "error: "<< e<< std::endl<<std::endl;
H+=J.transpose()*J;
b+=J.transpose()*e;
}
}
Vector2d dx;
std::cout << "H: "<<std::endl<<H<<std::endl<<std::endl<<std::endl;
std::cout << "b: "<<std::endl<<b<<std::endl;
LDLT<MatrixXd> ldlt(-H);
dx=ldlt.solve(b); // using a LDLT factorizationldlt;
return dx;
}
MatrixXd Triangle<ConcreteShape>::buildStiffnessMatrix(VectorXd velocity) {
Matrix2d invJ;
double detJ;
std::tie(invJ,detJ) = ConcreteShape::inverseJacobian(mVtxCrd);
//Jinv= rx, sx,
// rz, sz;
auto drdx = invJ(0,0);
auto dsdx = invJ(0,1);
auto drdz = invJ(1,0);
auto dsdz = invJ(1,1);
// save for later reuse
mInvJac = invJ;
mInvJacT_x_invJac = invJ.transpose() * invJ;
mInvJacT = invJ.transpose();
mDetJac = detJ;
// build material on all nodes
MatrixXd elementStiffnessMatrix(mNumIntPnt,mNumIntPnt);
// interpolate velocity at all nodes
// for(int i=0;i<mNumIntPnt;i++) {
// auto r = mIntegrationCoordinates_r[i];
// auto s = mIntegrationCoordinates_s[i];
// velocity(i) = interpolateAtPoint(r,s).dot(mMaterialVelocityAtVertices);
// }
// loop over matrix(i,j)
for(int i=0;i<mNumIntPnt;i++) {
VectorXd dPhi_dr_i = mGradientPhi_dr.row(i);
VectorXd dPhi_ds_i = mGradientPhi_ds.row(i);
auto dPhi_dx_i = dPhi_dr_i*drdx + dPhi_ds_i*dsdx;
auto dPhi_dz_i = dPhi_dr_i*drdz + dPhi_ds_i*dsdz;
for(int j=0;j<mNumIntPnt;j++) {
VectorXd dPhi_dr_j = mGradientPhi_dr.row(j);
VectorXd dPhi_ds_j = mGradientPhi_ds.row(j);
auto dPhi_dx_j = dPhi_dr_j*drdx + dPhi_ds_j*dsdx;
auto dPhi_dz_j = dPhi_dr_j*drdz + dPhi_ds_j*dsdz;
elementStiffnessMatrix(i,j) = detJ*mIntegrationWeights.dot((velocity.array().pow(2) * dPhi_dx_i.array() * dPhi_dx_j.array()).matrix()) +
detJ*mIntegrationWeights.dot((velocity.array().pow(2) * dPhi_dz_i.array() * dPhi_dz_j.array()).matrix());
}
}
return elementStiffnessMatrix;
}