/**
* create and score a trajectory given the current pose of the robot and selected velocities
*/
void CalibrateAction::generateTrajectory(
double x, double y, double theta,
double vx, double vy, double vtheta, double sim_time_, double vx_samp, double vy_samp, double vtheta_samp,
double acc_x, double acc_y, double acc_theta, Trajectory& traj) {
double x_i = x;
double y_i = y;
double theta_i = theta;
double vx_i, vy_i, vtheta_i;
vx_i = vx;
vy_i = vy;
vtheta_i = vtheta;
//compute the number of steps we must take along this trajectory to be "safe"
int num_steps = int(sim_time_ / traj_sim_granularity_ + 0.5);
//we at least want to take one step... even if we won't move, we want to score our current position
if(num_steps == 0) {
num_steps = 1;
}
double dt = sim_time_ / num_steps;
//create a potential trajectory
traj.resetPoints();
traj.xv_ = vx_i;
traj.yv_ = vy_i;
traj.thetav_ = vtheta_i;
for(int i = 0; i < num_steps; ++i){
//the point is legal... add it to the trajectory
traj.addPoint(x_i, y_i, theta_i);
//calculate velocities
vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);
//calculate positions
x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);
} // end for i < numsteps
}
void Main::doWork()
{
Trajectory trajectory;
IplImage *img = imAcqGetImg(imAcq);
Mat grey(img->height, img->width, CV_8UC1);
cvtColor(cvarrToMat(img), grey, CV_BGR2GRAY);
tld->detectorCascade->imgWidth = grey.cols;
tld->detectorCascade->imgHeight = grey.rows;
tld->detectorCascade->imgWidthStep = grey.step;
if(showTrajectory)
{
trajectory.init(trajectoryLength);
}
if(selectManually)
{
CvRect box;
if(getBBFromUser(img, box, gui) == PROGRAM_EXIT)
{
return;
}
if(initialBB == NULL)
{
initialBB = new int[4];
}
initialBB[0] = box.x;
initialBB[1] = box.y;
initialBB[2] = box.width;
initialBB[3] = box.height;
}
FILE *resultsFile = NULL;
if(printResults != NULL)
{
resultsFile = fopen(printResults, "w");
if(!resultsFile)
{
fprintf(stderr, "Error: Unable to create results-file \"%s\"\n", printResults);
exit(-1);
}
}
bool reuseFrameOnce = false;
bool skipProcessingOnce = false;
if(loadModel && modelPath != NULL)
{
tld->readFromFile(modelPath);
reuseFrameOnce = true;
}
else if(initialBB != NULL)
{
Rect bb = tldArrayToRect(initialBB);
printf("Starting at %d %d %d %d\n", bb.x, bb.y, bb.width, bb.height);
tld->selectObject(grey, &bb);
skipProcessingOnce = true;
reuseFrameOnce = true;
}
while(imAcqHasMoreFrames(imAcq))
{
double tic = cvGetTickCount();
if(!reuseFrameOnce)
{
cvReleaseImage(&img);
img = imAcqGetImg(imAcq);
if(img == NULL)
{
printf("current image is NULL, assuming end of input.\n");
break;
}
cvtColor(cvarrToMat(img), grey, CV_BGR2GRAY);
}
if(!skipProcessingOnce)
{
tld->processImage(cvarrToMat(img));
if (tld->currBB)
{
CvPoint center = cvPoint(tld->currBB->x + tld->currBB->width / 2, tld->currBB->y + tld->currBB->height / 2);
Point statePt = tld->particlefilter->ParticleFileterProcess(center.x, center.y);
cout << "center" << center.x << "\t" << center.y << endl;
cout << "state" << statePt.x+20 << "\t" << statePt.y+40 << endl;
getchar();
}
}
else
{
skipProcessingOnce = false;
}
if(printResults != NULL)
{
if(tld->currBB != NULL)
{
fprintf(resultsFile, "%d %.2d %.2d %.2d %.2d %f\n", imAcq->currentFrame - 1, tld->currBB->x, tld->currBB->y, tld->currBB->width, tld->currBB->height, tld->currConf);
}
else
{
fprintf(resultsFile, "%d NaN NaN NaN NaN NaN\n", imAcq->currentFrame - 1);
}
}
double toc = (cvGetTickCount() - tic) / cvGetTickFrequency();
toc = toc / 1000000;
float fps = 1 / toc;
int confident = (tld->currConf >= threshold) ? 1 : 0;
if(showOutput || saveDir != NULL)
{
char string[128];
char learningString[10] = "";
if(tld->learning)
{
strcpy(learningString, "Learning");
}
sprintf(string, "#%d,Posterior %.2f; fps: %.2f, #numwindows:%d, %s", imAcq->currentFrame - 1, tld->currConf, fps, tld->detectorCascade->numWindows, learningString);
CvScalar yellow = CV_RGB(255, 255, 0);
CvScalar blue = CV_RGB(0, 0, 255);
CvScalar black = CV_RGB(0, 0, 0);
CvScalar white = CV_RGB(255, 255, 255);
CvScalar red = CV_RGB(255, 0, 0);
CvScalar green = CV_RGB(0, 255, 0);
if(tld->currBB != NULL)
{
CvScalar rectangleColor = (confident) ? blue : yellow;
cvRectangle(img, tld->currBB->tl(), tld->currBB->br(), rectangleColor, 8, 8, 0);
cvCircle(img, tld->statePt, 5,red,4);
CvPoint center = cvPoint(tld->currBB->x + tld->currBB->width / 2, tld->currBB->y + tld->currBB->height / 2);
cvCircle(img, center, 5, green, 4);
if(showTrajectory)
{
CvPoint center = cvPoint(tld->currBB->x+tld->currBB->width/2, tld->currBB->y+tld->currBB->height/2);
cvLine(img, cvPoint(center.x-2, center.y-2), cvPoint(center.x+2, center.y+2), rectangleColor, 2);
cvLine(img, cvPoint(center.x-2, center.y+2), cvPoint(center.x+2, center.y-2), rectangleColor, 2);
trajectory.addPoint(center, rectangleColor);
}
}
else if(showTrajectory)
{
trajectory.addPoint(cvPoint(-1, -1), cvScalar(-1, -1, -1));
}
if(showTrajectory)
{
trajectory.drawTrajectory(img);
}
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, .5, .5, 0, 1, 8);
cvRectangle(img, cvPoint(0, 0), cvPoint(img->width, 50), black, CV_FILLED, 8, 0);
cvPutText(img, string, cvPoint(25, 25), &font, white);
if(showForeground)
{
for(size_t i = 0; i < tld->detectorCascade->detectionResult->fgList->size(); i++)
{
Rect r = tld->detectorCascade->detectionResult->fgList->at(i);
cvRectangle(img, r.tl(), r.br(), white, 1);
}
}
if(showOutput)
{
gui->showImage(img);
char key = gui->getKey();
if(key == 'q') break;
if(key == 'b')
{
ForegroundDetector *fg = tld->detectorCascade->foregroundDetector;
if(fg->bgImg.empty())
{
fg->bgImg = grey.clone();
}
else
{
fg->bgImg.release();
}
}
if(key == 'c')
{
//clear everything
tld->release();
}
if(key == 'l')
{
tld->learningEnabled = !tld->learningEnabled;
printf("LearningEnabled: %d\n", tld->learningEnabled);
}
if(key == 'a')
{
tld->alternating = !tld->alternating;
printf("alternating: %d\n", tld->alternating);
}
if(key == 'e')
{
tld->writeToFile(modelExportFile);
}
if(key == 'i')
{
tld->readFromFile(modelPath);
}
if(key == 'r')
{
CvRect box;
if(getBBFromUser(img, box, gui) == PROGRAM_EXIT)
{
break;
}
Rect r = Rect(box);
tld->selectObject(grey, &r);
}
}
if(saveDir != NULL)
{
char fileName[256];
sprintf(fileName, "%s/%.5d.png", saveDir, imAcq->currentFrame - 1);
cvSaveImage(fileName, img);
}
}
if(reuseFrameOnce)
{
reuseFrameOnce = false;
}
}
cvReleaseImage(&img);
img = NULL;
if(exportModelAfterRun)
{
tld->writeToFile(modelExportFile);
}
if(resultsFile)
{
fclose(resultsFile);
}
}
bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) {
Trajectory loop_traj;
Trajectory best_traj;
double loop_traj_cost, best_traj_cost = -1;
bool gen_success;
int count, count_valid;
for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic) {
TrajectoryCostFunction* loop_critic_p = *loop_critic;
if (loop_critic_p->prepare() == false) {
ROS_WARN("A scoring function failed to prepare");
return false;
}
}
for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {
count = 0;
count_valid = 0;
TrajectorySampleGenerator* gen_ = *loop_gen;
while (gen_->hasMoreTrajectories()) {
gen_success = gen_->nextTrajectory(loop_traj);
if (gen_success == false) {
// TODO use this for debugging
continue;
}
loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
if (all_explored != NULL) {
loop_traj.cost_ = loop_traj_cost;
all_explored->push_back(loop_traj);
}
if (loop_traj_cost >= 0) {
count_valid++;
if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
best_traj_cost = loop_traj_cost;
best_traj = loop_traj;
}
}
count++;
if (max_samples_ > 0 && count >= max_samples_) {
break;
}
}
if (best_traj_cost >= 0) {
traj.xv_ = best_traj.xv_;
traj.yv_ = best_traj.yv_;
traj.thetav_ = best_traj.thetav_;
traj.cost_ = best_traj_cost;
traj.resetPoints();
double px, py, pth;
for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
best_traj.getPoint(i, px, py, pth);
traj.addPoint(px, py, pth);
}
}
ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
if (best_traj_cost >= 0) {
// do not try fallback generators
break;
}
}
return best_traj_cost >= 0;
}
/**
* create and score a trajectory given the current pose of the robot and selected velocities
*/
void TrajectoryPlanner::generateTrajectory(
double x, double y, double theta,
double vx, double vy, double vtheta,
double vx_samp, double vy_samp, double vtheta_samp,
double acc_x, double acc_y, double acc_theta,
double impossible_cost,
Trajectory& traj) {
// make sure the configuration doesn't change mid run
boost::mutex::scoped_lock l(configuration_mutex_);
double x_i = x;
double y_i = y;
double theta_i = theta;
double vx_i, vy_i, vtheta_i;
vx_i = vx;
vy_i = vy;
vtheta_i = vtheta;
//compute the magnitude of the velocities
double vmag = sqrt(vx_samp * vx_samp + vy_samp * vy_samp);
//compute the number of steps we must take along this trajectory to be "safe"
int num_steps;
if(!heading_scoring_) {
num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
} else {
num_steps = int(sim_time_ / sim_granularity_ + 0.5);
}
//we at least want to take one step... even if we won't move, we want to score our current position
if(num_steps == 0) {
num_steps = 1;
}
double dt = sim_time_ / num_steps;
double time = 0.0;
//create a potential trajectory
traj.resetPoints();
traj.xv_ = vx_samp;
traj.yv_ = vy_samp;
traj.thetav_ = vtheta_samp;
traj.cost_ = -1.0;
//initialize the costs for the trajectory
double path_dist = 0.0;
double goal_dist = 0.0;
double occ_cost = 0.0;
double heading_diff = 0.0;
for(int i = 0; i < num_steps; ++i){
//get map coordinates of a point
unsigned int cell_x, cell_y;
//we don't want a path that goes off the know map
if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
traj.cost_ = -1.0;
return;
}
//check the point on the trajectory for legality
double footprint_cost = footprintCost(x_i, y_i, theta_i);
//if the footprint hits an obstacle this trajectory is invalid
if(footprint_cost < 0){
traj.cost_ = -1.0;
return;
//TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits,
//it can actually cause the robot to hit obstacles. There may be something to be done to fix, but I'll have to
//come back to it when I have time. Right now, pulling it out as it'll just make the robot a bit more conservative,
//but safe.
/*
double max_vel_x, max_vel_y, max_vel_th;
//we want to compute the max allowable speeds to be able to stop
//to be safe... we'll make sure we can stop some time before we actually hit
getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th);
//check if we can stop in time
if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){
ROS_ERROR("v: (%.2f, %.2f, %.2f), m: (%.2f, %.2f, %.2f) t:%.2f, st: %.2f, dt: %.2f", vx_samp, vy_samp, vtheta_samp, max_vel_x, max_vel_y, max_vel_th, time, stop_time_buffer_, dt);
//if we can stop... we'll just break out of the loop here.. no point in checking future points
break;
}
else{
traj.cost_ = -1.0;
return;
}
*/
}
occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
//do we want to follow blindly
if (simple_attractor_) {
goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *
(x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
(y_i - global_plan_[global_plan_.size() -1].pose.position.y) *
(y_i - global_plan_[global_plan_.size() -1].pose.position.y);
} else {
bool update_path_and_goal_distances = true;
// with heading scoring, we take into account heading diff, and also only score
// path and goal distance for one point of the trajectory
if (heading_scoring_) {
if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt) {
heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
} else {
update_path_and_goal_distances = false;
}
}
if (update_path_and_goal_distances) {
//update path and goal distances
path_dist = path_map_(cell_x, cell_y).target_dist;
goal_dist = goal_map_(cell_x, cell_y).target_dist;
//if a point on this trajectory has no clear path to goal it is invalid
if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
// ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
// goal_dist, path_dist, impossible_cost);
traj.cost_ = -2.0;
return;
}
}
}
//the point is legal... add it to the trajectory
traj.addPoint(x_i, y_i, theta_i);
//calculate velocities
vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);
//calculate positions
x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);
//increment time
time += dt;
} // end for i < numsteps
//ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
double cost = -1.0;
if (!heading_scoring_) {
cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
} else {
cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
}
traj.cost_ = cost;
}
void Main::doWork()
{
Trajectory trajectory;
//usleep( 1000 );
IplImage *img = imAcqGetImg( imAcq );
std::vector<cv::Point> flandmarkVector;
cv::Point pupil;
int loops = 50; // skip the first 50 frames of the video.
for ( int counter = 0; counter <= loops; counter++ )
{
img = imAcqGetImg( imAcq );
}
Mat grey( img->height, img->width, CV_8UC1 );
cvtColor( cv::Mat( img ), grey, CV_BGR2GRAY );
tld->detectorCascade->imgWidth = grey.cols;
tld->detectorCascade->imgHeight = grey.rows;
tld->detectorCascade->imgWidthStep = grey.step;
if( showTrajectory )
{
trajectory.init( trajectoryLength );
}
if( selectManually )
{
CvRect box;
if( getBBFromUser( img, box, gui ) == PROGRAM_EXIT )
{
return;
}
if( initialBB == NULL )
{
initialBB = new int[ 4 ];
}
initialBB[ 0 ] = box.x;
initialBB[ 1 ] = box.y;
initialBB[ 2 ] = box.width;
initialBB[ 3 ] = box.height;
}
// initialBB is not being set when there is no parameter -b given
doHaar( grey );
FILE *resultsFile = NULL;
if( printResults != NULL )
{
resultsFile = fopen( printResults, "w" );
}
bool reuseFrameOnce = false;
bool skipProcessingOnce = false;
if( loadModel && modelPath != NULL )
{
tld->readFromFile(modelPath);
reuseFrameOnce = true;
}
else if( initialBB != NULL )
{
Rect bb = tldArrayToRect( initialBB );
printf( "Starting at %d %d %d %d\n",
bb.x,
bb.y,
bb.width,
bb.height
);
tld->selectObject( grey, &bb );
skipProcessingOnce = true;
reuseFrameOnce = true;
}
// deleting initialBB to avoid memLeaks
delete initialBB;
cv::Rect faceBB;
eyeTld->detectorCascade->imgWidth = tld->currBB->width;
eyeTld->detectorCascade->imgHeight = tld->currBB->height;
eyeTld->detectorCascade->imgWidthStep = grey.step;
cv::Rect eye = doDetectEye( img );
eye.x += tld->currBB->x;
eye.y += tld->currBB->y;
eyeTld->selectObject( grey, &eye );
//cv::Point pupil = cv::Point( 0, 0 );
while( imAcqHasMoreFrames( imAcq ) )
{
double tic = cvGetTickCount();
if( !reuseFrameOnce )
{
img = imAcqGetImg( imAcq );
if( img == NULL )
{
printf( "current image is NULL, assuming end of input.\n" );
break;
}
cvtColor( cv::Mat( img ), grey, CV_BGR2GRAY );
}
if( !skipProcessingOnce )
{
tld->processImage( img );
eyeTld->processImage( img );
}
else
{
skipProcessingOnce = false;
}
faceBB = cv::Rect( tld->currBB->x,
tld->currBB->y,
tld->currBB->width,
tld->currBB->height
);
std::cout << "bb [ " <<
tld->currBB->x << ", " <<
tld->currBB->y << ", " <<
tld->currBB->width << ", " <<
tld->currBB->height << "]" <<
std::endl;
pupil = findPupil( img, cv::Rect( eyeTld->currBB->x,
eyeTld->currBB->y,
eyeTld->currBB->width,
eyeTld->currBB->height
) );
std::cout << "pupil [ " <<
pupil.x <<
", " <<
pupil.y <<
" ]" <<
std::endl;
/*
flandmarkVector = doFlandmark( *img, faceBB );
if ( flandmarkVector.size() == 2 )
{
std::cout << " [ " <<
flandmarkVector[ 0 ].x << ", " <<
flandmarkVector[ 0 ].y << " ]";
std::cout << " -- [ " <<
flandmarkVector[ 1 ].x << ", " <<
flandmarkVector[ 1 ].y << " ]" <<
std::endl;
if ( flandmarkVector[ 0 ].x > 0 &&
flandmarkVector[ 0 ].y > 0 &&
flandmarkVector[ 0 ].x < tld->detectorCascade->imgWidth &&
flandmarkVector[ 0 ].y < tld->detectorCascade->imgHeight
)
{
cvCircle ( img,
flandmarkVector[ 0 ],
4,
CV_RGB( 0, 111, 255 ),
4
);
}
if ( flandmarkVector[ 1 ].x > 0 &&
flandmarkVector[ 1 ].y > 0 &&
flandmarkVector[ 1 ].x < tld->detectorCascade->imgWidth &&
flandmarkVector[ 1 ].y < tld->detectorCascade->imgHeight
)
{
cvCircle ( img,
flandmarkVector[ 1 ],
4,
CV_RGB( 0, 111, 255 ),
4
);
}
}
else
{
std::cout << "Flandmark vector is of size " <<
flandmarkVector.size() << std::endl;
}
*/
/*
printf( "eye: %d %d %d %d\n",
eye.x,
eye.y,
eye.width,
eye.height
);
*/
//eye.width += tld->currBB->width;
//eye.height += tld->currBB->height;
if( printResults != NULL )
{
if( tld->currBB != NULL )
{
fprintf( resultsFile,
"%d %.2d %.2d %.2d %.2d %f\n",
imAcq->currentFrame - 1,
tld->currBB->x,
tld->currBB->y,
tld->currBB->width,
tld->currBB->height,
tld->currConf
);
}
else
{
fprintf( resultsFile,
"%d NaN NaN NaN NaN NaN\n",
imAcq->currentFrame - 1
);
}
}
double toc = ( cvGetTickCount() - tic ) / cvGetTickFrequency();
toc = toc / 1000000;
float fps = 1 / toc;
int confident = (tld->currConf >= threshold) ? 1 : 0;
if( showOutput || saveDir != NULL )
{
char string[128];
char learningString[10] = "";
if( tld->learning && !eyeTld->learning)
{
strcpy( learningString, "Learning+-" );
}
else if( tld->learning && eyeTld->learning )
{
strcpy( learningString, "Learning++" );
}
else if( !tld->learning && eyeTld->learning )
{
strcpy( learningString, "Learning-+" );
}
sprintf( string,
"#%d,Posterior %.2f; fps: %.2f, #numwindows:%d, %s",
imAcq->currentFrame - 1,
tld->currConf,
fps,
tld->detectorCascade->numWindows,
learningString
);
CvScalar yellow = CV_RGB( 255, 255, 0 );
CvScalar blue = CV_RGB( 0, 0, 255 );
CvScalar black = CV_RGB( 0, 0, 0 );
CvScalar white = CV_RGB( 255, 255, 255 );
if( tld->currBB != NULL && eyeTld->currBB != NULL )
{
CvScalar rectangleColor = ( confident ) ? blue : yellow;
cvRectangle( img,
tld->currBB->tl(),
tld->currBB->br(),
rectangleColor,
8,
8,
0
);
CvScalar rectangleColor2 = ( confident ) ? blue : yellow;
cvRectangle( img,
eyeTld->currBB->tl(),
eyeTld->currBB->br(),
rectangleColor2,
8,
8,
0
);
/*
pupil = findPupil( img, cv::Rect( eyeTld->currBB->x, eyeTld->currBB->y, eyeTld->currBB->width, eyeTld->currBB->height ) );
cvRectangle( img,
cv::Point( pupil.x, pupil.y ),
cv::Point( pupil.x + 2, pupil.y + 2 ),
2,
2,
0
);
*/
/*
if ( eye.x > 0 &&
eye.y > 0 &&
eye.width > 0 &&
eye.height > 0
)
{
cvRectangle( img,
cv::Point( eye.x, eye.y ),
cv::Point( eye.x + eye.width,
eye.y + eye.height
),
rectangleColor,
8,
8,
0
);
}*/
if( showTrajectory )
{
CvPoint center = cvPoint(
tld->currBB->x + tld->currBB->width / 2,
tld->currBB->y + tld->currBB->height / 2
);
cvLine( img,
cvPoint( center.x - 2, center.y - 2 ),
cvPoint( center.x + 2, center.y + 2 ),
rectangleColor,
2
);
cvLine( img,
cvPoint( center.x - 2, center.y + 2),
cvPoint( center.x + 2, center.y - 2),
rectangleColor,
2
);
trajectory.addPoint( center, rectangleColor );
}
}
else if( showTrajectory )
{
trajectory.addPoint( cvPoint( -1, -1 ),
cvScalar( -1, -1, -1 )
);
}
if( showTrajectory )
{
trajectory.drawTrajectory( img );
}
CvFont font;
cvInitFont( &font, CV_FONT_HERSHEY_SIMPLEX, .5, .5, 0, 1, 8 );
// display a black bar at the top of the output window
// for a better background to write on.
cvRectangle( img,
cvPoint( 0, 0 ),
cvPoint( img->width, 50 ),
black,
CV_FILLED,
8,
0
);
cvPutText( img, string, cvPoint( 25, 25 ), &font, white );
if( showForeground )
{
for( size_t i = 0;
i < tld->detectorCascade->detectionResult->fgList->size();
i++
)
{
Rect r = tld->detectorCascade->detectionResult->fgList->at( i );
cvRectangle( img, r.tl(), r.br(), white, 1 );
}
}
if( showOutput )
{
gui->showImage( img );
char key = gui->getKey();
if( key == 'q' )
{
break;
}
if( key == 'b' )
{
ForegroundDetector *fg =
tld->detectorCascade->foregroundDetector;
if( fg->bgImg.empty() )
{
fg->bgImg = grey.clone();
}
else
{
fg->bgImg.release();
}
}
if( key == 'c' )
{
//clear everything
tld->release();
}
if( key == 'l' )
{
tld->learningEnabled = !tld->learningEnabled;
printf( "LearningEnabled: %d\n", tld->learningEnabled );
}
if( key == 'a' )
{
tld->alternating = !tld->alternating;
printf( "alternating: %d\n", tld->alternating );
}
if( key == 'e' )
{
tld->writeToFile( modelExportFile );
}
if( key == 'i' )
{
tld->readFromFile( modelPath );
}
if( key == 'r' )
{
CvRect box;
if( getBBFromUser( img, box, gui ) == PROGRAM_EXIT )
{
break;
}
Rect r = Rect( box );
tld->selectObject( grey, &r );
}
}
if( saveDir != NULL )
{
char fileName[ 256 ];
sprintf( fileName,
"%s/%.5d.png",
saveDir,
imAcq->currentFrame - 1
);
cvSaveImage( fileName, img );
}
}
if( !reuseFrameOnce )
{
cvReleaseImage( &img );
}
else
{
reuseFrameOnce = false;
}
}
if( exportModelAfterRun )
{
tld->writeToFile( modelExportFile );
}
//FB: Avoiding leakage
if( printResults != NULL )
{
fclose( resultsFile );
}
}