/**
* @function fillEq
*/
void trifocalTensor::fillEq() {
mPointer = 0;
mNumTotalEq = 9*mPPP.size() + 3*mLLL.size() + 3*mPLP.size() + 1*mPLL.size();
printf("Total number of equations (some of them redundant): %d \n", mNumTotalEq );
mEq = Eigen::MatrixXf::Zero( mNumTotalEq, 27 );
mB = Eigen::VectorXf::Zero( mNumTotalEq );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* [Beginning] Rank of mEq is: %d \n", lu.rank() );
// Fill Point-Point-Point ( 9 Equations - 4 DOF )
for( int i = 0; i < mPPP.size(); i++ ) {
fillEq_PPP( mPPP[i][0], mPPP[i][1], mPPP[i][2] );
//Eigen::MatrixXf yam = mEq.block(mPointer - 9, 0, 9, mEq.cols() );
Eigen::MatrixXf yam = mEq;
Eigen::FullPivLU<Eigen::MatrixXf> lu(yam);
printf("* [PPP][%d] Rank of mEq(%d x %d) is: %d \n", i, yam.rows(), yam.cols(), lu.rank() );
}
Eigen::FullPivLU<Eigen::MatrixXf> luA(mEq);
printf("* After [PPP] Rank of mEq is: %d \n",luA.rank() );
// Fill Line -Line - Line ( 3 Equations - 2 DOF )
for( int j = 0; j < mLLL.size(); j++ ) {
fillEq_LLL( mLLL[j][0], mLLL[j][1], mLLL[j][2] );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* [LLL][%d] Rank of mEq is: %d \n", j, lu.rank() );
}
// Fill Point - Line - Line ( 1 Equations - 1 DOF )
for( int i = 0; i < mPLL.size(); ++i ) {
fillEq_PLL( mPLL[i][0], mPLL[i][1], mPLL[i][2] );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* [PLL][%d] Rank of mEq is: %d \n", i, lu.rank() );
}
// Fill Point - Line - Point ( 3 Equations - 2 DOF )
for( int i = 0; i < mPLP.size(); ++i ) {
fillEq_PLP( mPLP[i][0], mPLP[i][1], mPLP[i][2] );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* [PLP][%d] Rank of mEq is: %d \n", i, lu.rank() );
}
}
bool LinearAlgebra::isLinearIndependent(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
bool result = false;
int rank;
/* TODO: test if the vectors a and b are linear independent.
Return true if they are independent, false if they are dependent.*/
Eigen::MatrixXd A(2,3);
A << a.x(), a.y(), a.z(),
b.x(), b.y(), b.z();
//std::cout<<"A:"<<A<<endl;
Eigen::FullPivLU<Eigen::MatrixXd> luA(A);
rank = luA.rank();
if (rank == 2)
result = true;
else
result = false;
return result;
}
