//------------------------------------------------------------------------------
void test_getInverse()
{
Matrix* pA = new Matrix(ROWS,COLS,pArr,SIZE);
Matrix* pI = new Matrix(ROWS,COLS);
// Show the base matrix
std::cout << "=============================================" << std::endl;
std::cout << "A = " << std::endl;
pA->showMatrix(); std::cout << std::endl;
// Calculate inverse by GJ method
Matrix* pInv = pA->getInvGJ();
std::cout << "inv(A) by Gauss-Jordan method = " << std::endl;
pInv->showMatrix(); std::cout << std::endl;
std::cout << "A * inv(A) = " << std::endl;
pI = multiply(*pA, *pInv);
pI->showMatrix(); std::cout << std::endl;
// Calculate inverse by LU method
pInv = pA->getInvLU();
std::cout << "inv(A) by LU method = " << std::endl;
pInv->showMatrix(); std::cout << std::endl;
std::cout << "A * inv(A) = " << std::endl;
pI = multiply(*pA, *pInv);
pI->showMatrix(); std::cout << std::endl;
// cleanup
pA->unref();
pI->unref();
pInv->unref();
}
//------------------------------------------------------------------------------
// Test Matrix class transformation functions
//------------------------------------------------------------------------------
void test_getTriDiagonal()
{
// Test "getTriDiagonal"
std::cout << "=============================================" << std::endl;
std::cout << "Test getTriDiagonal" << std::endl;
Matrix* pA = new Matrix(ROWS,COLS,pArr,SIZE);
// Show the base matrix
std::cout << "=============================================" << std::endl;
std::cout << "A = " << std::endl;
pA->showMatrix(); std::cout << std::endl;
Matrix* pB = new Matrix(ROWS,COLS);
bool b1 = pA->getTriDiagonal(pB);
std::cout << "getTriDiagonal function return value = " << std::boolalpha << b1 << std::endl;
std::cout << std::endl;
std::cout << "getTriDiagonal = " << std::endl;
pB->showMatrix(); std::cout << std::endl;
// cleanup
if (pA != 0) pA->unref(); // Decrement the reference counter for pointer pA
if (pB != 0) pB->unref(); // Decrement the reference counter for pointer pB
}
//------------------------------------------------------------------------------
// Test Matrix class overloaded operators
//------------------------------------------------------------------------------
void testOverloadedOps()
{
// Test Matrix class overloaded operators
std::cout << "=============================================" << std::endl;
std::cout << "Test overloaded operators" << std::endl;
//---------------------------------------------
// Matrix data
//---------------------------------------------
Matrix* pA = new Matrix(ROWS,COLS,pArr,SIZE);
std::cout << "A = " << std::endl;
pA->showMatrix(); std::cout << std::endl;
// overloaded "="
Matrix B = *pA; // Now matrix A and B are identical
std::cout << "B = " << std::endl;
B.showMatrix(); std::cout << std::endl;
// overloaded "()"
std::cout << "B(1,1) = " << B(1,1) << std::endl;
// overloaded "[]"
std::cout << "B[2] = " << B[2] << std::endl;
std::cout << std::endl;
// overloaded "=="
std::cout << "A == B (?) = " << std::boolalpha << (*pA == B) << std::endl;
std::cout << "A != B (?) = " << std::boolalpha << (*pA != B) << std::endl;
// overloaded "!="
B(0,0) = 0.0; // Now matrix A and B are different
std::cout << "A == B (?) = " << std::boolalpha << (*pA == B) << std::endl;
std::cout << "A != B (?) = " << std::boolalpha << (*pA != B) << std::endl;
std::cout << std::endl;
// overloaded "<<"
std::cout << "B = " << std::endl << B << std::endl;
CVector* pV = new CVector(3);
(*pV)[0] = std::sqrt(2.0);
(*pV)[1] = std::log(2.0);
(*pV)[2] = std::exp(2.0);
pV->setDecPoint(7);
pV->setFldWidth(15);
std::cout << "DP = " << pV->getDecPoint() << std::endl;
std::cout << "FW = " << pV->getFldWidth() << std::endl;
std::cout << "V = " << std::endl;
std::cout << *pV << std::endl; // Note field width setting of 15 is ignored for
// display with "<<"
std::cout << "DP = " << pV->getDecPoint() << std::endl;
std::cout << "FW = " << pV->getFldWidth() << std::endl;
std::cout << "V = " << std::endl;
pV->showVector(3,16);
// Cleanup
if (pA != 0) pA->unref(); // Decrement the reference counter for pointer pA
if (pV != 0) pV->unref(); // Decrement the reference counter for pointer pV
}
//------------------------------------------------------------------------------
// Test Matrix class information access
//------------------------------------------------------------------------------
void testInformation()
{
// Test Matrix class information functions
std::cout << "=============================================" << std::endl;
std::cout << "Test information functions" << std::endl;
//---------------------------------------------
// Matrix data
//---------------------------------------------
Matrix* pA = new Matrix(ROWS,COLS,pArr,SIZE);
std::cout << "A = " << std::endl;
pA->showMatrix(); std::cout << std::endl;
// Numerical info
std::cout << "Rows = " << pA->getRows() << std::endl;
std::cout << "Columns = " << pA->getCols() << std::endl;
std::cout << "Field Width = " << pA->getFldWidth() << std::endl;
std::cout << "Precision = " << pA->getDecPoint() << std::endl;
std::cout << "Max Mag = " << pA->getMaxMag() << std::endl;
std::cout << "Min Mag = " << pA->getMinMag() << std::endl;
std::cout << "Element(1,2) = " << pA->getElem(1,2) << std::endl;
// Boolean info
std::cout << "Good index(1,0)? = " << std::boolalpha << pA->isGoodIndex(1,0) << std::endl;
std::cout << "Good index(1,8)? = " << std::boolalpha << pA->isGoodIndex(1,8) << std::endl;
std::cout << "Good matrix? = " << std::boolalpha << pA->isGoodMatrix() << std::endl;
std::cout << "Square? = " << std::boolalpha << pA->isSquare() << std::endl;
std::cout << "Symmetric? = " << std::boolalpha << pA->isSymmetric() << std::endl;
// Cleanup
if (pA != 0) pA->unref(); // Decrement the reference counter for pointer pA
}
//------------------------------------------------------------------------------
void test_getCholesky()
{
// Test "getCholesky"
std::cout << "=============================================" << std::endl;
std::cout << "Test getCholesky" << std::endl;
Matrix* pA = new Matrix(ROWS,COLS,pArr,SIZE);
// Show the base matrix
std::cout << "=============================================" << std::endl;
std::cout << "A = " << std::endl;
pA->showMatrix(); std::cout << std::endl;
Matrix* pL = new Matrix(ROWS,COLS);
Matrix* pU = new Matrix(ROWS,COLS);
bool b1 = pA->getCholesky(pL, pU);
std::cout << "getCholesky function return value = " << std::boolalpha << b1 << std::endl;
std::cout << std::endl;
if (pL != 0) {
std::cout << "L = " << std::endl;
pL->showMatrix(); std::cout << std::endl;
}
if (pU != 0) {
std::cout << "U = " << std::endl;
pU->showMatrix(); std::cout << std::endl;
}
Matrix* pB = multiply(*pL, *pU);
if (pB != 0) {
std::cout << "L*U = A (?)" << std::endl;
pB->showMatrix(); std::cout << std::endl;
}
// cleanup
if (pA != 0) pA->unref(); // Decrement the reference counter for pointer pA
if (pB != 0) pB->unref(); // Decrement the reference counter for pointer pB
if (pL != 0) pL->unref(); // Decrement the reference counter for pointer pL
if (pU != 0) pU->unref(); // Decrement the reference counter for pointer pU
}
//------------------------------------------------------------------------------
void test_getLU()
{
// Test "getLU"
std::cout << "=============================================" << std::endl;
std::cout << "Test getLU" << std::endl;
Matrix* pA = new Matrix(ROWS,COLS,pArr,SIZE);
// Show the base matrix
std::cout << "=============================================" << std::endl;
std::cout << "A = " << std::endl;
pA->showMatrix(); std::cout << std::endl;
Matrix* pL = new Matrix(ROWS,COLS);
Matrix* pU = new Matrix(ROWS,COLS);
bool b1 = pA->getLU(pL, pU);
std::cout << "getLU function return value = " << std::boolalpha << b1 << std::endl;
std::cout << std::endl;
std::cout << "L = " << std::endl;
pL->showMatrix(); std::cout << std::endl;
CVector* pV = getDiag(*pL);
std::cout << "Diagonal = all 1's (?)" << std::endl;
pV->showVector(); std::cout << std::endl;
std::cout << "U = " << std::endl;
pU->showMatrix(); std::cout << std::endl;
Matrix* pB = multiply(*pL, *pU);
std::cout << "L*U = A (?)" << std::endl;
pB->showMatrix(); std::cout << std::endl;
// cleanup
if (pA != 0) pA->unref(); // Decrement the reference counter for pointer pA
if (pB != 0) pB->unref(); // Decrement the reference counter for pointer pB
if (pL != 0) pL->unref(); // Decrement the reference counter for pointer pL
if (pU != 0) pU->unref(); // Decrement the reference counter for pointer pU
if (pV != 0) pV->unref(); // Decrement the reference counter for pointer pV
}
//------------------------------------------------------------------------------
void test_getEigenPower()
{
double maxErr = .0001;
int maxIter = 100;
double Val = 0.0;
CVector* pVec = new CVector(ROWS);
Matrix* pA = new Matrix(ROWS,COLS,pArr,SIZE);
// Show the base matrix
std::cout << "=============================================" << std::endl;
std::cout << "A = " << std::endl;
pA->showMatrix(); std::cout << std::endl;
bool b1 = pA->getEigenPower(maxErr, maxIter, &Val, pVec);
std::cout << "getEigenPower function return value = " << std::boolalpha << b1 << std::endl;
std::cout << std::endl;
std::cout << std::setprecision(4);
std::cout << "Eigenvalue = " << Val << std::endl;
std::cout << "EigenVector = " << std::endl << *pVec << std::endl;
CVector* pV1 = multiply(*pA, *pVec);
std::cout << "A*Vec = " << std::endl;
pV1->showVector(6); std::cout << std::endl;
CVector* pV2 = multiply(*pVec, Val);
std::cout << "Vec*Val = " << std::endl;
pV2->showVector(6); std::cout << std::endl;
CVector* pErr = subtract(*pV1, *pV2);
double NormErr = pErr->getNorm();
std::cout << "Err = " << std::endl;
pErr->showVector(6); std::cout << std::endl;
std::cout << "normalized error = " << std::endl << NormErr << std::endl;
// cleanup
pVec->unref();
pA->unref();
pV1->unref();
pV2->unref();
pErr->unref();
}
void testSegmentation(){
///////////////////////////////////////////////////
std::ofstream outFile;
std::string pathName;
pathName = "./outputFile_StereoTracking.txt";
outFile.open(pathName.c_str());
///////////////////////////////////////////////////
std::cout << "TESTING SEGMENTATION ALGORITHM && EKF" << std::endl;
cv::Mat img1, ori1, img2, ori2;
std::string path = "";
#if defined (__linux)
path = "/home/bardo91/Programming/Images/";
#endif
#if defined (_WIN32)
path = "C:/Programming/ImagenesStereoTracking/P2_640x480/Images/";
#endif
std::cout << "--Path of images: " << path << std::endl;
bool condition = true;
int i = 0;
std::cout << "--Init Timer" << std::endl;
BOViL::STime::init();
BOViL::STime *time = BOViL::STime::get();
double t0, t1;
std::cout << "--Create Clustered Space" << std::endl;
BOViL::ColorClusterSpace *cs = BOViL::CreateHSVCS_8c(255U,255U, std::uint8_t(BOViL::bin2dec("00010000")));
//-------
std::cout << "--Init Stereo EKF" << std::endl;
BOViL::algorithms::StereoVisionEKF stereoEKF;
stereoEKF.setUpEKF( Matrix<double>(arrayQ, 6, 6),
Matrix<double>(arrayR, 4, 4),
Matrix<double>(arrayX0, 6, 1));
stereoEKF.setUpCameras(738.143358488352310, 346.966835812843040 , 240.286986071815390);
double inputBuffer[20];
std::ifstream inputFile;
std::cout << "--Open Input File" << std::endl;
#if defined (__linux)
condition = openInputFile(inputFile, "/home/bardo91/Programming/Images/ViconData2.txt");
#endif
#if defined (_WIN32)
condition = openInputFile(inputFile, "C:/Programming/ImagenesStereoTracking/P2_640x480/ViconData2.txt");
#endif
double lastTime = 0;
while(condition){
//t0 = time->getTime();
// ----------------- IMAGE SEGMENTATION ------------------------
std::stringstream ss1, ss2;
ss1 << path;
ss1 << "img";
ss1 << i;
ss2 << ss1.str();
ss1 << "_cam1.jpg";
ss2 << "_cam2.jpg";
std::string imagePath1 = ss1.str();
std::string imagePath2= ss2.str();
img1 = cv::imread(imagePath1, CV_LOAD_IMAGE_COLOR);
img2 = cv::imread(imagePath2, CV_LOAD_IMAGE_COLOR);
img1.copyTo(ori1);
img2.copyTo(ori2);
t0 = time->getTime();
std::vector<BOViL::ImageObject> objects1;
std::vector<BOViL::ImageObject> objects2;
cv::cvtColor(img1, img1, CV_BGR2HSV);
cv::cvtColor(img2, img2, CV_BGR2HSV);
BOViL::algorithms::ColorClustering<std::uint8_t>( img1.data, // Image pointer
img1.cols, // Width
img1.rows, // Height
5, // Size Threshold
objects1, // Output Objects
*cs); // Segmentation function
BOViL::algorithms::ColorClustering<std::uint8_t>( img2.data, // Image pointer
img2.cols, // Width
img2.rows, // Height
5, // Size Threshold
objects2, // Output Objects
*cs); // Segmentation function
t1 = time->getTime();
double fps = 1/(t1-t0);
std::cout << fps << " fps" << std::endl;
std::cout << "Number of detected Objects1 in the scene: " << objects1.size() << std::endl;
std::cout << "Number of detected Objects2 in the scene: " << objects2.size() << std::endl;
int maxY1 = 0, maxIndex1 = 0;
int maxY2 = 0, maxIndex2 = 0;
if (objects1.size() != 0 && objects2.size() != 0){
// Select Object
for (unsigned int obj = 0; obj < objects1.size(); obj++){
if (objects1[obj].getCentroid().y > maxY1){
maxY1 = objects1[obj].getCentroid().y;
maxIndex1 = obj;
}
}
for (unsigned int obj = 0; obj < objects2.size(); obj++){
if (objects2[obj].getCentroid().y > maxY2){
maxY2 = objects2[obj].getCentroid().y;
maxIndex2 = obj;
}
}
BOViL::Point2ui p = objects1[maxIndex1].getCentroid();
cv::circle(ori1, cv::Point2d(p.x, p.y), objects1[maxIndex1].getHeight() / 2, cv::Scalar(0, 255, 0), 1);
p = objects2[maxIndex2].getCentroid();
cv::circle(ori2, cv::Point2d(p.x, p.y), objects2[maxIndex2].getHeight() / 2, cv::Scalar(0, 255, 0), 1);
}
cv::hconcat(ori1, ori2, ori1);
cv::imshow("ORIGINAL", ori1);
// ----------------- TRACKING ALGORITHM ------------------------
dropLineIntoBuffer(inputFile, inputBuffer); // Get objects info.
if (objects1.size() == 0 || objects2.size() == 0)
continue;
// Update cameras pos and ori
double arrayPosC1[3] = {inputBuffer[7], inputBuffer[8], inputBuffer[9]};
double arrayPosC2[3] = {inputBuffer[13], inputBuffer[14], inputBuffer[15]};
Matrix<double> Rx = createRotationMatrix(eEdges::EdgeX, inputBuffer[10]);
Matrix<double> Ry = createRotationMatrix(eEdges::EdgeY, inputBuffer[11]);
Matrix<double> Rz = createRotationMatrix(eEdges::EdgeZ, inputBuffer[12]);
Matrix<double> camOri1 = Rz*Ry*Rx;
Rx = createRotationMatrix(eEdges::EdgeX, inputBuffer[16]);
Ry = createRotationMatrix(eEdges::EdgeY, inputBuffer[17]);
Rz = createRotationMatrix(eEdges::EdgeZ, inputBuffer[18]);
Matrix<double> camOri2 = Rz*Ry*Rx;
Matrix<double> adaptRot = createRotationMatrix(eEdges::EdgeX, PiCte / 2) *
createRotationMatrix(eEdges::EdgeZ, PiCte / 2);
stereoEKF.updateCameras(Matrix<double>(arrayPosC1, 3, 1),
Matrix<double>(arrayPosC2, 3, 1),
adaptRot*camOri1,
adaptRot*camOri2);
double arrayZk[4] = { double (objects1[maxIndex1].getCentroid().x),
img1.rows - double (objects1[maxIndex1].getCentroid().y),
double (objects2[maxIndex2].getCentroid().x),
img2.rows - double(objects2[maxIndex2].getCentroid().y) };
stereoEKF.stepEKF(Matrix<double>(arrayZk, 4, 1), inputBuffer[0] - lastTime);
lastTime = inputBuffer[0];
Matrix<double> state = stereoEKF.getStateVector();
state.showMatrix();
outFile << inputBuffer[0] << "\t" << state[0] << "\t" << state[1] << "\t" << state[2] << "\t" << arrayZk[0] << "\t" << arrayZk[1] << "\t" << arrayZk[2] << "\t" << arrayZk[3] << "\n";
cv::waitKey(1);
++i;
}
std::cout << "----------End----------" << std::endl;
}
//------------------------------------------------------------------------------
void test_getQR()
{
// Test "getQR"
std::cout << "=============================================" << std::endl;
std::cout << "Test getQR" << std::endl;
Matrix* pA = new Matrix(ROWS,COLS,pArr,SIZE);
// Show the base matrix
std::cout << "=============================================" << std::endl;
std::cout << "A = " << std::endl;
pA->showMatrix(); std::cout << std::endl;
Matrix* pQ = new Matrix(ROWS,COLS);
Matrix* pR = new Matrix(ROWS,COLS);
bool b1 = pA->getQR(pQ, pR);
std::cout << "getQR function return value = " << std::boolalpha << b1 << std::endl;
std::cout << std::endl;
std::cout << "Q = " << std::endl;
pQ->showMatrix(); std::cout << std::endl;
std::cout << "R = " << std::endl;
pR->showMatrix(); std::cout << std::endl;
std::cout << "R = upper triangular (?)" << std::endl;
std::cout << "Q = orthogonal (?)" << std::endl;
std::cout << "det(Q) = " << pQ->getDeterm() << std::endl;
std::cout << std::endl;
Matrix* pQT = pQ->getTranspose();
std::cout << "transpose(Q) = Q' (?)" << std::endl;
pQT->showMatrix(); std::cout << std::endl;
Matrix* pQinv = pQ->getInvLU();
std::cout << "inverse(Q) = Q' (?)" << std::endl;
pQinv->showMatrix(); std::cout << std::endl;
Matrix* pI = multiply(*pQ, *pQinv);
std::cout << "Q * Qinv = I (?)" << std::endl;
pI->showMatrix(); std::cout << std::endl;
// get eigenvalues
Matrix* pB = new Matrix(*pA);
for (int i=0; i<10; i++)
{
pB->getQR(pQ, pR);
pB = multiply(*pR, *pQ);
}
std::cout << "R * Q (after 10 iterations) = " << std::endl;
pB->showMatrix(); std::cout << std::endl;
CVector* pV = getDiag(*pB);
std::cout << "Diagonal = eigenvalue estimates" << std::endl;
pV->showVector(); std::cout << std::endl;
// cleanup
if (pQ != 0) pQ->unref(); // Decrement the reference counter for pointer pQ
if (pR != 0) pR->unref(); // Decrement the reference counter for pointer pR
if (pQT != 0) pQT->unref(); // Decrement the reference counter for pointer pQT
if (pQinv != 0) pQinv->unref(); // Decrement the reference counter for pointer pQinv
if (pI != 0) pI->unref(); // Decrement the reference counter for pointer pI
}
