void ZOHTest::testMatrixExp1()
{
std::cout << "===========================================" <<std::endl;
std::cout << " ===== ZOH tests start ... ===== " <<std::endl;
std::cout << "===========================================" <<std::endl;
std::cout << "------- Compute matrix exponential of a upper triangular matrix -------" <<std::endl;
_A->zero();
(*_A)(0, 1) = 1;
init();
_sim->computeOneStep();
_sim->nextStep();
SP::SimpleMatrix tmpM(new SimpleMatrix(_n, _n, 0));
tmpM->eye();
(*tmpM)(0, 1) = _h;
const SimpleMatrix& Phi = _ZOH->Ad(_DS);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp1 : ", Phi.size(0) == _n, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp1 : ", Phi.size(1) == _n, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp1 : ", (*tmpM - Phi).normInf() < _tol, true);
std::cout << "------- Second computation ok, error = " << (*tmpM - Phi).normInf() << " -------" <<std::endl;
std::cout <<std::endl <<std::endl;
}
void ZOHTest::testMatrixExp0()
{
std::cout << "===========================================" <<std::endl;
std::cout << " ===== ZOH tests start ... ===== " <<std::endl;
std::cout << "===========================================" <<std::endl;
std::cout << "------- Compute matrix exponential of the identity matrix -------" <<std::endl;
_A->eye();
init();
_sim->computeOneStep();
_sim->nextStep();
SP::SimpleMatrix tmpM(new SimpleMatrix(_n, _n, 0));
tmpM->eye();
*tmpM = (*tmpM) * exp(_h);
const SimpleMatrix& Phi = _ZOH->Ad(_DS);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp0 : ", Phi.size(0) == _n, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp0 : ", Phi.size(1) == _n, true);
double diff = (*tmpM - Phi).normInf();
CPPUNIT_ASSERT_EQUAL_MESSAGE("testMatrixExp0 : ", diff < _tol, true);
std::cout << "------- First computation ok, error = " << diff << " -------" <<std::endl;
std::cout <<std::endl <<std::endl;
}
void calcAndResetEmissions(SeqData const & seqData, Grammar & g, EmitWrappers & ew, EmitAnnoMap & eam, string const & annoName)
{
// mk emissions
unsigned n = seqData.seqSize();
xvector_t tmpV(n);
reset(tmpV);
vector<xvector_t> vv(ew.vecWrapNames().size(), tmpV);
ew.vectorEmissions(vv, seqData);
xmatrix_t tmpM(n, n);
reset(tmpM);
vector<xmatrix_t> vm(ew.matWrapNames().size(), tmpM);
ew.matrixEmissions(vm, seqData);
// mask according to anno
if ( eam.size() and annoName.size() ) {
string const & anno = seqData.getAnno(annoName);
maskEmissions(vv, ew.vecWrapNames(), anno, eam);
maskEmissions(vm, ew.matWrapNames(), anno, eam);
}
g.resetEmissions(vv, vm);
}
void GFilter::InitiateGuidance( const cv::Mat_<cv::Vec3b> &gImg, int radius, float epsl )
{
cv::Mat_<float> varIBB, varIBG, varIBR, varIGG, varIGR, varIRR;
cv::Mat_<float> mIBB, mIBG, mIBR, mIGG, mIGR, mIRR;
int height, width;
height = gImg.rows;
width = gImg.cols;
cv::Mat gArr[3];
cv::split(gImg, gArr);
gArr[0].convertTo(iB, CV_32FC1, 1/255.0f);
gArr[1].convertTo(iG, CV_32FC1, 1/255.0f);
gArr[2].convertTo(iR, CV_32FC1, 1/255.0f);
// window normalize size
normWin.create(height, width);
normWin.setTo(1.0);
TheBoxFilter(normWin, normWin, radius);
// mean of guidance image I
TheBoxFilter(iB, mIB, radius);
TheBoxFilter(iG, mIG, radius);
TheBoxFilter(iR, mIR, radius);
mIB /= normWin;
mIG /= normWin;
mIR /= normWin;
// variance of guidance image I
TheBoxFilter(iB.mul(iB), mIBB, radius);
TheBoxFilter(iB.mul(iG), mIBG, radius);
TheBoxFilter(iB.mul(iR), mIBR, radius);
TheBoxFilter(iG.mul(iG), mIGG, radius);
TheBoxFilter(iG.mul(iR), mIGR, radius);
TheBoxFilter(iR.mul(iR), mIRR, radius);
mIBB /= normWin;
mIBG /= normWin;
mIBR /= normWin;
mIGG /= normWin;
mIGR /= normWin;
mIRR /= normWin;
varIBB = mIBB - mIB.mul(mIB);
varIBG = mIBG - mIB.mul(mIG);
varIBR = mIBR - mIB.mul(mIR);
varIGG = mIGG - mIG.mul(mIG);
varIGR = mIGR - mIG.mul(mIR);
varIRR = mIRR - mIR.mul(mIR);
// calculate inverse
int iy, ix;
//invIU.create(height, width);
invIU.resize(height);
cv::Matx33f epsU(
epsl, 0.0, 0.0,
0.0, epsl, 0.0,
0.0, 0.0, epsl);
for (iy=0; iy<height; ++iy)
{
invIU[iy].resize(width);
for (ix=0; ix<width; ++ix)
{
//printf("0: %d %d\n", iy, ix);
cv::Matx33f tmpM(
varIBB[iy][ix], varIBG[iy][ix], varIBR[iy][ix],
varIBG[iy][ix], varIGG[iy][ix], varIGR[iy][ix],
varIBR[iy][ix], varIGR[iy][ix], varIRR[iy][ix]);
invIU[iy][ix] = (tmpM+epsU).inv();
//invIU(iy, ix) = epsU.inv();
//printf("%d %d %d %d\n", iy, ix, invIU.rows, invIU.cols);
//invIU[iy][ix] = epsU.inv();
}
}
}
void GFilter::InitiateGuidance( const cv::Mat_<cv::Vec3f> &gImg, int radius, float epsl )
{
cv::Mat_<float> varIBB, varIBG, varIBR, varIGG, varIGR, varIRR;
cv::Mat_<float> mIBB, mIBG, mIBR, mIGG, mIGR, mIRR;
int height, width;
height = gImg.rows;
width = gImg.cols;
cv::Mat gArr[3];
cv::split(gImg, gArr);
gArr[0].convertTo(iB, CV_32FC1, 1/255.0f);
gArr[1].convertTo(iG, CV_32FC1, 1/255.0f);
gArr[2].convertTo(iR, CV_32FC1, 1/255.0f);
// window normalize size
normWin.create(height, width);
normWin.setTo(1.0);
TheBoxFilter(normWin, normWin, radius);
// mean of guidance image I
TheBoxFilter(iB, mIB, radius);
TheBoxFilter(iG, mIG, radius);
TheBoxFilter(iR, mIR, radius);
mIB /= normWin;
mIG /= normWin;
mIR /= normWin;
// variance of guidance image I
TheBoxFilter(iB.mul(iB), mIBB, radius);
TheBoxFilter(iB.mul(iG), mIBG, radius);
TheBoxFilter(iB.mul(iR), mIBR, radius);
TheBoxFilter(iG.mul(iG), mIGG, radius);
TheBoxFilter(iG.mul(iR), mIGR, radius);
TheBoxFilter(iR.mul(iR), mIRR, radius);
mIBB /= normWin;
mIBG /= normWin;
mIBR /= normWin;
mIGG /= normWin;
mIGR /= normWin;
mIRR /= normWin;
varIBB = mIBB - mIB.mul(mIB);
varIBG = mIBG - mIB.mul(mIG);
varIBR = mIBR - mIB.mul(mIR);
varIGG = mIGG - mIG.mul(mIG);
varIGR = mIGR - mIG.mul(mIR);
varIRR = mIRR - mIR.mul(mIR);
// calculate inverse
int iy, ix;
//invIU.create(height, width);
invIU.resize(height);
cv::Matx33f epsU(
epsl, 0.0, 0.0,
0.0, epsl, 0.0,
0.0, 0.0, epsl);
for (iy=0; iy<height; ++iy)
{
invIU[iy].resize(width);
for (ix=0; ix<width; ++ix)
{
//printf("0: %d %d\n", iy, ix);
cv::Matx33f tmpM(
varIBB[iy][ix], varIBG[iy][ix], varIBR[iy][ix],
varIBG[iy][ix], varIGG[iy][ix], varIGR[iy][ix],
varIBR[iy][ix], varIGR[iy][ix], varIRR[iy][ix]);
// [added - 2013-10-18], try to avoid singular matrix
// but no effect
tmpM = tmpM+epsU;
/*double dtrm = cv::determinant(tmpM);
if (dtrm <= std::numeric_limits<double>::epsilon())
{
tmpM(0, 0) += 0.01;
tmpM(1, 1) += 0.01;
tmpM(2, 2) += 0.01;
}*/
invIU[iy][ix] = tmpM.inv();
//invIU(iy, ix) = epsU.inv();
//printf("%d %d %d %d\n", iy, ix, invIU.rows, invIU.cols);
//invIU[iy][ix] = epsU.inv();
}
}
}
