MatrixXd Utils::getUpperTriangularCholesky(MatrixXd KKt)
{
LLT<MatrixXd> llt(KKt);
MatrixXd K = llt.matrixU();
return K;
}
MatrixXd Utils::calculateHomographyMatrixFromFiveOrtoghonalLines(QList<Line*> firstOrtoghonalLines, QList<Line*> secondOrthogonalLines,
QList<Line*> thirdOrthogonalLines, QList<Line*> fourthOrthogonalLines,
QList<Line*> fifthOrthogonalLines)
{
// A * x = b.
MatrixXd A(5, 6);
MatrixXd b(5, 1);
MatrixXd x(5, 1);
Vector3d l1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(0));
Vector3d m1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(1));
Vector3d l2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(0));
Vector3d m2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(1));
Vector3d l3 = getLineInHomogeneousCoordinates(thirdOrthogonalLines.at(0));
Vector3d m3 = getLineInHomogeneousCoordinates(thirdOrthogonalLines.at(1));
Vector3d l4 = getLineInHomogeneousCoordinates(fourthOrthogonalLines.at(0));
Vector3d m4 = getLineInHomogeneousCoordinates(fourthOrthogonalLines.at(1));
Vector3d l5 = getLineInHomogeneousCoordinates(fifthOrthogonalLines.at(0));
Vector3d m5 = getLineInHomogeneousCoordinates(fifthOrthogonalLines.at(1));
b << -l1(1)*m1(1), -l2(1)*m2(1), -l3(1)*m3(1), -l4(1)*m4(1), -l5(1)*m5(1);
A << l1(0)*m1(0), (l1(0)*m1(1)+l1(1)*m1(0))/2, l1(1)*m1(1), (l1(0)*m1(2)+l1(2)*m1(0))/2, (l1(1)*m1(2)+l1(2)*m1(1))/2, l1(2)*m1(2),
l2(0)*m2(0), (l2(0)*m2(1)+l2(1)*m2(0))/2, l2(1)*m2(1), (l2(0)*m2(2)+l2(2)*m2(0))/2, (l2(1)*m2(2)+l2(2)*m2(1))/2, l2(2)*m2(2),
l3(0)*m3(0), (l3(0)*m3(1)+l3(1)*m3(0))/2, l3(1)*m3(1), (l3(0)*m3(2)+l3(2)*m3(0))/2, (l3(1)*m3(2)+l3(2)*m3(1))/2, l3(2)*m3(2),
l4(0)*m4(0), (l4(0)*m4(1)+l4(1)*m4(0))/2, l4(1)*m4(1), (l4(0)*m4(2)+l4(2)*m4(0))/2, (l4(1)*m4(2)+l4(2)*m4(1))/2, l4(2)*m4(2),
l5(0)*m5(0), (l5(0)*m5(1)+l5(1)*m5(0))/2, l5(1)*m5(1), (l5(0)*m5(2)+l5(2)*m5(0))/2, (l5(1)*m5(2)+l5(2)*m5(1))/2, l5(2)*m5(2);
x = A.colPivHouseholderQr().solve(b);
x/=x(2);
Matrix3d C;
C << x(0), x(1)/2, x(3)/2,
x(1)/2, x(2), x(4)/2,
x(3)/2, x(4)/2, 1;
Matrix2d kkt;
kkt << C(0,0), C(0,1),
C(1,0), C(1,1);
MatrixXd vKKt(1,2);
vKKt << C(2,0), C(2,1);
MatrixXd V(1,2);
V = vKKt * kkt.inverse();
LLT<MatrixXd> llt(kkt);
MatrixXd U = llt.matrixU();
MatrixXd J (3,3);
J << U(0,0), U(0,1),0, U(1,0), U(1,1),0, V(0), V(1), 1;
return J;
}
//---------------------------------------------------------------------------------------------------------------------
void UnscentedKalmanFilter::sigmaPoints(){
unsigned n = mXak.rows();
double w0 = -1; // 666 Where is choosen?
mSigmaPoints.push_back(pair<MatrixXd, double>(mXak, w0));
MatrixXd lambda = n/(1-w0)*mPk;
// Using not the square root matrix but L of cholesky decomposition
LLT<MatrixXd> llt(lambda);
lambda = llt.matrixL();
for (unsigned i = 0; i < n; i++){
double wj = (1 - w0) / 2 / n;
mSigmaPoints.push_back(pair<MatrixXd, double>(mXak + lambda.col(i), wj));
mSigmaPoints.push_back(pair<MatrixXd, double>(mXak - lambda.col(i), wj));
}
}
