/*
*
* Returns 1 if the worm is consistent with previous frame.
* Returns 0 if the worm's head and tail had been reversed from
* previous frame and fixes the problem.
* Returns -1 if the head and the tail do not match the previous frame at all
* Returns 2 if there is no previous worm information
*/
int PrevFrameImproveWormHeadTail(WormAnalysisData* Worm,
WormAnalysisParam* Params, WormGeom* PrevWorm) {
int DEBUG = 0;
if (PrevWorm->Head.x == NULL || PrevWorm->Head.y == NULL
|| PrevWorm->Tail.y == NULL || PrevWorm->Tail.x == NULL
|| PrevWorm->Perimeter == NULL) {
/** No previous worm to provide information **/
if (DEBUG)
printf("No previous worm to provide information.\n");
return 2;
}
/** Is the Worm's Head and Tail Close to the Previous Frames **/
CvPoint CurrHead=cvPoint(Worm->Head->x,Worm->Head->y);
CvPoint CurrTail=cvPoint(Worm->Tail->x,Worm->Tail->y);
int SqDeltaHead = sqDist(CurrHead, PrevWorm->Head);
int SqDeltaTail = sqDist(CurrTail, PrevWorm->Tail);
if (DEBUG) printf("=======================\n");
if (DEBUG) printf("CurrHead=(%d,%d),CurrTail=(%d,%d)\n",Worm->Head->x,Worm->Head->y,Worm->Tail->x,Worm->Tail->y);
if (DEBUG) printf("PrevHead=(%d,%d),PrevTail=(%d,%d)\n",PrevWorm->Head.x,PrevWorm->Head.y,PrevWorm->Tail.x,PrevWorm->Tail.y);
if (DEBUG) printf("SqDeltaTail=%d,SqDeltaHead=%d\n",SqDeltaTail,SqDeltaHead);
int rsquared=(Params->MaxLocationChange) * (Params->MaxLocationChange);
if ((SqDeltaHead > rsquared) || (SqDeltaTail > rsquared)) {
/** The previous head/tail locations aren't close.. **/
/** Is the inverse close? **/
int SqDeltaHeadInv = sqDist(CurrHead, PrevWorm->Tail);
int SqDeltaTailInv = sqDist(CurrTail, PrevWorm->Head);
if (DEBUG) printf("SqDeltaTailInv=%d,SqDeltaHeadInv=%d\n",SqDeltaTailInv,SqDeltaTailInv);
if ( (SqDeltaHeadInv < rsquared) || (SqDeltaTailInv < rsquared )){
/** The inverse is close, so let's reverse the Head Tail**/
ReverseWormHeadTail(Worm);
if (DEBUG) printf("ReversedWormHeadTail\n");
return 0;
} else {
/** The Head and Tail is screwed up and its not related to simply inverted **/
if (DEBUG) printf(
"Head moved by a squared distance of %d pixels\n Tail moved by a squared distance of %d pixels\n",
SqDeltaHead, SqDeltaTail);
if (DEBUG)
printf("Head and Tail Screwed Up");
return -1;
}
}
if (DEBUG)
printf("All good.\n");
return 1; /** The Head and Tail are within the required distance **/
}
GMMDesc initutil::adaptiveSphericalGMM(commonutil::DataSet const& input, idx_type k, std::mt19937& gen)
{
idx_type d = input.points.rows();
idx_type n = input.points.cols();
initutil::check(k, d, n);
Matrix samples(d,k+1); // two samples for the first gaussian, one for each of the others
Vector sqDist(k); // minimum distance to nearest sample (for each sample)
GMMDesc desc;
for (idx_type i=0; i<k+1; ++i)
{
if (i<2)
// draw the first two points uniformly
samples.col(i) = input.points.col(commonutil::randomIndex(input.weights, gen));
else
{
// draw next point w.r.t. current mixture
Vector densities = gmmutil::adaptiveDensities(input, desc, 1);
samples.col(i) = input.points.col(commonutil::randomIndex(densities, gen));
}
// std::cout << "sampled points = " << samples.col(i).transpose() << std::endl;
if (i>0)
{
desc.weights = Vector::Constant(i, 1.0/i);
desc.means = samples.block(0,1,d,i);
desc.covariances = std::vector<Matrix>(i, (1.0/(2*d))*Matrix::Identity(d,d));
if (i==1) // after uniformly drawing two samples create the first model with one component
{
sqDist[0] = (samples.col(0)-samples.col(1)).squaredNorm();
if (sqDist[0]<=0)
sqDist[0] = 1;
}
else if (i==2)
{
sqDist[0] = (samples.col(1)-samples.col(2)).squaredNorm();
if (sqDist[0]<=0)
sqDist[0] = 1;
sqDist[1] = sqDist[0];
}
else if (i>2)
{
Vector mins = (samples.block(0,1,d,i-1).colwise()-samples.col(i)).colwise().squaredNorm();
// set sqDist to the minimum non-zero distance to one of the other samples
// this is due to the possibility of sampling the same point multiple times
sqDist[i-1] = 0;
for (idx_type j=0; j<i-1; ++j)
if (mins[j]>0 && (sqDist[i-1]==0 || mins[j]<sqDist[i-1]))
sqDist[i-1] = mins[j];
if (sqDist[i-1]<=0)
sqDist[i-1] = 1; // prevent zero covariances in pathological cases
for (idx_type j=0; j<i-1; ++j)
if (mins[j]>0 && mins[j]<sqDist[j])
sqDist[j] = mins[j];
}
for (idx_type j=0; j<i; ++j)
desc.covariances[j] *= sqDist[j];
}
}
return desc;
}