Eigen::MatrixXd FeatureModel::dRq_times_a_by_dq (const Eigen::Quaterniond &q, const Eigen::Vector3d &a)
{
Eigen::MatrixXd aMat(3,1);
aMat(0,0) = a(0);
aMat(1,0) = a(1);
aMat(2,0) = a(2);
Eigen::Matrix3d TempR;
Eigen::MatrixXd Temp31(3,1);
Eigen::MatrixXd dRq_times_a_by_dq(3,4);
// Make Jacobian by stacking four vectors together side by side
TempR = dR_by_dq0(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.block(0,0,3,1) = Temp31;
TempR = dR_by_dqx(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.block(0,1,3,1) = Temp31;
TempR = dR_by_dqy(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.block(0,2,3,1) = Temp31;
TempR = dR_by_dqz(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.block(0,3,3,1) = Temp31;
return dRq_times_a_by_dq;
}
void ImRON2ParmEllipse
(
REAL & A,
REAL & B,
REAL & C,
const Pt2dr & aV0,
const Pt2dr & aV1
)
{
ElMatrix<REAL> aMat(2,2);
SetCol(aMat,0,aV0);
SetCol(aMat,1,aV1);
aMat = gaussj(aMat);
aMat = aMat.transpose() * aMat;
ElMatrix<REAL> aVecP(2,2);
ElMatrix<REAL> aValP(2,2);
jacobi_diag(aMat,aValP,aVecP);
aValP(0,0) = sqrt(aValP(0,0));
aValP(1,1) = sqrt(aValP(1,1));
ElMatrix<REAL> aABC = aVecP * aValP * aVecP.transpose();
A = aABC(0,0);
B = aABC(1,0);
C = aABC(1,1);
}
ElMatrix<double> ImportMat(const cTypeCodageMatr & aCM)
{
ElMatrix<double> aMat(3,3);
SetLig(aMat,0,aCM.L1());
SetLig(aMat,1,aCM.L2());
SetLig(aMat,2,aCM.L3());
return aMat;
}
bool EllipseEq2ParamPhys
(
REAL & V1,
REAL & V2,
REAL & teta,
REAL A,
REAL B,
REAL C
)
{
ElMatrix<REAL> aMat(2,2);
aMat(0,0) = A;
aMat(1,0) = B;
aMat(0,1) = B;
aMat(1,1) = C;
ElMatrix<REAL> aVecP(2,2);
ElMatrix<REAL> aValP(2,2);
jacobi_diag(aMat,aValP,aVecP);
if ((aValP(0,0) <=0) || (aValP(1,1) <= 0))
return false;
V1 = 1 / aValP(0,0);
V2 = 1 / aValP(1,1);
teta = angle(Pt2dr(aVecP(0,0),aVecP(0,1)));
if (V1 < V2)
{
ElSwap(V1,V2);
teta += PI/2;
if (teta> PI)
teta -= PI;
}
return true;
}
REAL TestMulMat(INT aNbTime, INT aNbVar)
{
ElTimer aChrono;
Im2D_REAL8 aMat(aNbVar,aNbVar,0.0);
Im1D_REAL8 aVec(aNbVar,0.0);
REAL8 ** aDM = aMat.data();
REAL8 * aDV = aVec.data();
for (INT aKT=0 ; aKT<aNbTime ; aKT++)
for (INT aKV=0 ; aKV<aNbVar ; aKV++)
{
for (INT aX=0 ; aX<aNbVar ; aX++)
for (INT aY=0 ; aY<aNbVar ; aY++)
aDM[aY][aX] += aDV[aY] * aDV[aX];
}
return aChrono.uval();
}
cv::Mat Deformation::DeformByMovingLeastSquares(const cv::Mat& inputImg,
const std::vector<int>& originIndex, const std::vector<int>& targetIndex)
{
int imgW = inputImg.cols;
int imgH = inputImg.rows;
cv::Size imgSize(imgW, imgH);
cv::Mat resImg(imgSize, CV_8UC3);
int markNum = originIndex.size() / 2;
std::vector<double> wList(markNum);
std::vector<MagicMath::Vector2> pHatList(markNum);
std::vector<MagicMath::Vector2> qHatList(markNum);
MagicMath::Vector2 pStar, qStar;
std::vector<MagicMath::Vector2> pList(markNum);
for (int mid = 0; mid < markNum; mid++)
{
pList.at(mid) = MagicMath::Vector2(originIndex.at(mid * 2), originIndex.at(mid * 2 + 1));
}
std::vector<MagicMath::Vector2> qList(markNum);
for (int mid = 0; mid < markNum; mid++)
{
qList.at(mid) = MagicMath::Vector2(targetIndex.at(mid * 2), targetIndex.at(mid * 2 + 1));
}
std::vector<std::vector<double> > aMatList(markNum);
std::vector<bool> visitFlag(imgW * imgH, 0);
for (int hid = 0; hid < imgH; hid++)
{
for (int wid = 0; wid < imgW; wid++)
{
MagicMath::Vector2 pos(wid, hid);
//calculate w
bool isMarkVertex = false;
int markedIndex = -1;
double wSum = 0;
for (int mid = 0; mid < markNum; mid++)
{
//double dTemp = (pos - pList.at(mid)).LengthSquared(); //variable
double dTemp = (pos - pList.at(mid)).Length();
//dTemp = pow(dTemp, 1.25);
if (dTemp < 1.0e-15)
{
isMarkVertex = true;
markedIndex = mid;
break;
}
dTemp = pow(dTemp, 1.25);
wList.at(mid) = 1.0 / dTemp;
wSum += wList.at(mid);
}
//
if (isMarkVertex)
{
const unsigned char* pPixel = inputImg.ptr(hid, wid);
int targetH = targetIndex.at(2 * markedIndex + 1);
int targetW = targetIndex.at(2 * markedIndex);
unsigned char* pResPixel = resImg.ptr(targetH, targetW);
pResPixel[0] = pPixel[0];
pResPixel[1] = pPixel[1];
pResPixel[2] = pPixel[2];
visitFlag.at(targetH * imgW + targetW) = 1;
}
else
{
//Calculate pStar qStar
pStar = MagicMath::Vector2(0.0, 0.0);
qStar = MagicMath::Vector2(0.0, 0.0);
for (int mid = 0; mid < markNum; mid++)
{
pStar += (pList.at(mid) * wList.at(mid));
qStar += (qList.at(mid) * wList.at(mid));
}
pStar /= wSum;
qStar /= wSum;
//Calculate pHat qHat
for (int mid = 0; mid < markNum; mid++)
{
pHatList.at(mid) = pList.at(mid) - pStar;
qHatList.at(mid) = qList.at(mid) - qStar;
}
//Calculate A
MagicMath::Vector2 col0 = pos - pStar;
MagicMath::Vector2 col1(col0[1], -col0[0]);
for (int mid = 0; mid < markNum; mid++)
{
std::vector<double> aMat(4);
MagicMath::Vector2 row1(pHatList.at(mid)[1], -pHatList.at(mid)[0]);
aMat.at(0) = pHatList.at(mid) * col0 * wList.at(mid);
aMat.at(1) = pHatList.at(mid) * col1 * wList.at(mid);
aMat.at(2) = row1 * col0 * wList.at(mid);
aMat.at(3) = row1 * col1 * wList.at(mid);
aMatList.at(mid) = aMat;
}
//Calculate fr(v)
MagicMath::Vector2 fVec(0, 0);
for (int mid = 0; mid < markNum; mid++)
{
fVec[0] += (qHatList.at(mid)[0] * aMatList.at(mid).at(0) + qHatList.at(mid)[1] * aMatList.at(mid).at(2));
fVec[1] += (qHatList.at(mid)[0] * aMatList.at(mid).at(1) + qHatList.at(mid)[1] * aMatList.at(mid).at(3));
}
//Calculate target position
fVec.Normalise();
MagicMath::Vector2 targetPos = fVec * ((pos - pStar).Length()) + qStar;
int targetW = targetPos[0];
int targetH = targetPos[1];
if (targetH >= 0 && targetH < imgH && targetW >= 0 && targetW < imgW)
{
const unsigned char* pPixel = inputImg.ptr(hid, wid);
unsigned char* pResPixel = resImg.ptr(targetH, targetW);
pResPixel[0] = pPixel[0];
pResPixel[1] = pPixel[1];
pResPixel[2] = pPixel[2];
visitFlag.at(targetH * imgW + targetW) = 1;
}
}
}
}
std::vector<int> unVisitVecH;
std::vector<int> unVisitVecW;
for (int hid = 0; hid < imgH; hid++)
{
int baseIndex = hid * imgW;
for (int wid = 0; wid < imgW; wid++)
{
if (!visitFlag.at(baseIndex + wid))
{
unVisitVecH.push_back(hid);
unVisitVecW.push_back(wid);
}
}
}
int minAcceptSize = 4;
int fillTime = 1;
while (unVisitVecH.size() > 0)
{
DebugLog << "unVisit number: " << unVisitVecH.size() << std::endl;
std::vector<int> unVisitVecHCopy = unVisitVecH;
std::vector<int> unVisitVecWCopy = unVisitVecW;
unVisitVecH.clear();
unVisitVecW.clear();
int unVisitSize = unVisitVecHCopy.size();
for (int uid = 0; uid < unVisitSize; uid++)
{
MagicMath::Vector3 avgColor(0, 0, 0);
int hid = unVisitVecHCopy.at(uid);
int wid = unVisitVecWCopy.at(uid);
int avgSize = 0;
if ((hid - 1) >= 0 && visitFlag.at((hid - 1) * imgW + wid))
{
unsigned char* pPixel = resImg.ptr(hid - 1, wid);
avgColor[0] += pPixel[0];
avgColor[1] += pPixel[1];
avgColor[2] += pPixel[2];
avgSize++;
}
if ((hid + 1) < imgH && visitFlag.at((hid + 1) * imgW + wid))
{
unsigned char* pPixel = resImg.ptr(hid + 1, wid);
avgColor[0] += pPixel[0];
avgColor[1] += pPixel[1];
avgColor[2] += pPixel[2];
avgSize++;
}
if ((wid - 1) >= 0 && visitFlag.at(hid * imgW + wid - 1))
{
unsigned char* pPixel = resImg.ptr(hid, wid - 1);
avgColor[0] += pPixel[0];
avgColor[1] += pPixel[1];
avgColor[2] += pPixel[2];
avgSize++;
}
if ((wid + 1) < imgW && visitFlag.at(hid * imgW + wid + 1))
{
unsigned char* pPixel = resImg.ptr(hid, wid + 1);
avgColor[0] += pPixel[0];
avgColor[1] += pPixel[1];
avgColor[2] += pPixel[2];
avgSize++;
}
if (avgSize >= minAcceptSize)
{
visitFlag.at(hid * imgW + wid) = 1;
avgColor /= avgSize;
unsigned char* pFillPixel = resImg.ptr(hid, wid);
pFillPixel[0] = avgColor[0];
pFillPixel[1] = avgColor[1];
pFillPixel[2] = avgColor[2];
}
else
{
unVisitVecH.push_back(hid);
unVisitVecW.push_back(wid);
}
}
if (fillTime == 4)
{
minAcceptSize--;
}
else if (fillTime == 6)
{
minAcceptSize--;
}
else if (fillTime == 8)
{
minAcceptSize--;
}
fillTime++;
}
//fill hole
/*for (int hid = 0; hid < imgH; hid++)
{
int baseIndex = hid * imgW;
for (int wid = 0; wid < imgW; wid++)
{
if (!visitFlag.at(baseIndex + wid))
{
double wSum = 0;
MagicMath::Vector3 avgColor(0, 0, 0);
for (int wRight = wid + 1; wRight < imgW; wRight++)
{
if (visitFlag.at(baseIndex + wRight))
{
double wTemp = 1.0 / (wRight - wid);
wSum += wTemp;
unsigned char* pPixel = resImg.ptr(hid, wRight);
avgColor[0] += wTemp * pPixel[0];
avgColor[1] += wTemp * pPixel[1];
avgColor[2] += wTemp * pPixel[2];
break;
}
}
for (int wLeft = wid - 1; wLeft >= 0; wLeft--)
{
if (visitFlag.at(baseIndex + wLeft))
{
double wTemp = 1.0 / (wid - wLeft);
wSum += wTemp;
unsigned char* pPixel = resImg.ptr(hid, wLeft);
avgColor[0] += wTemp * pPixel[0];
avgColor[1] += wTemp * pPixel[1];
avgColor[2] += wTemp * pPixel[2];
break;
}
}
for (int hUp = hid - 1; hUp >= 0; hUp--)
{
if (visitFlag.at(hUp * imgW + wid))
{
double wTemp = 1.0 / (hid - hUp);
unsigned char* pPixel = resImg.ptr(hUp, wid);
wSum += wTemp;
avgColor[0] += wTemp * pPixel[0];
avgColor[1] += wTemp * pPixel[1];
avgColor[2] += wTemp * pPixel[2];
break;
}
}
for (int hDown = hid + 1; hDown < imgH; hDown++)
{
if (visitFlag.at(hDown * imgW + wid))
{
double wTemp = 1.0 / (hDown - hid);
unsigned char* pPixel = resImg.ptr(hDown, wid);
wSum += wTemp;
avgColor[0] += wTemp * pPixel[0];
avgColor[1] += wTemp * pPixel[1];
avgColor[2] += wTemp * pPixel[2];
break;
}
}
if (wSum > 1.0e-15)
{
avgColor /= wSum;
}
unsigned char* pFillPixel = resImg.ptr(hid, wid);
pFillPixel[0] = avgColor[0];
pFillPixel[1] = avgColor[1];
pFillPixel[2] = avgColor[2];
}
}
}*/
return resImg;
}
/* The following takes in a polynomial stored in *c, and yields the roots of
this polynomial (*wr stores the real comp, and *wi stores the imag comp)
It forms a companion matrix, then uses the hqr algorithm
VV 19 June 2003
Input arguments:
c: coefficients of the polynomial
wr: real parts of the roots (output)
wi: imaginary parts of the roots (output)
*/
int LPFormantTracker::hqr_roots(dtype * c, dtype * wr, dtype * wi) {
#ifndef aMat
#define aMat(k, j) AHess[((j) - 1) * nLPC + (k) - 1]
#endif
int nn, m, l, k, j, i, its, mmin, nLPC_SQR = nLPC * nLPC;
/* dtype AHess[maxNLPC_squared]; */
dtype z, y, x, w, v, u, t, s, r, q, p, anorm = 0.0F;
/* generate companion matrix, starting off with an intialized version */
DSPF_dp_blk_move(Acompanion, AHess, nLPC_SQR);
for (i = 0; i < nLPC; i++)
AHess[nLPC * i] = -c[i + 1];
/* end of companion matrix generation */
/* the following performs the hqr algoritm */
/* NOTE: This was taken from Numerical Recipes in C, with modification
Specifically, the wr and wi arrays were assumed to number from 1..n
in the book. I modified calls to these arrays so that they number 0..n-1
Additionally, n (the order of the polynomial) is hardset to be 8
VV 19 June 2003 */
for (i = 1; i < nLPC + 1; i++)
for (j = imax(i - 1, 1); j < nLPC + 1; j++)
anorm += fabs(aMat(i, j));
/*anorm += fabs(AHess[(j - 1) * nLPC + i - 1]);*/
nn = nLPC;
t = 0.0;
while (nn >= 1) {
its=0;
do {
for (l = nn; l >= 2; l--) {
s = fabs(aMat(l - 1, l - 1)) + fabs(aMat(l, l));
/*s = fabs(AHess[(l - 2) * nLPC + l - 2]) + fabs(AHess[(l - 1) * nLPC + l - 1]);*/
if (s == 0.0) s = anorm;
if ((dtype) (fabs(aMat(l, l - 1)) + s) == s)
/*if ((dtype) (fabs(AHess[(l - 2) * nLPC + l - 1]) + s) == s)*/
break;
}
x=aMat(nn, nn);
/*x = AHess[(nn - 1) * nLPC + nn - 1];*/
if (l == nn) {
wr[(-1) + nn] = x + t;
wi[(-1) + nn--] = 0.0;
}
else {
y = aMat(nn - 1 ,nn - 1);
/*y = AHess[(nn - 2) * nLPC + nn - 2];*/
w = aMat(nn, nn - 1) * aMat(nn - 1, nn);
/*w = AHess[(nn - 2) * nLPC + nn - 1] * AHess[(nn - 2) * nLPC + nn - 1];*/
if (l == (nn-1)) {
p = 0.5 * (y - x);
q = p * p + w;
z=sqrt(fabs(q));
x += t;
if (q >= 0.0) {
z = p + mul_sign(z, p);
wr[(-1) + nn - 1] = wr[(-1) + nn] = x + z;
if (z) wr[(-1) + nn] = x - w / z;
wi[(-1) + nn - 1] = wi[(-1) + nn] = 0.0;
}
else {
wr[(-1) + nn - 1] = wr[(-1) + nn] = x + p;
wi[(-1) + nn - 1] = -(wi[(-1) + nn] = z);
}
nn -= 2;
}
else {
if (its == 10 || its == 20) {
t += x;
for (i = 1; i <= nn; i++)
aMat(i, i) -= x;
/*AHess[(i - 1) * nLPC + i - 1] -= x;*/
s = fabs(aMat(nn, nn - 1)) + fabs(aMat(nn - 1, nn - 2));
/*s = fabs(AHess[(nn - 2) * nLPC + nn - 1]) + fabs(AHess[(nn - 3) * nLPC + nn - 2]);*/
y = x = 0.75 * s;
w = -0.4375 * s * s;
}
++its;
for (m = nn - 2; m >= l; m--) {
z = aMat(m, m);
/*z = AHess[(m - 1) * nLPC + m - 1];*/
r = x - z;
s = y - z;
p = (r * s - w) / aMat(m + 1, m) + aMat(m, m + 1);
/*p = (r * s - w) / AHess[(m - 1) * nLPC + m] + AHess[m * nLPC + m - 1];*/
q = aMat(m + 1, m + 1) - z - r - s;
/*q = AHess[m * nLPC + m] - z - r - s;*/
r = aMat(m + 2, m + 1);
/*r = AHess[m * nLPC + m + 1];*/
s = fabs(p) + fabs(q) + fabs(r);
p /= s;
q /= s;
r /= s;
if (m == l) break;
u = fabs(aMat(m, m - 1)) * (fabs(q) + fabs(r));
/*u = fabs(AHess[(m - 2) * nLPC + m - 1]) * (fabs(q) + fabs(r));*/
v = fabs(p) * (fabs(aMat(m - 1, m - 1)) + fabs(z) + fabs(aMat(m + 1, m + 1)));
/*v = fabs(p) * (fabs(AHess[(m - 2) * nLPC + m - 2]) + fabs(z) + fabs(AHess[m * nLPC + m]));*/
if ((dtype) (u+v) == v) break;
}
for (i = m + 2; i <= nn; i++) {
aMat(i, i - 2) = 0.0F;
//AHess[(i - 3) * nLPC + i - 1] = 0.0F;
if (i != (m + 2))
aMat(i, i - 3) = 0.0F;
/*AHess[(i - 4) * nLPC + i - 1] = 0.0F;*/
}
for (k = m; k <= nn - 1; k++) {
if (k != m) {
p = aMat(k, k - 1);
/*p = AHess[(k - 2) * nLPC + k - 1];*/
q = aMat(k + 1,k - 1);
/*p = AHess[(k - 2) * nLPC + k];*/
r = 0.0F;
if (k != (nn - 1))
r = aMat(k + 2, k - 1);
/*r = AHess[(k - 2) * nLPC + k + 1];*/
if ((x = fabs(p) + fabs(q) + fabs(r)) != 0.0) {
p /= x;
q /= x;
r /= x;
}
}
if ((s = mul_sign(sqrt(p * p + q * q + r * r), p)) != 0.0) {
if (k == m) {
if (l != m)
aMat(k,k-1) = -aMat(k,k-1);
/*AHess[(k - 2) * nLPC + k - 1] *= -1.0;*/
}
else
aMat(k, k - 1) = -s * x;
/*AHess[(k - 2) * nLPC + k - 1] = -s * x;*/
p += s;
x=p/s;
y=q/s;
z=r/s;
q /= p;
r /= p;
for (j=k;j<=nn;j++) {
p=aMat(k,j)+q*aMat(k+1,j);
/*p = AHess[(j - 1) * nLPC + k - 1] + q * AHess[(j - 1) * nLPC + k];*/
if (k != (nn-1)) {
p += r * aMat(k + 2, j);
/*p += r * AHess[(j - 1) * nLPC + k + 1];*/
aMat(k + 2, j) -= p * z;
/* AHess[(j - 1) * nLPC + k + 1] -= p * z;*/
}
aMat(k+1,j) -= p*y;
/*AHess[(j - 1) * nLPC + k] -= p * y;*/
aMat(k,j) -= p*x;
/*AHess[(j - 1) * nLPC + k - 1] -= p * x;*/
}
mmin = nn<k+3 ? nn : k+3;
for (i=l;i<=mmin;i++) {
p = x * aMat(i, k) + y * aMat(i, k + 1);
/*p = x * AHess[(k - 1) * nLPC + i - 1] + y * AHess[k * nLPC + i - 1];*/
if (k != (nn-1)) {
p += z*aMat(i,k+2);
/*p += z * AHess[(k + 1) * nLPC + i - 1];*/
aMat(i,k+2) -= p*r;
/*AHess[(k + 1) * nLPC + i - 1] -= p * r;*/
}
aMat(i,k+1) -= p*q;
/*AHess[k * nLPC + i - 1] -= p * q;*/
aMat(i,k) -= p;
/*AHess[(k - 1) * nLPC + i - 1] -= p;*/
}
}
}
}
}
}
while (l < nn - 1);
}
if (nn == 0)
return 1;
else
return 0;
}
