int getPredecessors(Solver& S, int attr_number, int num_var, int curr_depth) {
vec<Lit> lits;
int nBasins = 0;
int i = num_var;
while (i--){
lits.push((global_var.Attractors[attr_number].states[0].c_str()[i]=='1')? Lit(i) : ~Lit(i));
S.addClause(lits);
lits.clear();
}
i = num_var;
while (i--){
lits.push((global_var.Attractors[attr_number].states[0].c_str()[i]=='1')? ~Lit(i+num_var) : Lit(i+num_var));
}
S.addClause(lits);
lits.clear();
while (1){
if (!S.solve()) break;
constrain_state(curr_depth-1, S.model, curr_depth-1, S, num_var);
nBasins++;
}
return nBasins;
}
int
main(int argc, char *argv[])
{
int i,j,n;
Solver S;
unsigned atractor_count=0;
unsigned atractor_stats_count=0;
std::vector<Lit> lits;
char command[100];
float avg_length = 0;
std::vector< std::vector<Lit> > orig_clauses;
global_var.orig_clauses = &orig_clauses;
ReadNET(argv[1], S); //ckhong: read networks and then make transition relations with depth 2 by using update functions
//PRINT(orig_clauses.size());
vec<Clause*>* orig_clauses_pr = S.Clauses();
lits.clear();
i=(*orig_clauses_pr).size();
//PRINT(i);
while(i--){
int j=(*((*orig_clauses_pr)[i])).size(); //ckhong: j has the number of literals in each clause
while(j--){
lits.push_back((*((*orig_clauses_pr)[i]))[j]);
}
orig_clauses.push_back( lits );
lits.clear();
}
/*Handle assignments of variables made in solver*/
/*ckhong: what is this for ?*/
/*ckhong: initial state value setting ?*/
i=number_of_var;
while(i--){
if(S.value(i)!= l_Undef){
lits.push_back((S.value(i)==l_True)? Lit(i) : ~Lit(i));
orig_clauses.push_back( lits );
lits.clear();
}
}
//PRINT(orig_clauses.size());
global_var.S = &S;
global_var.number_of_var = number_of_var;
global_var.depth=2;
if(number_of_var<100){ //ckhong: make n-length formula
construct_depth(number_of_var);
}else{
construct_depth(100);
}
//puts("Start searching...");
while(1){
if (!S.solve()) break;
/*ckhong: found valid path*/
for( i=1; i<global_var.depth; i++ ){
if(compare_states(0,i,number_of_var,S.model )){
atractor_count++;
atractor_stats_count+=i;
//PRINT(atractor_stats_count);
//constrain all states of atractor sequence on 0 index variable
//char temp_attr[i*number_of_var];
char tState[number_of_var];
Attractor tAttr;
tAttr.length = i;
for( j = 0; j < i; j++ ){
constrain_state(j, S.model, 0, S, number_of_var );
#ifdef PRINT_STATE
/*ckhong: attractor state print out*/
int a_tmp=j*number_of_var;
int b_tmp=0;
int counter=number_of_var;
while(counter--){
if(S.model[a_tmp] != l_Undef){
//printf( "%s", (S.model[a_tmp]==l_True)?"1":"0");
//sprintf(temp_attr+a_tmp, "%s", (S.model[a_tmp]==l_True)?"1":"0");
sprintf(tState+b_tmp, "%s", (S.model[a_tmp]==l_True)?"1":"0");
}else{
//printf("-");
//sprintf(temp_attr+a_tmp, "-");
sprintf(tState+b_tmp, "-");
}
a_tmp++;
b_tmp++;
}
tAttr.states.push_back(tState);
//printf("\n");
#endif
}
//results.push_back(temp_attr);
//results.push_back("\n");
global_var.Attractors.push_back(tAttr);
//printf("Attractor %d is of length %d\n\n",atractor_count,i);
//avg_length = avg_length + i;
break;
}
}
if(global_var.depth == i){
construct_depth( global_var.depth << 1 ); //ckhong: depth = depth * 2
printf("Depth of unfolding transition relation is increased to %d\n",global_var.depth);
}
}
printf("Total number of attractors is %d\n",atractor_count);
printf("Average length of attractors is %0.2f\n",avg_length/(float)atractor_count);
for (int i=0; i<global_var.Attractors.size(); i++) {
std::cout << "Attractor " << i << "\n";
for (int j=0; j<global_var.Attractors[i].length; j++)
std::cout << global_var.Attractors[i].states[j] << " " ;
std::cout << "\n";
}
int MAX_DEPTH = global_var.depth;
/* ####### Basin Analysis ####### */
int total_basin_size = 1;
int curr_depth = 0;
int attr_num = 0;
for (curr_depth=2; curr_depth<MAX_DEPTH; curr_depth++) {
Solver S_;
orig_clauses.clear();
lits.clear();
ReadNET(argv[1], S_);
//PRINT(S_.nClauses());
orig_clauses_pr = S_.Clauses();
i=(*orig_clauses_pr).size();
//PRINT(i);
while(i--){
int j=(*((*orig_clauses_pr)[i])).size();
while(j--){
lits.push_back((*((*orig_clauses_pr)[i]))[j]);
}
orig_clauses.push_back( lits );
lits.clear();
}
//PRINT(orig_clauses.size());
int count = 0;
i=number_of_var;
while(i--){
if(S_.value(i)!= l_Undef){
count++;
lits.push_back((S_.value(i)==l_True)? Lit(i) : ~Lit(i));
orig_clauses.push_back( lits );
lits.clear();
}
}
global_var.S = &S_;
global_var.depth = 2;
global_var.number_of_var = number_of_var;
//global_var.orig_clauses = &orig_clauses;
construct_depth(curr_depth);
int tSize = getPredecessors(S_, attr_num, number_of_var, curr_depth);
//printf("level %d: %d\n", curr_depth, tSize);
total_basin_size = total_basin_size + tSize;
if (tSize == 0) {
break;
}
}
printf("Basin size for attractor %d: %d\n", attr_num, total_basin_size);
return 0;
}
/* Main tracking function - gets called by MT_TrackerFrameBase every
* time step when the application is not paused. */
void DanceTracker::doTracking(IplImage* frame)
{
/* time-keeping, if necessary
* NOTE this is not necessary for keeping track of frame rate */
static double t_prev = MT_getTimeSec();
double t_now = MT_getTimeSec();
m_dDt = t_now - t_prev;
t_prev = t_now;
/* keeping track of the frame number, if necessary */
m_iFrameCounter++;
/* This checks every time step to see if the UKF parameters have
changed and modifies the UKF structures accordingly. This will
also get called the first time through b/c the "Prev" values get
set to zero initially. There may be a more efficient way to do
this, but because the values need to be embedded into the CvMat
objects I'm not sure how else to do it. */
if(
m_dSigmaPosition != m_dPrevSigmaPosition ||
m_dSigmaSpeed != m_dPrevSigmaSpeed ||
m_dSigmaPositionMeas != m_dPrevSigmaPositionMeas
)
{
/* these are the diagonal entries of the "Q" matrix, which
represents the variances of the process noise. They're
modeled here as being independent and uncorrellated. */
cvSetReal2D(m_pQ, 0, 0, m_dSigmaPosition*m_dSigmaPosition);
cvSetReal2D(m_pQ, 1, 1, m_dSigmaPosition*m_dSigmaPosition);
cvSetReal2D(m_pQ, 2, 2, m_dSigmaHeading*m_dSigmaSpeed);
cvSetReal2D(m_pQ, 3, 3, m_dSigmaSpeed*m_dSigmaSpeed);
/* these are the diagonal entries of the "R matrix, also
assumed to be uncorrellated. */
cvSetReal2D(m_pR, 0, 0, m_dSigmaPositionMeas*m_dSigmaPositionMeas);
cvSetReal2D(m_pR, 1, 1, m_dSigmaPositionMeas*m_dSigmaPositionMeas);
/* this step actually copies the Q and R matrices to the UKF
and makes sure that it's internals are properly initialized -
it's set up to handle the fact that the sizes of these
matrices could have changed. */
for(unsigned int i = 0; i < m_iNObj; i++)
{
MT_UKFCopyQR(m_vpUKF[i], m_pQ, m_pR);
}
}
HSVSplit(frame);
cvThreshold(m_pHFrame, m_pThreshFrame, m_iHThresh_Low, 255, CV_THRESH_BINARY);
cvThreshold(m_pHFrame, m_pTempFrame1, m_iHThresh_High, 255, CV_THRESH_BINARY_INV);
cvAnd(m_pThreshFrame, m_pTempFrame1, m_pThreshFrame);
cvThreshold(m_pSFrame, m_pTempFrame1, m_iSThresh_Low, 255, CV_THRESH_BINARY);
cvThreshold(m_pSFrame, m_pTempFrame2, m_iSThresh_High, 255, CV_THRESH_BINARY_INV);
cvAnd(m_pTempFrame1, m_pTempFrame2, m_pTempFrame1);
cvAnd(m_pThreshFrame, m_pTempFrame1, m_pThreshFrame);
cvThreshold(m_pVFrame, m_pTempFrame1, m_iVThresh_Low, 255, CV_THRESH_BINARY);
cvThreshold(m_pVFrame, m_pTempFrame2, m_iVThresh_High, 255, CV_THRESH_BINARY_INV);
cvAnd(m_pTempFrame1, m_pTempFrame2, m_pTempFrame1);
cvAnd(m_pThreshFrame, m_pTempFrame1, m_pTempFrame1);
cvSub(BG_frame, m_pVFrame, m_pTempFrame2);
cvThreshold(m_pTempFrame2, m_pTempFrame2, m_iBGThresh, 255, CV_THRESH_BINARY);
cvOr(m_pTempFrame1, m_pTempFrame2, m_pThreshFrame);
cvSmooth(m_pThreshFrame, m_pThreshFrame, CV_MEDIAN, 3);
if(ROI_frame)
{
cvAnd(m_pThreshFrame, ROI_frame, m_pThreshFrame);
}
/* std::vector<YABlob> yblobs = m_YABlobber.FindBlobs(m_pThreshFrame,
5,
m_iBlobAreaThreshLow,
NO_MAX,
m_iBlobAreaThreshHigh);
int ny = yblobs.size();
m_vdBlobs_X.resize(ny);
m_vdBlobs_Y.resize(ny);
m_vdBlobs_Orientation.resize(ny);
for(unsigned int i = 0; i < yblobs.size(); i++)
{
m_vdBlobs_X[i] = yblobs[i].COMx;
m_vdBlobs_Y[i] = yblobs[i].COMy;
m_vdBlobs_Orientation[i] = 0;
}*/
m_vbNoMeasurement.assign(m_iNObj, false);
Dance_Segmenter segmenter(this);
segmenter.setDebugFile(stdout);
segmenter.m_iMinBlobPerimeter = 1;
segmenter.m_iMinBlobArea = m_iBlobAreaThreshLow;
segmenter.m_iMaxBlobArea = m_iBlobAreaThreshHigh;
segmenter.m_dOverlapFactor = m_dOverlapFactor;
if(m_iFrameCounter <= 1)
{
std::ifstream in_file;
in_file.open("initials.dat");
double x, y;
m_vBlobs.resize(0);
m_vBlobs = MT_readDSGYABlobsFromFile("initials.dat");
m_vInitBlobs.resize(0);
m_viAssignments.resize(0);
/* m_vBlobs = segmenter.segmentFirstFrame(m_pThreshFrame,
m_iNObj); */
}
else
{
MT_writeDSGYABlobsToFile(m_vBlobs, "blobs-in.dat");
MT_writeDSGYABlobsToFile(m_vPredictedBlobs, "predicted-in.dat");
bool use_prediction = true;
m_vBlobs = segmenter.doSegmentation(m_pThreshFrame,
use_prediction ? m_vPredictedBlobs : m_vBlobs);
m_viAssignments = segmenter.getAssignmentVector(&m_iAssignmentRows, &m_iAssignmentCols);
m_vInitBlobs = segmenter.getInitialBlobs();
}
/* prediction is done below - this makes sure the predicted blobs
are OK no matter what */
m_vPredictedBlobs = m_vBlobs;
unsigned int sc = 0;
bool same_frame = false;
for(unsigned int i = 0; i < m_vBlobs.size(); i++)
{
if(m_vdBlobs_X[i] == m_vBlobs[i].m_dXCenter)
{
sc++;
}
m_vdBlobs_X[i] = m_vBlobs[i].m_dXCenter;
m_vdBlobs_Y[i] = m_vBlobs[i].m_dYCenter;
m_vdBlobs_Orientation[i] = m_vBlobs[i].m_dOrientation;
}
same_frame = (sc >= m_iNObj - 2);
if(same_frame)
{
return;
}
/* Tracking / UKF / State Estimation
*
* Now that we've got the mapping of which measurement goes with
* which object, we need to feed the measurements into the UKF in
* order to obtain a state estimate.
*
* This is a loop over each object we're tracking.
*/
for(unsigned int i = 0; i< m_iNObj; i++)
{
/* we could throw out a measurement and use the blob
state as an estimate for various reasons. On the first
frame we want to set the initial state, so we flag the
measurement as invalid */
bool invalid_meas = m_vbNoMeasurement[i];
bool need_state = m_iFrameCounter == 1;
/* if any state is NaN, reset the UKF
* This shouldn't happen anymore, but it's a decent safety
* check. It could probably be omitted if we want to
* optimize for speed... */
if(m_iFrameCounter > 1 &&
(!CvMatIsOk(m_vpUKF[i]->x) ||
!CvMatIsOk(m_vpUKF[i]->P)))
{
MT_UKFFree(&(m_vpUKF[i]));
m_vpUKF[i] = MT_UKFInit(4, 2, 0.1);
MT_UKFCopyQR(m_vpUKF[i], m_pQ, m_pR);
need_state = true;
}
if(need_state)
{
cvSetReal2D(m_px0, 0, 0, m_vdBlobs_X[i]);
cvSetReal2D(m_px0, 1, 0, m_vdBlobs_Y[i]);
cvSetReal2D(m_px0, 2, 0, 0);
cvSetReal2D(m_px0, 3, 0, 0);
MT_UKFSetState(m_vpUKF[i], m_px0);
}
/* if we're going to accept this measurement */
if(!invalid_meas)
{
/* UKF prediction step, note we use function pointers to
the fish_dynamics and fish_measurement functions defined
above. The final parameter would be for the control input
vector, which we don't use here so we pass a NULL pointer */
MT_UKFPredict(m_vpUKF[i],
&dance_dynamics,
&dance_measurement,
NULL);
/* finally, set the measurement vector z */
cvSetReal2D(m_pz, 0, 0, m_vdBlobs_X[i]);
cvSetReal2D(m_pz, 1, 0, m_vdBlobs_Y[i]);
MT_UKFSetMeasurement(m_vpUKF[i], m_pz);
/* then do the UKF correction step, which accounts for the
measurement */
MT_UKFCorrect(m_vpUKF[i]);
}
else
{
/* use the predicted state */
CvMat* xp = m_vpUKF[i]->x1;
MT_UKFSetState(m_vpUKF[i], xp);
}
/* then constrain the state if necessary - see function
* definition above */
constrain_state(m_vpUKF[i]->x, m_vpUKF[i]->x1, frame);
/* grab the state estimate and store it in variables that will
make it convenient to save it to a file. */
CvMat* x = m_vpUKF[i]->x;
m_vdTracked_X[i] = cvGetReal2D(x, 0, 0);
m_vdTracked_Y[i] = cvGetReal2D(x, 1, 0);
m_vdTracked_Vx[i] = cvGetReal2D(x, 2, 0);
m_vdTracked_Vy[i] = cvGetReal2D(x, 3, 0);
/* take the tracked positions as the blob centers */
m_vBlobs[i].m_dXCenter = m_vdTracked_X[i];
m_vBlobs[i].m_dYCenter = m_vdTracked_Y[i];
/* predict blob locations */
CvMat* xp = m_vpUKF[i]->x1;
m_vPredictedBlobs[i].m_dXCenter = cvGetReal2D(xp, 0, 0);
m_vPredictedBlobs[i].m_dYCenter = cvGetReal2D(xp, 1, 0);
/* If we wanted the predicted state, this would be how to get
it */
/* CvMat* xp = m_vpUKF[i]->x1; */
}
MT_writeDSGYABlobsToFile(m_vBlobs, "blobs-out.dat");
MT_writeDSGYABlobsToFile(m_vPredictedBlobs, "predicted-out.dat");
/* write data to file */
writeData();
}
