void Beluga::SafeStop()
{
while(1000.0*(MT_getTimeSec() - m_dTimeOfLastSend) < m_dMinCommandPeriod_msec)
{
/* wait until we can send the command */
};
SendCommand(0, 0, 0);
std::vector<double> u(BELUGA_CONTROL_SIZE, 0.0);
SetControl(u);
}
/* 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();
}
int main(int argc, char** argv)
{
WX_CONSOLE_APP_INIT;
if(argc > 1)
{
g_ShowOnlyErrors = false;
}
int status = MT_TEST_SUCCESS;
FILE* f = fopen("output.txt", "w");
if(!f)
{
fprintf(stderr, "Could not open output file.\n");
return MT_TEST_ERROR;
}
MT_TEST_START("MT_Sequence");
double t0 = MT_getTimeSec();
double t;
MT_COMSequence* pSeq1 = new MT_COMSequence("stderr", f);
t = MT_getTimeSec() - t0;
try
{
if(!pSeq1->clearEvents())
{
cerr << " MT_COMSequence Error: Couldn't clear empty events.\n";
status = MT_TEST_ERROR;
}
}
catch (int e)
{
cerr << " MT_COMSequence Error: Exception " << e <<
" while trying to clear empty events.\n";
status = MT_TEST_ERROR;
}
unsigned char d[] = {65, 66, 67};
if(!pSeq1->sendData(d, 3))
{
cerr << " MT_COMSequence Error: Couldn't send data to port.\n";
status = MT_TEST_ERROR;
}
unsigned char d2[] = {'b', '@', 'T'};
unsigned char d3[] = {'w', 'o', 'o', 't'};
TRY_PUSH_EVENT(pSeq1, 0.1, d, 3, 0, &status);
TRY_PUSH_EVENT(pSeq1, 0.2, d2, 3, 1, &status);
TRY_PUSH_EVENT(pSeq1, 0.23, d2, 3, 2, &status);
TRY_PUSH_EVENT(pSeq1, 0.22, d3, 4, 3, &status);
if(!g_ShowOnlyErrors)
{
cout << pSeq1->getEventTableAsString();
}
pSeq1->setMinDelay(0.025);
if(!g_ShowOnlyErrors)
{
cout << pSeq1->getEventTableAsString();
}
pSeq1->goSequence();
while(pSeq1->getIsRunning()){wxMilliSleep(10);};
pSeq1->clearEvents();
/* make sure we send a newline to clean up the output window. */
unsigned char d4[] = {'\n','d','o','n','e','\n'};
if(!pSeq1->sendData(d4, 6))
{
cerr << " MT_COMSequence Error: Couldn't send data to port.\n";
status = MT_TEST_ERROR;
}
delete pSeq1;
fclose(f);
if(!g_ShowOnlyErrors)
{
cout << "\n============== file output =============\n" ;
MT_CatTextFile("output.txt");
/* Note we should probably check the output against what's
* expected. This is a skeleton of how to tokenize the
* output log file. */
/*
std::vector<std::string> lines =
MT_TextFileToStringVector("output.txt");
std::vector<std::string> second = MT_SplitString(lines[1],
std::string(" | "));
cout <<
MT_StringVectorToString(second);
*/
}
return status;
}
/* function to send a command to a Beluga via the COM port. Makes
* sure that the final two bytes of the command are an exclamation
* mark '!' followed by a line feed (10). Assumes that the line feed
* is not already present (TODO: should check for this) */
void Beluga::SendCommand(const char* command)
{
char cmd[BELUGA_MAX_COMMAND_LENGTH];
/* make sure the command ends with a ! */
if(*(command + strlen(command)-1) == '!')
{
sprintf(cmd, "%s%c", command, BELUGA_LINEFEED);
}
else
{
sprintf(cmd, "%s!%c", command, BELUGA_LINEFEED);
}
int f, v, t;
f = 0; v = 0; t = 0;
sscanf(command, "$%03d!%03d!%03d!", &v, &f, &t);
if(v > 100)
{
v = 100 - v;
}
if(f > 100)
{
f = 100 - f;
}
f = -f;
double half_speed = 0.5*((double) (BELUGA_SERVO_MIN + BELUGA_SERVO_MAX));
t = t - half_speed;
m_vdControls[BELUGA_CONTROL_FWD_SPEED] = f;
m_vdControls[BELUGA_CONTROL_STEERING] = t;
m_vdControls[BELUGA_CONTROL_VERT_SPEED] = v;
/* command rate throttling */
double t_now = MT_getTimeSec();
if(1000.0*(t_now - m_dTimeOfLastSend) > m_dMinCommandPeriod_msec)
{
m_dTimeOfLastSend = MT_getTimeSec();
/* uncomment to see output in console */
//printf("Sending %s\n", cmd);
m_COMPort.SendCommand(cmd);
int r;
r = m_COMPort.ReadData(m_ucDepthByte, BYTES_TO_READ);
/*
printf("Depth Bytes (%d): ", r);
for(unsigned int i = 0; i < BYTES_TO_READ; i++)
{
printf("%c ", m_ucDepthByte[i]);
}
printf("\n"); */
m_ucDepthByte[4] = 0;
int d;
sscanf((const char*) m_ucDepthByte, "%d", &d);
m_iDepthMeas = d;
m_dDepth = convertDepthMeasurement(d);
}
}
