Vector4 Transform(const Matrix4 &m, const Vector4 &v)
{
// ************************************************************
// The formula to transform the vector/point is as follows:
// ------------------------------------------------------------
// p' = xX' + yY' + zZ' + T
// OR
// p'.x = (x * X'.x) + (x * Y'.x) + (x * Z'.x) + T.x
// p'.y = (y * X'.y) + (y * Y'.y) + (y * Z'.y) + T.y
// p'.z = (z * X'.z) + (z * Y'.z) + (z * Z'.z) + T.z
// ------------------------------------------------------------
// Where:
// p' is the transformed point/vector.
// x is the x component/scalar of the vector.
// X' is the x axis component of the Matrix4.
// y is the y component/scalar of the vector.
// Y' is the y axis component of the Matrix4.
// z is the z component/scalar of the vector.
// Z' is the z axis component of the Matrix4.
// T is the translation component of the Matrix4 (W axis).
// ************************************************************
Vector4 result;
Vector4 xAx(0,0,0,0);
Vector4 yAx(0,0,0,0);
Vector4 zAx(0,0,0,0);
Vector4 wAx(0,0,0,0);
// xX' component of formula
xAx.x = v.x * m.xX;
xAx.y = v.x * m.xY;
xAx.z = v.x * m.xZ;
xAx.w = v.x * m.xW;
// yY' component of formula
yAx.x = v.y * m.yX;
yAx.y = v.y * m.yY;
yAx.z = v.y * m.yZ;
yAx.w = v.y * m.yW;
// zZ' component of formula
zAx.x = v.z * m.zX;
zAx.y = v.z * m.zY;
zAx.z = v.z * m.zZ;
zAx.w = v.z * m.zW;
//
wAx.x = v.w * m.wX;
wAx.y = v.w * m.wY;
wAx.z = v.w * m.wZ;
wAx.w = v.w * m.wW;
result.x = xAx.x + yAx.x + zAx.x + wAx.x;
result.y = xAx.y + yAx.y + zAx.y + wAx.y;
result.z = xAx.z + yAx.z + zAx.z + wAx.z;
result.w = v.w;
return result;
}
void ClassicDescSearch( // search along whisker for best profile match
double& x, // io: (in: old posn of landmark, out: new posn)
double& y, // io:
const Image& img, // in: the image scaled to this pyramid level
const Shape& inshape, // in: current posn of landmarks (for whisker directions)
int ipoint, // in: index of the current landmark
const MAT& meanprof, // in: mean of the training profiles for this point
const MAT& covi) // in: inverse of the covar of the training profiles
{
const int proflen = NSIZE(meanprof);
CV_Assert(proflen % 2 == 1); // proflen must be odd in this implementation
// fullprof is the 1D profile along the whisker including the extra
// elements to allow search +-CLASSIC_MAX_OFFSET pixels away from
// the current position of the landmark.
// We precalculate the fullprof for efficiency in the for loop below.
const int fullproflen = proflen + 2 * CLASSIC_MAX_OFFSET;
CV_Assert(fullproflen % 2 == 1); // fullprof length must be odd
const VEC fullprof(FullProf(img, inshape, ipoint, fullproflen));
// move along the whisker looking for the best match
int bestoffset = 0;
double mindist = FLT_MAX;
for (int offset = -CLASSIC_MAX_OFFSET;
offset <= CLASSIC_MAX_OFFSET;
offset += CLASSIC_SEARCH_RESOL)
{
// Get the profile distance. That is, get the image profile at the given
// offset along the whisker, and return the Mahalanobis distance between
// it and the model mean profile. Low distance means good fit.
const VEC prof(SubProf(offset, proflen, fullprof));
// The following code is equivalent to
// dist = (prof - meanprof).t() * covi * (prof - meanprof)
// but is optimized for speed.
prof -= meanprof; // for efficiency, use "-=" not "=" with "-"
const double dist = xAx(prof, covi);
if (dist < mindist)
{
mindist = dist;
bestoffset = offset;
}
}
// change x,y to the best position along the whisker
double xstep, ystep;
WhiskerStep(xstep, ystep, inshape, ipoint);
x = inshape(ipoint, IX) + (bestoffset * xstep);
y = inshape(ipoint, IY) + (bestoffset * ystep);
}
Vector3 Transform(const Matrix4 &m, const Vector3 &v)
{
// ************************************************************
// The formula to transform the vector/point is as follows:
// ------------------------------------------------------------
// p' = xX' + yY' + zZ' + T
// OR
// p'.x = (x * X'.x) + (x * Y'.x) + (x * Z'.x) + T.x
// p'.y = (y * X'.y) + (y * Y'.y) + (y * Z'.y) + T.y
// p'.z = (z * X'.z) + (z * Y'.z) + (z * Z'.z) + T.z
// ------------------------------------------------------------
// Where:
// p' is the transformed point/vector.
// x is the x component/scalar of the vector.
// X' is the x axis component of the Matrix4.
// y is the y component/scalar of the vector.
// Y' is the y axis component of the Matrix4.
// z is the z component/scalar of the vector.
// Z' is the z axis component of the Matrix4.
// T is the translation component of the Matrix4 (W axis).
// ************************************************************
Vector3 result;
Vector3 xAx(0,0,0);
Vector3 yAx(0,0,0);
Vector3 zAx(0,0,0);
// xX' component of formula
xAx.x = v.x * m.xX;
xAx.y = v.x * m.xY;
xAx.z = v.x * m.xZ;
// yY' component of formula
yAx.x = v.y * m.yX;
yAx.y = v.y * m.yY;
yAx.z = v.y * m.yZ;
// zZ' component of formula
zAx.x = v.z * m.zX;
zAx.y = v.z * m.zY;
zAx.z = v.z * m.zZ;
// Using the above equation, we now add the components (including translation components).
// This isnt a true transform as I'm cheating by simply adding on the translation component.
result.x = xAx.x + yAx.x + zAx.x + m.wX;
result.y = xAx.y + yAx.y + zAx.y + m.wY;
result.z = xAx.z + yAx.z + zAx.z + m.wZ;
return result;
}
