// Called on worker thread to get an item. Will wait if no items are available.
// Does busy wait for 2ms.
qitem* queue_pop(queue *queue)
{
qitem *r = qpop(&queue->q);
if (r)
return r;
else
{
TIME start;
GETTIME(start);
INITTIME;
while (1)
{
u64 diff;
TIME stop;
sched_yield();
GETTIME(stop);
NANODIFF(stop, start, diff);
r = qpop(&queue->q);
if (r)
return r;
if (diff > 2000000) // 2ms max busy wait
{
SEM_WAIT(queue->sem);
}
}
}
}
int
main(int argc, char ** argv)
{
struct mandelbrot_param param;
param.height = HEIGHT;
param.width = WIDTH;
param.lower_r = LOWER_R;
param.upper_r = UPPER_R;
param.lower_i = LOWER_I;
param.upper_i = UPPER_I;
param.maxiter = MAXITER;
param.mandelbrot_color.red = (MANDELBROT_COLOR >> 16) & 255;
param.mandelbrot_color.green = (MANDELBROT_COLOR >> 8) & 255;
param.mandelbrot_color.blue = MANDELBROT_COLOR & 255;
// Initializes the mandelbrot computation framework. Among other, spawns the threads in thread pool
init_mandelbrot(¶m);
#ifdef MEASURE
struct mandelbrot_timing ** thread, global;
GETTIME(&global.start);
thread = compute_mandelbrot(param);
GETTIME(&global.stop);
#elif GLUT == 1
srand(time(NULL));
gl_mandelbrot_init(argc, argv);
gl_mandelbrot_start(¶m);
#else
compute_mandelbrot(param);
ppm_save(param.picture, "mandelbrot.ppm");
#endif
#ifdef MEASURE
#if NB_THREADS > 0
int i;
for (i = 0; i < NB_THREADS; i++)
{
printf("%i %li %li %li %li %li %li %li %li\n", i + 1, thread[i]->start.tv_sec, thread[i]->start.tv_nsec, thread[i]->stop.tv_sec, thread[i]->stop.tv_nsec, global.start.tv_sec, global.start.tv_nsec, global.stop.tv_sec, global.stop.tv_nsec);
}
#else
printf("0 %li %li %li %li %li %li %li %li\n", thread[0]->start.tv_sec, thread[0]->start.tv_nsec, thread[0]->stop.tv_sec, thread[0]->stop.tv_nsec, global.start.tv_sec, global.start.tv_nsec, global.stop.tv_sec, global.stop.tv_nsec);
#endif
#endif
// Final: deallocate structures
destroy_mandelbrot(param);
return EXIT_SUCCESS;
}
int main(int argc, const char* argv[])
{
queue* q;
int i;
pthread_t threads[NUM_THREADS];
threadinf infos[NUM_THREADS];
uint64_t thrnums[NUM_THREADS];
q = queue_create();
for (i = 0; i < NUM_THREADS; i++)
{
thrnums[i] = 1;
infos[i].thread = i;
infos[i].q = q;
pthread_create(&threads[i], NULL, producer, (void *)&infos[i]);
}
// for (i = 0; i < ITERATIONS*NUM_THREADS; i++)
i = 0;
TIME start;
GETTIME(start);
while (i < NUM_THREADS)
{
qitem *qi = queue_pop(q);
item *it = (item*)qi->cmd;
if (thrnums[it->thread] != it->n)
{
printf("Items not sequential thread=%d, n=%llu, shouldbe=%llu, recycled=%u!!\n",
it->thread, it->n, thrnums[it->thread], it->recycled);
return 0;
}
thrnums[it->thread]++;
if (thrnums[it->thread] == ITERATIONS)
i++;
// printf("Recycle thr=%d val=%llu, recycled=%u\n",it->thread,it->n, it->recycled++);
queue_recycle(q,qi);
}
uint64_t diff;
TIME stop;
GETTIME(stop);
NANODIFF(stop, start, diff);
printf("Done in: %llums\n",diff / 1000000);
// for (i = 0; i < NUM_THREADS; i++)
// printf("threadpos =%llu\n",thrnums[i]);
// pthread_join(threads[i],NULL);
// printf("No thread sync errors!\n");
return 0;
}
int main(int argc, char **argv) {
int i;
long long diff;
int *res = NULL;
int times = 10;
int clean_avc = 0;
struct timespec start, end;
if (argc > 1)
times = atoi(argv[1]);
if (argc > 2)
clean_avc = atoi(argv[2]);
Hash hmap;
HashInit(&hmap, HASH_STRING, 0);
char *scon = "unconfined_u:unconfined_r:unconfined_t:s0-s0:c0.c1023";
char *tcon = "unconfined_u:object_r:sesqlite_public:s0";
char *clas = "db_column";
char *perm = "select";
char *key = "unconfined_u:unconfined_r:unconfined_t:s0-s0:c0.c1023 unconfined_u:object_r:sesqlite_public:s0 db_column select";
res = malloc(sizeof(int));
*res = selinux_check_access(scon, tcon, clas, perm, NULL);
HashInsert(&hmap, strdup(key), strlen(key), res);
for (i = 0; i < times; ++i) {
if ( clean_avc != 0 ){
cleanavc();
HashClear(&hmap);
}
GETTIME(start)
res = HashFind(&hmap, key, strlen(key));
if (res == NULL) {
res = malloc(sizeof(int));
*res = selinux_check_access(scon, tcon, clas, perm, NULL);
HashInsert(&hmap, strdup(key), strlen(key), res);
}
GETTIME(end)
diff = TIMESPEC_DIFF(start, end);
printf("%lld\n", diff);
}
return 0;
}
void
g_timer_reset (GTimer *timer)
{
g_return_if_fail (timer != NULL);
GETTIME (timer->start);
}
SoftAVC::SoftAVC(
const char *name,
const OMX_CALLBACKTYPE *callbacks,
OMX_PTR appData,
OMX_COMPONENTTYPE **component)
: SoftVideoDecoderOMXComponent(
name, componentName, codingType,
kProfileLevels, ARRAY_SIZE(kProfileLevels),
320 /* width */, 240 /* height */, callbacks,
appData, component),
mCodecCtx(NULL),
mMemRecords(NULL),
mFlushOutBuffer(NULL),
mOmxColorFormat(OMX_COLOR_FormatYUV420Planar),
mIvColorFormat(IV_YUV_420P),
mNewWidth(mWidth),
mNewHeight(mHeight),
mNewLevel(0),
mChangingResolution(false),
mSignalledError(false) {
initPorts(
kNumBuffers, INPUT_BUF_SIZE, kNumBuffers, CODEC_MIME_TYPE);
GETTIME(&mTimeStart, NULL);
// If input dump is enabled, then open create an empty file
GENERATE_FILE_NAMES();
CREATE_DUMP_FILE(mInFile);
CHECK_EQ(initDecoder(mWidth, mHeight), (status_t)OK);
}
void Timer::Idle() {
static GLint TempFPS = 0; // FPS temporaires
static GLfloat fPreviousFPSTime = 0.0f;
GLfloat fNewCurrentTime;
// do
{
fNewCurrentTime = GETTIME() - fStartTime;
fElapsedTime = fNewCurrentTime - fCurrentTime;
}
// while(fElapsedTime < (1.0f/60)); // blocage du fps
++TempFPS; // Incrémentation du nombre de FPS
// Si il s'est écoul?plus d'une seconde, on affiche les FPS :
if( fNewCurrentTime - fPreviousFPSTime > 1.0f ) {
fPreviousFPSTime = fNewCurrentTime;
nFPS = TempFPS;
TempFPS = 0;
}
fCurrentTime = fNewCurrentTime; // On sauvegarde le temps actuel
}
void CResultThread::OnShwo_Track_ThreadMessage(WPARAM wParam, LPARAM lParam)
{
double StartTime = GETTIME();
static IplImage *ShowTrack_ = cvCreateImage(cvSize(800, 480), IPL_DEPTH_8U, 3);//彩色图
//wParam传参跟踪序列的数量
ShowTrackResult(ShowTrack_, TrackT, int(wParam) + 1);
double Endtime = GETTIME();
if (Endtime - StartTime > 10)
{
cout << "显示跟踪耗时" << Endtime - StartTime << endl;
}
}
void
g_timer_start (GTimer *timer)
{
g_return_if_fail (timer != NULL);
timer->active = TRUE;
GETTIME (timer->start);
}
void
g_timer_stop (GTimer *timer)
{
g_return_if_fail (timer != NULL);
timer->active = FALSE;
GETTIME (timer->end);
}
GTimer*
g_timer_new (void)
{
GTimer *timer;
timer = g_new (GTimer, 1);
timer->active = TRUE;
GETTIME (timer->start);
return timer;
}
void
testmem(size_t size)
{
int i;
void (*rout)();
size_t sz;
again:
if (domode == NEXTPAT) {
TIMEVAL st, et;
sz = size >> 2;
printf("%d longwords, 4 pattern tests ", sz);
GETTIME(st);
i = testpatterns(sz);
GETTIME(et);
if (i == 0)
printf("[ OK ]");
else
printf("[ FAILED ]");
PRINTTIME(st, et, size);
} else {
//显示函数
void CResultThread::OnShwo_Pair_ThreadMessage(WPARAM wParam, LPARAM lParam)
{
double StartTime = GETTIME();
static IplImage *OUTImage = cvCreateImage(cvSize(640 * 2, 480), IPL_DEPTH_8U, 1);
CompareImage(Src_Left_Gray[int(wParam)], Src_Right_Gray[int(lParam)], OUTImage);
//画配对线
for (int i = 0; i < NumOfPairCounter; i++)
{
cvLine(OUTImage, cvPoint((int)PairCounter[i].Left->x, (int)PairCounter[i].Left->y), cvPoint((int)PairCounter[i].Right->x + 640, (int)PairCounter[i].Right->y), cvScalar(150));
}
cvShowImage("配对线", OUTImage);
double Endtime = GETTIME();
if (Endtime - StartTime > 10)
{
cout << "显示配对耗时" << Endtime - StartTime << endl;
}
}
int print_midstats(struct schedule *sched, struct stats *old_stats)
{
TIMERTYPE temptime;
float timedif = 0;
struct listed_entity *le = sched->br.busylist;
struct stats tempstats;
float total_mbits = 0;
GETTIME(temptime);
timedif = floatdiff(&temptime, &sched->lasttick);
LOG("Time:\t%lu\n", GETSECONDS(temptime));
while (le != NULL)
{
struct scheduled_event *ev = (struct scheduled_event *)le->entity;
if (ev->opt->get_stats != NULL)
{
memset(&tempstats, 0, sizeof(struct stats));
ev->opt->get_stats((void *)ev->opt, (void *)&tempstats);
neg_stats(&tempstats, ev->stats);
total_mbits = BYTES_TO_MBITSPS(tempstats.total_bytes);
LOG("Event:\t%s\t", ev->opt->filename);
LOG("Network:\t%5.0fMb/s\tDropped %lu\tIncomplete %lu",
total_mbits / timedif, tempstats.dropped, tempstats.incomplete);
if (tempstats.progress != -1)
{
LOG("\tProgress %02d%%", tempstats.progress);
}
LOG("\n");
add_stats(ev->stats, &tempstats);
}
le = le->child;
}
memset(&tempstats, 0, sizeof(struct stats));
oper_to_all(sched->default_opt->diskbranch, BRANCHOP_GETSTATS,
(void *)&tempstats);
neg_stats(&tempstats, old_stats);
total_mbits = BYTES_TO_MBITSPS(tempstats.total_written);
LOG("HD-Speed:\t%5.0fMb/s\n", total_mbits / timedif);
add_stats(old_stats, &tempstats);
LOG("Ringbuffers: ");
print_br_stats(sched->default_opt->membranch);
LOG("Recpoints: ");
print_br_stats(sched->default_opt->diskbranch);
COPYTIME(temptime, sched->lasttick);
LOG("----------------------------------------\n");
return 0;
}
void vacfunc(real_T *dx, real_T *xms, SimStruct *S)
{
real_T mvs[NM], x[NS];
#ifdef DEBUG
debug("vacfunc entered.\n");
#endif
// #ifdef DEBUG
// debug("Time:%f (in minutes: %f) entered.\n",GETTIME(S), GETTIME(S)*60);
// #endif
GETXMV(XMV,S);
GETIDV(IDV,S);
VAModel(dx, x, mvs, xms, STS(S), XMV, GETTIME(S)*60, 0, IDV);
#if defined(CONTINUOUS_STATES)
MULTIPLY(dx, NS, 60);
#elif defined(DISCRETE_STATES)
DIVIDE(dx, NS, 180);
#endif
#ifdef DEBUG
debug("vacfunc left.\n");
#endif
}
gdouble
g_timer_elapsed (GTimer *timer,
gulong *microseconds)
{
gdouble total;
gint64 elapsed;
g_return_val_if_fail (timer != NULL, 0);
if (timer->active)
GETTIME (timer->end);
elapsed = timer->end - timer->start;
total = elapsed / 1e9;
if (microseconds)
*microseconds = (elapsed / 1000) % 1000000;
return total;
}
void
g_timer_continue (GTimer *timer)
{
guint64 elapsed;
g_return_if_fail (timer != NULL);
g_return_if_fail (timer->active == FALSE);
/* Get elapsed time and reset timer start time
* to the current time minus the previously
* elapsed interval.
*/
elapsed = timer->end - timer->start;
GETTIME (timer->start);
timer->start -= elapsed;
timer->active = TRUE;
}
void Timer::Start() {
fStartTime = GETTIME();
fCurrentTime = 0.0f;
fElapsedTime = 0.0f;
nFPS = 0;
}
int main(int argc, char **argv)
#endif
{
int err = 0;
pthread_t streamer_pthread;
#if(DAEMON)
struct opt_s *opt = (struct opt_s *)opti;
D("Starting thread from daemon");
#else
LOG("Running in non-daemon mode\n");
struct opt_s *opt = malloc(sizeof(struct opt_s));
CHECK_ERR_NONNULL(opt, "opt malloc");
LOG("STREAMER: Reading parameters\n");
clear_and_default(opt, 1);
err = parse_options(argc, argv, opt);
if (err != 0)
STREAMER_ERROR_EXIT;
err = init_branches(opt);
CHECK_ERR("init branches");
err = init_recp(opt);
CHECK_ERR("init recpoints");
#ifdef HAVE_LIBCONFIG_H
err = init_cfg(opt);
//TODO: cfg destruction
if (err != 0)
{
E("Error in cfg init");
STREAMER_ERROR_EXIT;
}
#endif //HAVE_LIBCONFIG_H
err = init_rbufs(opt);
CHECK_ERR("init rbufs");
#endif //DAEMON
/* Check and set cfgs at this point */
//return -1;
/* If we're sending stuff, check all the diskbranch members for the files they have */
/* Also updates the fileholders list to point the owners of files to correct drives */
#ifdef HAVE_LIBCONFIG_H
if (opt->optbits & READMODE)
{
oper_to_all(opt->diskbranch, BRANCHOP_CHECK_FILES, (void *)opt);
LOG
("For recording %s: %lu files were found out of %lu total. file index shows %ld files\n",
opt->filename, opt->cumul_found, opt->cumul, afi_get_n_files(opt->fi));
}
#endif //HAVE_LIBCONFIG_H
/* Handle hostname etc */
/* TODO: Whats the best way that accepts any format? */
err = prep_streamer(opt);
if (err != 0)
{
STREAMER_ERROR_EXIT;
}
if (opt->optbits & READMODE)
LOG("STREAMER: In main, starting sending thread \n");
else
LOG("STREAMER: In main, starting receiver thread \n");
#ifdef HAVE_LRT
/* TCP streams start counting from accept and others from here */
if (!(opt->optbits & (CAPTURE_W_TCPSPLICE | CAPTURE_W_TCPSTREAM)))
GETTIME(opt->starting_time);
set_status_for_opt(opt, STATUS_RUNNING);
err =
pthread_create(&streamer_pthread, NULL, opt->streamer_ent->start,
(void *)opt->streamer_ent);
if (err != 0)
{
printf("ERROR; return code from pthread_create() is %d\n", err);
STREAMER_ERROR_EXIT;
}
/* Other thread spawned so we can minimize our priority */
minimize_priority();
#ifdef TUNE_AFFINITY
/* Caused some weird ass bugs and crashes so not used anymore NEVER */
/* Put the capture on the first core */
CPU_SET(0, &(opt->cpuset));
err = pthread_setaffinity_np(streamer_pthread, sizeof(cpu_set_t), &cpuset);
if (err != 0)
{
E("Error: setting affinity: %d", err);
}
CPU_ZERO(&cpuset);
#endif
#endif
{
/* READMODE shuts itself down so we just go to pthread_join */
/* Check also that last_packet is 0. Else the thread should shut itself */
/* down */
if (!(opt->optbits & READMODE) && opt->last_packet == 0 &&
!(opt->
optbits & (CAPTURE_W_TCPSPLICE | CAPTURE_W_TCPSTREAM |
CAPTURE_W_MULTISTREAM)))
{
TIMERTYPE now;
GETTIME(now);
while ((GETSECONDS(now) <=
(GETSECONDS(opt->starting_time) + (long)opt->time)) &&
get_status_from_opt(opt) == STATUS_RUNNING)
{
sleep(1);
GETTIME(now);
}
shutdown_thread(opt);
pthread_mutex_lock(&(opt->membranch->branchlock));
pthread_cond_broadcast(&(opt->membranch->busysignal));
pthread_mutex_unlock(&(opt->membranch->branchlock));
}
}
err = pthread_join(streamer_pthread, NULL);
if (err < 0)
{
printf("ERROR; return code from pthread_join() is %d\n", err);
}
else
D("Streamer thread exit OK");
LOG("STREAMER: Threads finished. Getting stats for %s\n", opt->filename);
GETTIME(opt->endtime);
LOG("Blocking until owned buffers are released\n");
block_until_free(opt->membranch, opt);
#if(DAEMON)
LOG("Buffers finished\n");
if (get_status_from_opt(opt) != STATUS_STOPPED)
{
E("Thread didnt finish nicely with STATUS_STOPPED");
set_status_for_opt(opt, STATUS_FINISHED);
}
else
set_status_for_opt(opt, STATUS_FINISHED);
#else
close_streamer(opt);
#endif
#if(DAEMON)
D("Streamer thread exiting for %s", opt->filename);
pthread_exit(NULL);
#else
close_opts(opt);
STREAMER_EXIT;
#endif
}
void SoftHEVC::onQueueFilled(OMX_U32 portIndex) {
UNUSED(portIndex);
if (mSignalledError) {
return;
}
if (mOutputPortSettingsChange != NONE) {
return;
}
if (NULL == mCodecCtx) {
if (OK != initDecoder()) {
ALOGE("Failed to initialize decoder");
notify(OMX_EventError, OMX_ErrorUnsupportedSetting, 0, NULL);
mSignalledError = true;
return;
}
}
if (outputBufferWidth() != mStride) {
/* Set the run-time (dynamic) parameters */
mStride = outputBufferWidth();
setParams(mStride);
}
List<BufferInfo *> &inQueue = getPortQueue(kInputPortIndex);
List<BufferInfo *> &outQueue = getPortQueue(kOutputPortIndex);
while (!outQueue.empty()) {
BufferInfo *inInfo;
OMX_BUFFERHEADERTYPE *inHeader;
BufferInfo *outInfo;
OMX_BUFFERHEADERTYPE *outHeader;
size_t timeStampIx;
inInfo = NULL;
inHeader = NULL;
if (!mIsInFlush) {
if (!inQueue.empty()) {
inInfo = *inQueue.begin();
inHeader = inInfo->mHeader;
} else {
break;
}
}
outInfo = *outQueue.begin();
outHeader = outInfo->mHeader;
outHeader->nFlags = 0;
outHeader->nTimeStamp = 0;
outHeader->nOffset = 0;
if (inHeader != NULL && (inHeader->nFlags & OMX_BUFFERFLAG_EOS)) {
mReceivedEOS = true;
if (inHeader->nFilledLen == 0) {
inQueue.erase(inQueue.begin());
inInfo->mOwnedByUs = false;
notifyEmptyBufferDone(inHeader);
inHeader = NULL;
setFlushMode();
}
}
/* Get a free slot in timestamp array to hold input timestamp */
{
size_t i;
timeStampIx = 0;
for (i = 0; i < MAX_TIME_STAMPS; i++) {
if (!mTimeStampsValid[i]) {
timeStampIx = i;
break;
}
}
if (inHeader != NULL) {
mTimeStampsValid[timeStampIx] = true;
mTimeStamps[timeStampIx] = inHeader->nTimeStamp;
}
}
{
ivd_video_decode_ip_t s_dec_ip;
ivd_video_decode_op_t s_dec_op;
WORD32 timeDelay, timeTaken;
size_t sizeY, sizeUV;
if (!setDecodeArgs(&s_dec_ip, &s_dec_op, inHeader, outHeader, timeStampIx)) {
ALOGE("Decoder arg setup failed");
notify(OMX_EventError, OMX_ErrorUndefined, 0, NULL);
mSignalledError = true;
return;
}
GETTIME(&mTimeStart, NULL);
/* Compute time elapsed between end of previous decode()
* to start of current decode() */
TIME_DIFF(mTimeEnd, mTimeStart, timeDelay);
IV_API_CALL_STATUS_T status;
status = ivdec_api_function(mCodecCtx, (void *)&s_dec_ip, (void *)&s_dec_op);
bool unsupportedResolution =
(IVD_STREAM_WIDTH_HEIGHT_NOT_SUPPORTED == (s_dec_op.u4_error_code & 0xFF));
/* Check for unsupported dimensions */
if (unsupportedResolution) {
ALOGE("Unsupported resolution : %dx%d", mWidth, mHeight);
notify(OMX_EventError, OMX_ErrorUnsupportedSetting, 0, NULL);
mSignalledError = true;
return;
}
bool allocationFailed = (IVD_MEM_ALLOC_FAILED == (s_dec_op.u4_error_code & 0xFF));
if (allocationFailed) {
ALOGE("Allocation failure in decoder");
notify(OMX_EventError, OMX_ErrorUnsupportedSetting, 0, NULL);
mSignalledError = true;
return;
}
bool resChanged = (IVD_RES_CHANGED == (s_dec_op.u4_error_code & 0xFF));
getVUIParams();
GETTIME(&mTimeEnd, NULL);
/* Compute time taken for decode() */
TIME_DIFF(mTimeStart, mTimeEnd, timeTaken);
ALOGV("timeTaken=%6d delay=%6d numBytes=%6d", timeTaken, timeDelay,
s_dec_op.u4_num_bytes_consumed);
if (s_dec_op.u4_frame_decoded_flag && !mFlushNeeded) {
mFlushNeeded = true;
}
if ((inHeader != NULL) && (1 != s_dec_op.u4_frame_decoded_flag)) {
/* If the input did not contain picture data, then ignore
* the associated timestamp */
mTimeStampsValid[timeStampIx] = false;
}
// If the decoder is in the changing resolution mode and there is no output present,
// that means the switching is done and it's ready to reset the decoder and the plugin.
if (mChangingResolution && !s_dec_op.u4_output_present) {
mChangingResolution = false;
resetDecoder();
resetPlugin();
mStride = outputBufferWidth();
setParams(mStride);
continue;
}
if (resChanged) {
mChangingResolution = true;
if (mFlushNeeded) {
setFlushMode();
}
continue;
}
// Combine the resolution change and coloraspects change in one PortSettingChange event
// if necessary.
if ((0 < s_dec_op.u4_pic_wd) && (0 < s_dec_op.u4_pic_ht)) {
uint32_t width = s_dec_op.u4_pic_wd;
uint32_t height = s_dec_op.u4_pic_ht;
bool portWillReset = false;
handlePortSettingsChange(&portWillReset, width, height);
if (portWillReset) {
resetDecoder();
resetPlugin();
return;
}
} else if (mUpdateColorAspects) {
notify(OMX_EventPortSettingsChanged, kOutputPortIndex,
kDescribeColorAspectsIndex, NULL);
mUpdateColorAspects = false;
return;
}
if (s_dec_op.u4_output_present) {
outHeader->nFilledLen = (outputBufferWidth() * outputBufferHeight() * 3) / 2;
outHeader->nTimeStamp = mTimeStamps[s_dec_op.u4_ts];
mTimeStampsValid[s_dec_op.u4_ts] = false;
outInfo->mOwnedByUs = false;
outQueue.erase(outQueue.begin());
outInfo = NULL;
notifyFillBufferDone(outHeader);
outHeader = NULL;
} else if (mIsInFlush) {
/* If in flush mode and no output is returned by the codec,
* then come out of flush mode */
mIsInFlush = false;
/* If EOS was recieved on input port and there is no output
* from the codec, then signal EOS on output port */
if (mReceivedEOS) {
outHeader->nFilledLen = 0;
outHeader->nFlags |= OMX_BUFFERFLAG_EOS;
outInfo->mOwnedByUs = false;
outQueue.erase(outQueue.begin());
outInfo = NULL;
notifyFillBufferDone(outHeader);
outHeader = NULL;
resetPlugin();
}
}
}
/* If input EOS is seen and decoder is not in flush mode,
* set the decoder in flush mode.
* There can be a case where EOS is sent along with last picture data
* In that case, only after decoding that input data, decoder has to be
* put in flush. This case is handled here */
if (mReceivedEOS && !mIsInFlush) {
setFlushMode();
}
// TODO: Handle more than one picture data
if (inHeader != NULL) {
inInfo->mOwnedByUs = false;
inQueue.erase(inQueue.begin());
inInfo = NULL;
notifyEmptyBufferDone(inHeader);
inHeader = NULL;
}
}
}
void SoftAVC::onQueueFilled(OMX_U32 portIndex) {
UNUSED(portIndex);
if (mSignalledError) {
return;
}
if (mOutputPortSettingsChange != NONE) {
return;
}
List<BufferInfo *> &inQueue = getPortQueue(kInputPortIndex);
List<BufferInfo *> &outQueue = getPortQueue(kOutputPortIndex);
/* If input EOS is seen and decoder is not in flush mode,
* set the decoder in flush mode.
* There can be a case where EOS is sent along with last picture data
* In that case, only after decoding that input data, decoder has to be
* put in flush. This case is handled here */
if (mReceivedEOS && !mIsInFlush) {
setFlushMode();
}
while (!outQueue.empty()) {
BufferInfo *inInfo;
OMX_BUFFERHEADERTYPE *inHeader;
BufferInfo *outInfo;
OMX_BUFFERHEADERTYPE *outHeader;
size_t timeStampIx;
inInfo = NULL;
inHeader = NULL;
if (!mIsInFlush) {
if (!inQueue.empty()) {
inInfo = *inQueue.begin();
inHeader = inInfo->mHeader;
if (inHeader == NULL) {
inQueue.erase(inQueue.begin());
inInfo->mOwnedByUs = false;
continue;
}
} else {
break;
}
}
outInfo = *outQueue.begin();
outHeader = outInfo->mHeader;
outHeader->nFlags = 0;
outHeader->nTimeStamp = 0;
outHeader->nOffset = 0;
if (inHeader != NULL) {
if (inHeader->nFilledLen == 0) {
inQueue.erase(inQueue.begin());
inInfo->mOwnedByUs = false;
notifyEmptyBufferDone(inHeader);
if (!(inHeader->nFlags & OMX_BUFFERFLAG_EOS)) {
continue;
}
mReceivedEOS = true;
inHeader = NULL;
setFlushMode();
} else if (inHeader->nFlags & OMX_BUFFERFLAG_EOS) {
mReceivedEOS = true;
}
}
// When there is an init required and the decoder is not in flush mode,
// update output port's definition and reinitialize decoder.
if (mInitNeeded && !mIsInFlush) {
bool portWillReset = false;
status_t err = reInitDecoder(mNewWidth, mNewHeight);
if (err != OK) {
notify(OMX_EventError, OMX_ErrorUnsupportedSetting, err, NULL);
mSignalledError = true;
return;
}
handlePortSettingsChange(&portWillReset, mNewWidth, mNewHeight);
return;
}
/* Get a free slot in timestamp array to hold input timestamp */
{
size_t i;
timeStampIx = 0;
for (i = 0; i < MAX_TIME_STAMPS; i++) {
if (!mTimeStampsValid[i]) {
timeStampIx = i;
break;
}
}
if (inHeader != NULL) {
mTimeStampsValid[timeStampIx] = true;
mTimeStamps[timeStampIx] = inHeader->nTimeStamp;
}
}
{
ivd_video_decode_ip_t s_dec_ip;
ivd_video_decode_op_t s_dec_op;
WORD32 timeDelay, timeTaken;
size_t sizeY, sizeUV;
setDecodeArgs(&s_dec_ip, &s_dec_op, inHeader, outHeader, timeStampIx);
// If input dump is enabled, then write to file
DUMP_TO_FILE(mInFile, s_dec_ip.pv_stream_buffer, s_dec_ip.u4_num_Bytes);
GETTIME(&mTimeStart, NULL);
/* Compute time elapsed between end of previous decode()
* to start of current decode() */
TIME_DIFF(mTimeEnd, mTimeStart, timeDelay);
IV_API_CALL_STATUS_T status;
status = ivdec_api_function(mCodecCtx, (void *)&s_dec_ip, (void *)&s_dec_op);
bool unsupportedDimensions =
(IVD_STREAM_WIDTH_HEIGHT_NOT_SUPPORTED == (s_dec_op.u4_error_code & 0xFF));
bool resChanged = (IVD_RES_CHANGED == (s_dec_op.u4_error_code & 0xFF));
bool unsupportedLevel = (IH264D_UNSUPPORTED_LEVEL == (s_dec_op.u4_error_code & 0xFF));
GETTIME(&mTimeEnd, NULL);
/* Compute time taken for decode() */
TIME_DIFF(mTimeStart, mTimeEnd, timeTaken);
PRINT_TIME("timeTaken=%6d delay=%6d numBytes=%6d", timeTaken, timeDelay,
s_dec_op.u4_num_bytes_consumed);
if (s_dec_op.u4_frame_decoded_flag && !mFlushNeeded) {
mFlushNeeded = true;
}
if ((inHeader != NULL) && (1 != s_dec_op.u4_frame_decoded_flag)) {
/* If the input did not contain picture data, then ignore
* the associated timestamp */
mTimeStampsValid[timeStampIx] = false;
}
// This is needed to handle CTS DecoderTest testCodecResetsH264WithoutSurface,
// which is not sending SPS/PPS after port reconfiguration and flush to the codec.
if (unsupportedDimensions && !mFlushNeeded) {
bool portWillReset = false;
mNewWidth = s_dec_op.u4_pic_wd;
mNewHeight = s_dec_op.u4_pic_ht;
status_t err = reInitDecoder(mNewWidth, mNewHeight);
if (err != OK) {
notify(OMX_EventError, OMX_ErrorUnsupportedSetting, err, NULL);
mSignalledError = true;
return;
}
handlePortSettingsChange(&portWillReset, mNewWidth, mNewHeight);
setDecodeArgs(&s_dec_ip, &s_dec_op, inHeader, outHeader, timeStampIx);
ivdec_api_function(mCodecCtx, (void *)&s_dec_ip, (void *)&s_dec_op);
return;
}
if (unsupportedLevel && !mFlushNeeded) {
mNewLevel = 51;
status_t err = reInitDecoder(mNewWidth, mNewHeight);
if (err != OK) {
notify(OMX_EventError, OMX_ErrorUnsupportedSetting, err, NULL);
mSignalledError = true;
return;
}
setDecodeArgs(&s_dec_ip, &s_dec_op, inHeader, outHeader, timeStampIx);
ivdec_api_function(mCodecCtx, (void *)&s_dec_ip, (void *)&s_dec_op);
return;
}
// If the decoder is in the changing resolution mode and there is no output present,
// that means the switching is done and it's ready to reset the decoder and the plugin.
if (mChangingResolution && !s_dec_op.u4_output_present) {
mChangingResolution = false;
resetDecoder();
resetPlugin();
continue;
}
if (unsupportedDimensions || resChanged) {
mChangingResolution = true;
if (mFlushNeeded) {
setFlushMode();
}
if (unsupportedDimensions) {
mNewWidth = s_dec_op.u4_pic_wd;
mNewHeight = s_dec_op.u4_pic_ht;
mInitNeeded = true;
}
continue;
}
if (unsupportedLevel) {
if (mFlushNeeded) {
setFlushMode();
}
mNewLevel = 51;
mInitNeeded = true;
continue;
}
if ((0 < s_dec_op.u4_pic_wd) && (0 < s_dec_op.u4_pic_ht)) {
uint32_t width = s_dec_op.u4_pic_wd;
uint32_t height = s_dec_op.u4_pic_ht;
bool portWillReset = false;
handlePortSettingsChange(&portWillReset, width, height);
if (portWillReset) {
resetDecoder();
return;
}
}
if (s_dec_op.u4_output_present) {
outHeader->nFilledLen = (outputBufferWidth() * outputBufferHeight() * 3) / 2;
outHeader->nTimeStamp = mTimeStamps[s_dec_op.u4_ts];
mTimeStampsValid[s_dec_op.u4_ts] = false;
outInfo->mOwnedByUs = false;
outQueue.erase(outQueue.begin());
outInfo = NULL;
notifyFillBufferDone(outHeader);
outHeader = NULL;
} else {
/* If in flush mode and no output is returned by the codec,
* then come out of flush mode */
mIsInFlush = false;
/* If EOS was recieved on input port and there is no output
* from the codec, then signal EOS on output port */
if (mReceivedEOS) {
outHeader->nFilledLen = 0;
outHeader->nFlags |= OMX_BUFFERFLAG_EOS;
outInfo->mOwnedByUs = false;
outQueue.erase(outQueue.begin());
outInfo = NULL;
notifyFillBufferDone(outHeader);
outHeader = NULL;
resetPlugin();
}
}
}
if (inHeader != NULL) {
inInfo->mOwnedByUs = false;
inQueue.erase(inQueue.begin());
inInfo = NULL;
notifyEmptyBufferDone(inHeader);
inHeader = NULL;
}
}
}
//跟踪处理函数
void CResultThread::OnThreadMessage(WPARAM wParam,LPARAM lParam)
{
//cout << "OnThreadMessage" << endl;
//NEXT->PostThreadMessage(WM_SHOW_TRACK_THREADMESSAGE, NULL, NULL);
//cout << "NEXT->PostThreadMessage(WM_SHOW_TRACK_THREADMESSAGE, NULL, NULL);" << endl;
//return;
double Time_Temp = GETTIME();
//转换时间
//从队列中获取时间
//取平均值
int Time = (Time_L[int(wParam)] + Time_R[int(lParam)])/2;
//cout << Time << endl;
//最开始的资源处理完了，置位
To_Handle_Left[int(wParam)] = false;
To_Handle_Right[int(lParam)] = false;
//用于控制实际跟踪队列使用
static int ControlOfTrackNum = -1;
//添加配对map***************
static int MapPair[MaxTrackList][2]; //配对map[个数] (当前连通域点，跟踪序列)
int NumOfMapPair = 0;
static int UseOfTrackList[MaxTrackList]; //跟踪序列的使用次数，用于判断是否需要拷贝
std::memset(UseOfTrackList, 0, MaxTrackList*sizeof(int));//不值0 就没办法使用++
for (int i = 0; i < NumOfPairCounter; i++)
{
bool BelongToList = false;
for (int j = 0; j < ControlOfTrackNum + 1; j++)//这个要加1
{
if (!TrackT[j].TrackLife)//没有生命
continue;
//注意匹配优先级，级别为 先匹配 三个的，再匹配两个点的序列，最后匹配一个点的序列
//有一个匹配上则跳出
/*********************************************************************************/
//已有三或三个以上个运动序列的
if (TrackT[j].NumOFTrackList >= 2)
{
//system("pause");
//三或三个以上个运动序列的跟踪判断条件
//从GPlist中找对应时间下的点，做位置判断
//对应下标
//GPlist的时间起点是从第二个序列点开始的
//同时要对应的GPlist的下标应该 *时间系数 ms 与 GPlist的迭代时间比值
int mark = (Time - TrackT[j].startTrackTime) - TrackT[j].TrackTime[1];
if (mark < 0)
continue;
//只匹配三维
//三维距离(配对点 跟 某个跟踪序列的这个时刻值的位置 距离)
//这个观测点不能超过预测点太多
double ans3D =
MySquare(PairCounter[i].Position.x - TrackT[j].GPlist[mark * 10][0]) +
MySquare(PairCounter[i].Position.y - TrackT[j].GPlist[mark * 10][1]) +
MySquare(PairCounter[i].Position.z - TrackT[j].GPlist[mark * 10][2]);
if (ans3D < 0.35 * 0.35)//两帧之间球的运动距离最大为20mm / 20mm)
{
//假如这个点被添加到这个序列，计算这个序列上一个点的速度，跟预测的速度差别
//用以滤掉可能被添加进去的噪声
double tempV[3];
// T6-（T8 - T6） = 2*T6 - T8
//注意相对时间和绝对时间
int detaT = 2 * TrackT[j].TrackTime[TrackT[j].NumOFTrackList] - (Time - TrackT[j].startTrackTime);
if (detaT < (double)(TrackT[j].TrackTime[1] - TrackT[j].TrackTime[0]) / 2)
{
//使用 第1个点 ，上次的跟踪测量点 ，这帧的可疑点求取速度
if (Time - TrackT[j].startTrackTime == TrackT[j].TrackTime[0])
{
cout << endl;
}
getV(
TrackT[j].Position3D[0],
PairCounter[i].Position,
TrackT[j].TrackTime[0],
(Time - TrackT[j].startTrackTime),
tempV);
}
if (
detaT > (double)(TrackT[j].TrackTime[1] - TrackT[j].TrackTime[0]) / 2
&& detaT <= TrackT[j].TrackTime[1]
)
{
//使用 第2个点 ，上次的跟踪测量点 ，这帧的可疑点求取速度
if (Time - TrackT[j].startTrackTime == TrackT[j].TrackTime[1])
{
cout <<"************************************************"<< endl;
}
getV(
TrackT[j].Position3D[1],
PairCounter[i].Position,
TrackT[j].TrackTime[1],
(Time - TrackT[j].startTrackTime),
tempV);
}
if (detaT > TrackT[j].TrackTime[1])
{//使用GPlist中的预测点求取速度
//使用 第GPlist中的时间对应点个点 ，上次的跟踪测量点 ，这帧的跟踪点求取速度
//GPList中的下标是
//（图片个采集的绝对时间 - 跟踪开始的时间） = 相对时间
// 相对时间 - 第二个跟踪点的相对时间 = GPlist下标的10倍
int RT = (detaT - TrackT[j].TrackTime[1]) * 10;
//并预测轨迹
//计算速度分量
if (Time - TrackT[j].startTrackTime == detaT)
{
cout << endl;
}
getV(
TrackT[j].GPlist[RT][0],
TrackT[j].GPlist[RT][1],
TrackT[j].GPlist[RT][2],
PairCounter[i].Position.x,
PairCounter[i].Position.y,
PairCounter[i].Position.z,
detaT,
Time - TrackT[j].startTrackTime,
tempV);
}
//通过以上三种情况求取出速度后
//如果这个点属于这个序列，那么求出的上一个点的速度跟GPList中的上一个点的预测速度的 差进行比较
//如果差值小于某个范围，认定这个点属于这个序列，假设成立
if (
abs(tempV[0] - TrackT[j].GPlist[(TrackT[j].TrackTime[TrackT[j].NumOFTrackList] - TrackT[j].TrackTime[1]) * 10][3]) < 1 //误差 m/s
&& abs(tempV[1] - TrackT[j].GPlist[(TrackT[j].TrackTime[TrackT[j].NumOFTrackList] - TrackT[j].TrackTime[1]) * 10][4]) < 1 //误差 m/s
&& abs(tempV[2] - TrackT[j].GPlist[(TrackT[j].TrackTime[TrackT[j].NumOFTrackList] - TrackT[j].TrackTime[1]) * 10][5]) < 1 //误差 m/s
)
{
MapPair[NumOfMapPair][0] = i;
MapPair[NumOfMapPair][1] = j;
NumOfMapPair++;
if (NumOfMapPair == MaxTrackList)
{
cout << "NumOfMapPair会越界" << endl;
system("pause");
}
UseOfTrackList[j]++;
BelongToList = true;
//cout << "******************************************************************************************" << endl;
//system("pause");
continue;
}
}
}
/*********************************************************************************/
//只有两个点的跟踪序列
if (TrackT[j].NumOFTrackList == 1)
{
//使用二维距离不具有效用
//球离相机的距离不同，在二维投影面上的距离变化不同
//三维距离
double ans3D =
MySquare(PairCounter[i].Position.x - TrackT[j].Position3D[TrackT[j].NumOFTrackList].x) +
MySquare(PairCounter[i].Position.y - TrackT[j].Position3D[TrackT[j].NumOFTrackList].y) +
MySquare(PairCounter[i].Position.z - TrackT[j].Position3D[TrackT[j].NumOFTrackList].z);
if (ans3D < 0.350 * 0.350)//两帧之间球的运动距离最大为350mm
{
//运动距离满足
//判断左图
//判断运动方向是否大致相同
//使用向量的方法判断角度 点1-〉点2 点2-〉点3向量夹角
//采用这种方法可以很大程度上减少三个跟踪序列的判断，节省资源
double x1 = TrackT[j].PointList_Left[1][0] - TrackT[j].PointList_Left[0][0];
double y1 = TrackT[j].PointList_Left[1][1] - TrackT[j].PointList_Left[0][1];
double x2 = PairCounter[i].Left->x - TrackT[j].PointList_Left[1][0];
double y2 = PairCounter[i].Left->y - TrackT[j].PointList_Left[1][1];
//向量夹角
double COS_sita = (x1*x2 + y1*y2) / sqrt((x1*x1 + y1*y1)*(x2*x2 + y2*y2));
//sita一定是个正数
double sita = acos(COS_sita) / CV_PI * 180;
if (sita < 10)//向量角度小于10度
{
//std::cout << "sita " << sita << endl;
//判断右图
//判断运动方向是否大致相同
//使用向量的方法判断角度 点1-〉点2 点2-〉点3向量夹角
//采用这种方法可以很大程度上减少三个跟踪序列的判断，节省跟踪资源
x1 = TrackT[j].PointList_Right[1][0] - TrackT[j].PointList_Right[0][0];
y1 = TrackT[j].PointList_Right[1][1] - TrackT[j].PointList_Right[0][1];
x2 = PairCounter[i].Right->x - TrackT[j].PointList_Right[1][0];
y2 = PairCounter[i].Right->y - TrackT[j].PointList_Right[1][1];
//向量夹角
COS_sita = (x1*x2 + y1*y2) / sqrt((x1*x1 + y1*y1)*(x2*x2 + y2*y2));
//sita一定是个正数
sita = acos(COS_sita) / CV_PI * 180;
if (sita < 10)//向量角度小于10度
{
//std::cout << "sita1 " << sita << endl;
//记录配对
MapPair[NumOfMapPair][0] = i;
MapPair[NumOfMapPair][1] = j;
NumOfMapPair++;
if (NumOfMapPair == MaxTrackList)
{
cout << "NumOfMapPair会越界" << endl;
NumOfMapPair = 0;
//system("pause");
}
UseOfTrackList[j]++;
BelongToList = true;
continue;
}
}
}
}
//只有一个点的跟踪序列
if (TrackT[j].NumOFTrackList == 0)
{
////二维距离
//double ans2DLeft = (PairCounter[i].Left->x - TrackT[j].PointList_Left[TrackT[j].NumOFTrackList][0])* (PairCounter[i].Left->x - TrackT[j].PointList_Left[TrackT[j].NumOFTrackList][0])
// + (PairCounter[i].Left->y - TrackT[j].PointList_Left[TrackT[j].NumOFTrackList][1]) * (PairCounter[i].Left->y - TrackT[j].PointList_Left[TrackT[j].NumOFTrackList][1]);
//double ans2DRight = (PairCounter[i].Right->x - TrackT[j].PointList_Right[TrackT[j].NumOFTrackList][0])* (PairCounter[i].Right->x - TrackT[j].PointList_Right[TrackT[j].NumOFTrackList][0])
// + (PairCounter[i].Right->y - TrackT[j].PointList_Right[TrackT[j].NumOFTrackList][1]) * (PairCounter[i].Right->y - TrackT[j].PointList_Right[TrackT[j].NumOFTrackList][1]);
//三维距离
double ans3D =
MySquare(PairCounter[i].Position.x - TrackT[j].Position3D[TrackT[j].NumOFTrackList].x) +
MySquare(PairCounter[i].Position.y - TrackT[j].Position3D[TrackT[j].NumOFTrackList].y) +
MySquare(PairCounter[i].Position.z - TrackT[j].Position3D[TrackT[j].NumOFTrackList].z);
if (ans3D < 0.350 * 0.350)//两帧之间球的运动距离最大为350mm
{
//记录配对
MapPair[NumOfMapPair][0] = i;
MapPair[NumOfMapPair][1] = j;
NumOfMapPair++;
if (NumOfMapPair == MaxTrackList)
{
cout << "NumOfMapPair会越界" << endl;
NumOfMapPair = 0;
//system("pause");
}
UseOfTrackList[j]++;
BelongToList = true;
continue;
}
}
}
if (!BelongToList)//这个点没有一个序列，满足跟踪要求
{
//记录配对
MapPair[NumOfMapPair][0] = i;
MapPair[NumOfMapPair][1] = -1;//-1代表没有找到跟踪序列
NumOfMapPair++;
if (NumOfMapPair == MaxTrackList)
{
cout << "NumOfMapPair会越界" << endl;
NumOfMapPair = 0;
//system("pause");
}
continue;
}
}
//处理配对map***************
for (int i = 0; i < NumOfMapPair; i++)
{
if (MapPair[i][1] == -1)//没有配对，找一个失去生命的序列添加进去，或者重开新的序列
{
//用于判断跟踪队列是否过小
bool AddToTrack = false;
//这里要用所有的资源
for (int j = 0; j < MaxTrackList ; j++)
{
if (!TrackT[j].TrackLife)//没有生命
{
//添加这个点到这个序列
//修改序列的生命
//AddPairToTrackList(&PairCounter[MapPair[i][0]], &TrackT[j], Time);
MyCounterPair *Point = &PairCounter[MapPair[i][0]];
MyTrackStruct * TrackL = &TrackT[j];
int _time = Time;
/////******************************************************************************************************************
{
if (TrackL->TrackLife == 0)//添加点到无生命的跟踪序列
{
//添加点到序列
TrackL->NumOFTrackList = 0;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
TrackL->TrackTime[TrackL->NumOFTrackList] = 0;//第一个点的跟踪时间设为0,相对时间
TrackL->startTrackTime = _time;
TrackL->TrackLife = 2;//这个序列只有一个点，只允许存活到下一帧（后面会减少这个时间）
goto ENDOF_1;
}
if (TrackL->NumOFTrackList == 0)//这个序列只有一个点
{
//添加点到序列
TrackL->NumOFTrackList++;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
TrackL->TrackLife = 2;//这个序列只有一个点，只允许存活到下一帧（后面会减少这个时间）
goto ENDOF_1;
}
if (TrackL->NumOFTrackList == 1)//这个序列有两个点
{
//计算速度分量,可能存在不合理的情况
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[0] == _time - TrackL->startTrackTime)
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[0],
Point->Position,
TrackL->TrackTime[0],
_time - TrackL->startTrackTime,
TrackL->AV);
//物体的运动速度很小，判定为视野中的类球物体，静止
if (MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 3)//速度一定大于3m/s
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_1;
}
//添加点到序列
TrackL->NumOFTrackList++;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
//只有三个点 setkalman
//kalman(TrackL);
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,//注意单位
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,//注意单位
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,//注意单位
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
(TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[1]) * 10,//上一个测量跟踪点，减去GPlist的开始点（测量跟踪点的第二个点）的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_1;
}
//把4定义为时间生命，即由时间决定这个序列的存活，而不是由帧数
TrackL->TrackLife = 4;//这个序列只有三个点，允许存活到下面两帧（后面会减少这个时间）
goto ENDOF_1;
}
//更新序列
if (TrackL->NumOFTrackList >= 2)
{
//添加点到序列
TrackL->NumOFTrackList++;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
// T6-（T8 - T6） = 2*T6 - T8
int detaT = 2 * TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[TrackL->NumOFTrackList];
if (detaT < (double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2)
{
//使用 第0个点 ，上次的跟踪测量点 ，这帧的跟踪点求取速度
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[0] == TrackL->TrackTime[TrackL->NumOFTrackList])
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[0],
TrackL->Position3D[TrackL->NumOFTrackList],
TrackL->TrackTime[0],
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_1;
}
//detaT < (double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2
//选用第0个点，时间上有舍入
int RT = (int)((double)(TrackL->TrackTime[TrackL->NumOFTrackList] - TrackL->TrackTime[0]) / 2 - TrackL->TrackTime[1]);
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT * 10,//上一个测量跟踪点，减去GPlist的开始点（测量跟踪点的第二个点）的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_1;
}
}
if (
(double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2 < detaT
&& detaT <= TrackL->TrackTime[1]
)
{
//使用 第1个点 ，这帧的跟踪点求取速度
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[1] == TrackL->TrackTime[TrackL->NumOFTrackList])
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[1],
TrackL->Position3D[TrackL->NumOFTrackList],
TrackL->TrackTime[1],
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);//bug
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_1;
}
int RT = (int)((double)(TrackL->TrackTime[TrackL->NumOFTrackList] + detaT) / 2 - TrackL->TrackTime[1]);
//由于时间可能对应不准
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT * 10,//转换到GPlist中的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_1;
}
}
//从GPlist内找点做轨迹预测
if (detaT > TrackL->TrackTime[1])
{
//使用 第GPlist中的时间对应点个点 ，上次的跟踪测量点 ，这帧的跟踪点求取速度
//GPList中的下标是
//（图片个采集的绝对时间 - 跟踪开始的时间） = 相对时间
// 相对时间 - 第二个跟踪点的相对时间 = GPlist下标的10倍
int RT_T = (detaT - TrackL->TrackTime[1]) * 10;
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
||TrackL->TrackTime[TrackL->NumOFTrackList] == detaT)
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->GPlist[RT_T][0],
TrackL->GPlist[RT_T][1],
TrackL->GPlist[RT_T][2],
TrackL->Position3D[TrackL->NumOFTrackList].x,
TrackL->Position3D[TrackL->NumOFTrackList].y,
TrackL->Position3D[TrackL->NumOFTrackList].z,
detaT,
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);//bug
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_1;
}
//从上个点对应的时间开始GPList计算
int RT1 = TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[1];
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT1 * 10,
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_1;
}
}
//把4定义为时间生命，即由时间决定这个序列的存活，而不是由帧数
TrackL->TrackLife = 4;//这个序列只有三个点，允许存活到下面两帧（后面会减少这个时间）
}
ENDOF_1:
int a = 0;
}
//增大控制上限
if (ControlOfTrackNum < j)
{
ControlOfTrackNum = j;
}
AddToTrack = true;
break;//跳出循环
}
}
if (!AddToTrack)
{
std::cout << "跟踪队列太小" << endl;
goto KILLTRACK;
//system("pause");
}
}
else
{
if (UseOfTrackList[MapPair[i][1]] > 1)//需要拷贝序列
{
//用于判断跟踪队列是否过小
bool AddToTrack = false;
//拷贝这个序列
//首先找一个没有生命的
for (int k = 0; k < MaxTrackList; k++)
{
if (!TrackT[k].TrackLife)//没有生命
{
//添加这个点到这个序列
CopyTrack(&TrackT[MapPair[i][1]], &TrackT[k]);
//AddPairToTrackList(&PairCounter[MapPair[i][0]], &TrackT[k], Time);
MyCounterPair *Point = &PairCounter[MapPair[i][0]];
MyTrackStruct * TrackL = &TrackT[k];
int _time = Time;
/////******************************************************************************************************************
{
if (TrackL->NumOFTrackList == 0)//这个序列只有一个点
{
//添加点到序列
TrackL->NumOFTrackList = 1;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
TrackL->TrackLife = 2;//这个序列只有一个点，只允许存活到下一帧（后面会减少这个时间）
goto ENDOF_;
}
if (TrackL->NumOFTrackList == 1)//这个序列有两个点
{
//计算速度分量,可能存在不合理的情况
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[0] == _time - TrackL->startTrackTime)
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[0],
Point->Position,
TrackL->TrackTime[0],
_time - TrackL->startTrackTime,
TrackL->AV);
//物体的运动速度很小，判定为视野中的类球物体，静止
if (MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 3)//速度一定大于3m/s
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_;
}
//添加点到序列
TrackL->NumOFTrackList++;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
//只有三个点 setkalman
//kalman(TrackL);
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,//注意单位
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,//注意单位
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,//注意单位
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
(TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[1]) * 10,//上一个测量跟踪点，减去GPlist的开始点（测量跟踪点的第二个点）的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_;
}
//把4定义为时间生命，即由时间决定这个序列的存活，而不是由帧数
TrackL->TrackLife = 4;//这个序列只有三个点，允许存活到下面两帧（后面会减少这个时间）
goto ENDOF_;
}
//更新序列
if (TrackL->NumOFTrackList >= 2)
{
//添加点到序列
TrackL->NumOFTrackList++;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
// T6-（T8 - T6） = 2*T6 - T8
int detaT = 2 * TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[TrackL->NumOFTrackList];
if (detaT < (double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2)
{
//使用 第0个点 ，上次的跟踪测量点 ，这帧的跟踪点求取速度
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[0] == TrackL->TrackTime[TrackL->NumOFTrackList])
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[0],
TrackL->Position3D[TrackL->NumOFTrackList],
TrackL->TrackTime[0],
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_;
}
//detaT < (double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2
//选用第0个点，时间上有舍入
int RT = (int)((double)(TrackL->TrackTime[TrackL->NumOFTrackList] - TrackL->TrackTime[0]) / 2 - TrackL->TrackTime[1]);
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT * 10,//上一个测量跟踪点，减去GPlist的开始点（测量跟踪点的第二个点）的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_;
}
}
if (
(double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2 < detaT
&& detaT <= TrackL->TrackTime[1]
)
{
//使用 第1个点 ，这帧的跟踪点求取速度
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[1] == TrackL->TrackTime[TrackL->NumOFTrackList])
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[1],
TrackL->Position3D[TrackL->NumOFTrackList],
TrackL->TrackTime[1],
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);//bug
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_;
}
int RT = (int)((double)(TrackL->TrackTime[TrackL->NumOFTrackList] + detaT) / 2 - TrackL->TrackTime[1]);
//由于时间可能对应不准
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT * 10,//转换到GPlist中的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_;
}
}
//从GPlist内找点做轨迹预测
if (detaT > TrackL->TrackTime[1])
{
//使用 第GPlist中的时间对应点个点 ，上次的跟踪测量点 ，这帧的跟踪点求取速度
//GPList中的下标是
//（图片个采集的绝对时间 - 跟踪开始的时间） = 相对时间
// 相对时间 - 第二个跟踪点的相对时间 = GPlist下标的10倍
int RT_T = (detaT - TrackL->TrackTime[1]) * 10;
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
|| (TrackL->TrackTime[TrackL->NumOFTrackList]) == detaT)
{
cout << "TrackL->NumOFTrackList**********************************" << endl;//3
}
getV(
TrackL->GPlist[RT_T][0],
TrackL->GPlist[RT_T][1],
TrackL->GPlist[RT_T][2],
TrackL->Position3D[TrackL->NumOFTrackList].x,
TrackL->Position3D[TrackL->NumOFTrackList].y,
TrackL->Position3D[TrackL->NumOFTrackList].z,
detaT,
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);//bug
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_;
}
//从上个点对应的时间开始GPList计算
int RT1 = TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[1];
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT1 * 10,
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF_;
}
}
//把4定义为时间生命，即由时间决定这个序列的存活，而不是由帧数
TrackL->TrackLife = 4;//这个序列只有三个点，允许存活到下面两帧（后面会减少这个时间）
}
ENDOF_:
int a = 0;
}
//增大控制上限
if (ControlOfTrackNum < k)
{
ControlOfTrackNum = k;
}
AddToTrack = true;
break;//跳出循环
}
}
if (!AddToTrack)
{
std::cout << "跟踪队列太小" << endl;
goto KILLTRACK;
}
//整个跟踪中所有序列都有效，则需要增加新的序列
//把这个点，添加到拷贝的序列中
UseOfTrackList[MapPair[i][1]]--;//便于下次判断是否需要拷贝
}
else
{
//添加这个点到这个序列
//AddPairToTrackList(&PairCounter[MapPair[i][0]], &TrackT[MapPair[i][1]], Time);
//aaa(&PairCounter[MapPair[i][0]], &TrackT[MapPair[i][1]], Time);
MyCounterPair *Point = &PairCounter[MapPair[i][0]];
MyTrackStruct * TrackL = &TrackT[MapPair[i][1]];
int _time = Time;
/////******************************************************************************************************************
{
if (TrackL->TrackLife == 0)//添加点到无生命的跟踪序列
{
//添加点到序列
TrackL->NumOFTrackList = 0;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
TrackL->TrackTime[TrackL->NumOFTrackList] = 0;//第一个点的跟踪时间设为0,相对时间
TrackL->startTrackTime = _time;
TrackL->TrackLife = 2;//这个序列只有一个点，只允许存活到下一帧（后面会减少这个时间）
goto ENDOF;
}
if (TrackL->NumOFTrackList == 0)//这个序列只有一个点
{
//添加点到序列
TrackL->NumOFTrackList++;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
TrackL->TrackLife = 2;//这个序列只有一个点，只允许存活到下一帧（后面会减少这个时间）
goto ENDOF;
}
if (TrackL->NumOFTrackList == 1)//这个序列有两个点
{
//计算速度分量,可能存在不合理的情况
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[0] == _time - TrackL->startTrackTime)
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[0],
Point->Position,
TrackL->TrackTime[0],
_time - TrackL->startTrackTime,
TrackL->AV);
//物体的运动速度很小，判定为视野中的类球物体，静止
if (MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 3)//速度一定大于3m/s
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF;
}
//添加点到序列
TrackL->NumOFTrackList++;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
//只有三个点 setkalman
//kalman(TrackL);
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,//注意单位
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,//注意单位
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,//注意单位
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
(TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[1]) * 10,//上一个测量跟踪点，减去GPlist的开始点（测量跟踪点的第二个点）的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF;
}
//把4定义为时间生命，即由时间决定这个序列的存活，而不是由帧数
TrackL->TrackLife = 4;//这个序列只有三个点，允许存活到下面两帧（后面会减少这个时间）
goto ENDOF;
}
//更新序列
if (TrackL->NumOFTrackList >= 2)
{
//添加点到序列
TrackL->NumOFTrackList++;
TrackL->PointList_Left[TrackL->NumOFTrackList][0] = int(Point->Left->x);
TrackL->PointList_Left[TrackL->NumOFTrackList][1] = int(Point->Left->y);
TrackL->PointList_Right[TrackL->NumOFTrackList][0] = int(Point->Right->x);
TrackL->PointList_Right[TrackL->NumOFTrackList][1] = int(Point->Right->y);
TrackL->Position3D[TrackL->NumOFTrackList] = Point->Position;
//保存相对时间
TrackL->TrackTime[TrackL->NumOFTrackList] = _time - TrackL->startTrackTime;
// T6-（T8 - T6） = 2*T6 - T8
int detaT = 2 * TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[TrackL->NumOFTrackList];
if (detaT < (double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2)
{
//使用 第0个点 ，上次的跟踪测量点 ，这帧的跟踪点求取速度
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[0] == TrackL->TrackTime[TrackL->NumOFTrackList])
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[0],
TrackL->Position3D[TrackL->NumOFTrackList],
TrackL->TrackTime[0],
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF;
}
//detaT < (double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2
//选用第0个点，时间上有舍入
int RT = (int)((double)(TrackL->TrackTime[TrackL->NumOFTrackList] - TrackL->TrackTime[0]) / 2 - TrackL->TrackTime[1]);
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT * 10,//上一个测量跟踪点，减去GPlist的开始点（测量跟踪点的第二个点）的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF;
}
}
if (
(double)(TrackL->TrackTime[1] - TrackL->TrackTime[0]) / 2 < detaT
&& detaT <= TrackL->TrackTime[1]
)
{
//使用 第1个点 ，这帧的跟踪点求取速度
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[1] == TrackL->TrackTime[TrackL->NumOFTrackList])
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->Position3D[1],
TrackL->Position3D[TrackL->NumOFTrackList],
TrackL->TrackTime[1],
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);//bug
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF;
}
int RT = (int)((double)(TrackL->TrackTime[TrackL->NumOFTrackList] + detaT) / 2 - TrackL->TrackTime[1]);
//由于时间可能对应不准
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT * 10,//转换到GPlist中的时间
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF;
}
}
//从GPlist内找点做轨迹预测
if (detaT > TrackL->TrackTime[1])
{
//使用 第GPlist中的时间对应点个点 ，上次的跟踪测量点 ，这帧的跟踪点求取速度
//GPList中的下标是
//（图片个采集的绝对时间 - 跟踪开始的时间） = 相对时间
// 相对时间 - 第二个跟踪点的相对时间 = GPlist下标的10倍
int RT_T = (detaT - TrackL->TrackTime[1]) * 10;
//并预测轨迹
//计算速度分量
if (abs(TrackL->NumOFTrackList) > 10000
|| TrackL->TrackTime[TrackL->NumOFTrackList] == detaT)
{
cout << "TrackL->NumOFTrackList**********************************" << endl;
}
getV(
TrackL->GPlist[RT_T][0],
TrackL->GPlist[RT_T][1],
TrackL->GPlist[RT_T][2],
TrackL->Position3D[TrackL->NumOFTrackList].x,
TrackL->Position3D[TrackL->NumOFTrackList].y,
TrackL->Position3D[TrackL->NumOFTrackList].z,
detaT,
TrackL->TrackTime[TrackL->NumOFTrackList],
TrackL->AV);//bug
//物体的运动速度很小，判定为视野中的类球物体，静止
if (
MySquare(TrackL->AV[0]) + MySquare(TrackL->AV[1]) + MySquare(TrackL->AV[2])< 4
)
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF;
}
//从上个点对应的时间开始GPList计算
int RT1 = TrackL->TrackTime[TrackL->NumOFTrackList - 1] - TrackL->TrackTime[1];
if (!GuessPos(
TrackL->Position3D[TrackL->NumOFTrackList - 1].x,
TrackL->Position3D[TrackL->NumOFTrackList - 1].y,
TrackL->Position3D[TrackL->NumOFTrackList - 1].z,
TrackL->AV[0],
TrackL->AV[1],
TrackL->AV[2],
3,
Rex,
100000,
RT1 * 10,
TrackL->GPlist,
&TrackL->P_Num,
TrackL->NumOFTrackList
)
)//参数跟踪失败，循环球落地超过5s，这个序列错误
{
TrackL->TrackLife = 0;//跟踪终止
goto ENDOF;
}
}
//把4定义为时间生命，即由时间决定这个序列的存活，而不是由帧数
TrackL->TrackLife = 4;//这个序列只有三个点，允许存活到下面两帧（后面会减少这个时间）
}
ENDOF:
int a=0;
}
}
}
}
KILLTRACK:
//生命损耗***************
//由大到小减少 ，便于减少ControlOfTrackNum
for (int j = ControlOfTrackNum; j >= 0; j--)
{
//这条语句必须有
if (TrackT[j].TrackLife == 0)
continue;
if (TrackT[j].TrackLife == 4)//采用时间生命
{
if (Time - TrackT[j].startTrackTime > 3000)//这个序列存活超过3s了
{
TrackT[j].TrackLife = 0;
if (j == ControlOfTrackNum)//最后一个没有生命值，实际跟踪队列使用减少1
{
ControlOfTrackNum--;
}
continue;
}
if (TrackT[j].NumOFTrackList > 3000)
{
cout << endl;
}
if (Time - (TrackT[j].TrackTime[TrackT[j].NumOFTrackList] + TrackT[j].startTrackTime)> 300)//这个序列有0.3s没有新的点加入了
{
TrackT[j].TrackLife = 0;
if (j == ControlOfTrackNum)//最后一个没有生命值，实际跟踪队列使用减少1
{
ControlOfTrackNum--;
}
continue;
}
}
else
{
TrackT[j].TrackLife -= 1;//生命损耗
if (!TrackT[j].TrackLife)//没有生命
{
if (j == ControlOfTrackNum)//最后一个没有生命值，实际跟踪队列使用减少1
{
ControlOfTrackNum--;
}
}
}
}
//发送跟踪显示消息
NEXT->PostThreadMessage(WM_SHOW_TRACK_THREADMESSAGE, WPARAM(ControlOfTrackNum), NULL);
//置位配对标志位
To_Handle_PairCounter = false;
if ((GETTIME() - Time_Temp) > 10)
{
cout << "跟踪线程时间危险" << endl;
//system("pause");
}
return;
}
int udps_wait_function(struct sender_tracking *st, struct opt_s* opt)
{
long wait;
#if(PREEMPTKERNEL)
int err;
#endif
#ifdef HAVE_RATELIMITER
if(opt->wait_nanoseconds > 0)
{
//clock_gettime(CLOCK_REALTIME, &now);
GETTIME(st->now);
#if(SEND_DEBUG)
COPYTIME(st->now,st->reference);
#endif
wait = nanodiff(&(opt->wait_last_sent), &st->now);
#if(SEND_DEBUG)
#if(PLOTTABLE_SEND_DEBUG)
//fprintf(st->out,"%ld %ld \n",spec_ops->total_captured_packets, wait);
#else
fprintf(st->out, "UDP_STREAMER: %ld ns has passed since last->send\n", wait);
#endif
#endif
ZEROTIME(st->req);
SETNANOS(st->req,(opt->wait_nanoseconds-wait));
if(GETNANOS(st->req) > 0){
#if(SEND_DEBUG)
#if(PLOTTABLE_SEND_DEBUG)
fprintf(st->out, "%ld ", GETNANOS(st->req));
#else
fprintf(st->out, "UDP_STREAMER: Sleeping %ld ns before sending packet\n", GETNANOS(st->req));
#endif
#endif
#if!(PREEMPTKERNEL)
/* First->sleep in minsleep sleeps to get rid of the bulk */
while((unsigned long)GETNANOS(st->req) > st->minsleep){
SLEEP_NANOS(st->onenano);
SETNANOS(st->req,GETNANOS(st->req)-st->minsleep);
}
GETTIME(st->now);
while(nanodiff(&(opt->wait_last_sent),&st->now) < opt->wait_nanoseconds){
GETTIME(st->now);
}
#else
err = SLEEP_NANOS(st->req);
if(err != 0){
E("cant sleep");
return -1;
}
GETTIME(st->now);
#endif /*PREEMPTKERNEL */
#if(SEND_DEBUG)
#if(PLOTTABLE_SEND_DEBUG)
fprintf(st->out, "%ld\n", nanodiff(&st->reference, &st->now));
#else
fprintf(st->out, "UDP_STREAMER: Really slept %lu\n", nanodiff(&st->reference, &st->now));
#endif
#endif
nanoadd(&(opt->wait_last_sent), opt->wait_nanoseconds);
}
else{
#if(SEND_DEBUG)
#if(PLOTTABLE_SEND_DEBUG)
fprintf(st->out, "0 0\n");
#else
fprintf(st->out, "Runaway timer! Resetting\n");
#endif
#endif
COPYTIME(st->now,opt->wait_last_sent);
}
}
#endif //HAVE_RATELIMITER
return 0;
}
int generic_sendloop(struct streamer_entity * se, int do_wait, int(*sendcmd)(struct streamer_entity*, struct sender_tracking*), void(*buffer_boundary)(struct streamer_entity*, struct sender_tracking*, unsigned long **))
{
int err = 0;
struct socketopts *spec_ops = (struct socketopts *)se->opt;
struct sender_tracking st;
unsigned long * counter;
init_sender_tracking(spec_ops->opt, &st);
///if(do_wait == 1)
throttling_count(spec_ops->opt, &st);
se->be = NULL;
spec_ops->total_transacted_bytes = 0;
//spec_ops->total_captured_packets = 0;
spec_ops->out_of_order = 0;
spec_ops->incomplete = 0;
spec_ops->missing = 0;
D("Getting first loaded buffer for sender");
spec_ops->inc = &st.inc;
/* Data won't be instantaneous so get min_sleep here! */
if(do_wait==1){
unsigned long minsleep = get_min_sleeptime();
LOG("Can sleep max %lu microseconds on average\n", minsleep);
#if!(PREEMPTKERNEL)
st.minsleep = minsleep;
#endif
}
/*
jump_to_next_file(spec_ops->opt, se, &st);
CHECK_AND_EXIT(se->be);
st.buf = se->be->simple_get_writebuf(se->be, NULL);
D("Starting stream send");
*/
LOG("GENERIC_SENDER: Starting stream send\n");
if(do_wait == 1)
GETTIME(spec_ops->opt->wait_last_sent);
while(should_i_be_running(spec_ops->opt, &st) == 1){
err = jump_to_next_file(spec_ops->opt, se, &st);
if(err == ALL_DONE){
D("Jump to next file returned all done");
break;
}
else if (err < 0){
E("Error in getting buffer");
UDPS_EXIT_ERROR;
}
CHECK_AND_EXIT(se->be);
se->be->simple_get_writebuf(se->be, NULL);
*(spec_ops->inc) = 0;
buffer_boundary(se,&st,&counter);
while(*counter > 0)
{
if(do_wait==1)
udps_wait_function(&st, spec_ops->opt);
err = sendcmd(se, &st);
if(err != 0){
E("Error in sendcmd");
UDPS_EXIT_ERROR;
}
}
}
UDPS_EXIT;
}
void SoftHEVC::onQueueFilled(OMX_U32 portIndex) {
UNUSED(portIndex);
if (mOutputPortSettingsChange != NONE) {
return;
}
List<BufferInfo *> &inQueue = getPortQueue(kInputPortIndex);
List<BufferInfo *> &outQueue = getPortQueue(kOutputPortIndex);
/* If input EOS is seen and decoder is not in flush mode,
* set the decoder in flush mode.
* There can be a case where EOS is sent along with last picture data
* In that case, only after decoding that input data, decoder has to be
* put in flush. This case is handled here */
if (mReceivedEOS && !mIsInFlush) {
setFlushMode();
}
while (!outQueue.empty()) {
BufferInfo *inInfo;
OMX_BUFFERHEADERTYPE *inHeader;
BufferInfo *outInfo;
OMX_BUFFERHEADERTYPE *outHeader;
size_t timeStampIx;
inInfo = NULL;
inHeader = NULL;
if (!mIsInFlush) {
if (!inQueue.empty()) {
inInfo = *inQueue.begin();
inHeader = inInfo->mHeader;
} else {
break;
}
}
outInfo = *outQueue.begin();
outHeader = outInfo->mHeader;
outHeader->nFlags = 0;
outHeader->nTimeStamp = 0;
outHeader->nOffset = 0;
if (inHeader != NULL && (inHeader->nFlags & OMX_BUFFERFLAG_EOS)) {
ALOGD("EOS seen on input");
mReceivedEOS = true;
if (inHeader->nFilledLen == 0) {
inQueue.erase(inQueue.begin());
inInfo->mOwnedByUs = false;
notifyEmptyBufferDone(inHeader);
inHeader = NULL;
setFlushMode();
}
}
// When there is an init required and the decoder is not in flush mode,
// update output port's definition and reinitialize decoder.
if (mInitNeeded && !mIsInFlush) {
bool portWillReset = false;
handlePortSettingsChange(&portWillReset, mNewWidth, mNewHeight);
CHECK_EQ(reInitDecoder(), (status_t)OK);
return;
}
/* Get a free slot in timestamp array to hold input timestamp */
{
size_t i;
timeStampIx = 0;
for (i = 0; i < MAX_TIME_STAMPS; i++) {
if (!mTimeStampsValid[i]) {
timeStampIx = i;
break;
}
}
if (inHeader != NULL) {
mTimeStampsValid[timeStampIx] = true;
mTimeStamps[timeStampIx] = inHeader->nTimeStamp;
}
}
{
ivd_video_decode_ip_t s_dec_ip;
ivd_video_decode_op_t s_dec_op;
WORD32 timeDelay, timeTaken;
size_t sizeY, sizeUV;
setDecodeArgs(&s_dec_ip, &s_dec_op, inHeader, outHeader, timeStampIx);
GETTIME(&mTimeStart, NULL);
/* Compute time elapsed between end of previous decode()
* to start of current decode() */
TIME_DIFF(mTimeEnd, mTimeStart, timeDelay);
IV_API_CALL_STATUS_T status;
status = ivdec_api_function(mCodecCtx, (void *)&s_dec_ip, (void *)&s_dec_op);
//Fixed
if ( mWidth != s_dec_op.u4_pic_wd || mHeight != s_dec_op.u4_pic_ht) {
ALOGE("mWidth %d, mHeight %d -> mNewWidth %d, mNewHeight %d", mWidth, mHeight, mNewWidth, mNewHeight);
if(mFlushOutBuffer) {
ivd_aligned_free(mFlushOutBuffer);
mFlushOutBuffer = NULL;
}
uint32_t bufferSize = s_dec_op.u4_pic_wd * s_dec_op.u4_pic_ht * 3 / 2;
mFlushOutBuffer = (uint8_t *)ivd_aligned_malloc(128, bufferSize);
if (NULL == mFlushOutBuffer) {
ALOGE("Could not allocate flushOutputBuffer of size %zu", bufferSize);
return;
}
ALOGE("re-alloc mFlushOutBuffer");
}
// FIXME: Compare |status| to IHEVCD_UNSUPPORTED_DIMENSIONS, which is not one of the
// IV_API_CALL_STATUS_T, seems be wrong. But this is what the decoder returns right now.
// The decoder should be fixed so that |u4_error_code| instead of |status| returns
// IHEVCD_UNSUPPORTED_DIMENSIONS.
bool unsupportedDimensions =
((IHEVCD_UNSUPPORTED_DIMENSIONS == status)
|| (IHEVCD_UNSUPPORTED_DIMENSIONS == s_dec_op.u4_error_code));
bool resChanged = (IVD_RES_CHANGED == (s_dec_op.u4_error_code & 0xFF));
GETTIME(&mTimeEnd, NULL);
/* Compute time taken for decode() */
TIME_DIFF(mTimeStart, mTimeEnd, timeTaken);
ALOGV("timeTaken=%6d delay=%6d numBytes=%6d", timeTaken, timeDelay,
s_dec_op.u4_num_bytes_consumed);
if (s_dec_op.u4_frame_decoded_flag && !mFlushNeeded) {
mFlushNeeded = true;
}
if ((inHeader != NULL) && (1 != s_dec_op.u4_frame_decoded_flag)) {
/* If the input did not contain picture data, then ignore
* the associated timestamp */
mTimeStampsValid[timeStampIx] = false;
}
// This is needed to handle CTS DecoderTest testCodecResetsHEVCWithoutSurface,
// which is not sending SPS/PPS after port reconfiguration and flush to the codec.
if (unsupportedDimensions && !mFlushNeeded) {
bool portWillReset = false;
handlePortSettingsChange(&portWillReset, s_dec_op.u4_pic_wd, s_dec_op.u4_pic_ht);
CHECK_EQ(reInitDecoder(), (status_t)OK);
setDecodeArgs(&s_dec_ip, &s_dec_op, inHeader, outHeader, timeStampIx);
ivdec_api_function(mCodecCtx, (void *)&s_dec_ip, (void *)&s_dec_op);
return;
}
// If the decoder is in the changing resolution mode and there is no output present,
// that means the switching is done and it's ready to reset the decoder and the plugin.
if (mChangingResolution && !s_dec_op.u4_output_present) {
mChangingResolution = false;
resetDecoder();
resetPlugin();
continue;
}
if (unsupportedDimensions || resChanged) {
mChangingResolution = true;
if (mFlushNeeded) {
setFlushMode();
}
if (unsupportedDimensions) {
mNewWidth = s_dec_op.u4_pic_wd;
mNewHeight = s_dec_op.u4_pic_ht;
mInitNeeded = true;
}
continue;
}
if ((0 < s_dec_op.u4_pic_wd) && (0 < s_dec_op.u4_pic_ht)) {
uint32_t width = s_dec_op.u4_pic_wd;
uint32_t height = s_dec_op.u4_pic_ht;
bool portWillReset = false;
handlePortSettingsChange(&portWillReset, width, height);
if (portWillReset) {
resetDecoder();
return;
}
}
if (s_dec_op.u4_output_present) {
outHeader->nFilledLen = (mWidth * mHeight * 3) / 2;
outHeader->nTimeStamp = mTimeStamps[s_dec_op.u4_ts];
mTimeStampsValid[s_dec_op.u4_ts] = false;
outInfo->mOwnedByUs = false;
outQueue.erase(outQueue.begin());
outInfo = NULL;
notifyFillBufferDone(outHeader);
outHeader = NULL;
} else {
/* If in flush mode and no output is returned by the codec,
* then come out of flush mode */
mIsInFlush = false;
/* If EOS was recieved on input port and there is no output
* from the codec, then signal EOS on output port */
if (mReceivedEOS) {
outHeader->nFilledLen = 0;
outHeader->nFlags |= OMX_BUFFERFLAG_EOS;
outInfo->mOwnedByUs = false;
outQueue.erase(outQueue.begin());
outInfo = NULL;
notifyFillBufferDone(outHeader);
outHeader = NULL;
resetPlugin();
}
}
}
// TODO: Handle more than one picture data
if (inHeader != NULL) {
inInfo->mOwnedByUs = false;
inQueue.erase(inQueue.begin());
inInfo = NULL;
notifyEmptyBufferDone(inHeader);
inHeader = NULL;
}
}
}
void SoftMPEG2::onQueueFilled(OMX_U32 portIndex) {
UNUSED(portIndex);
if (mOutputPortSettingsChange != NONE) {
return;
}
List<BufferInfo *> &inQueue = getPortQueue(kInputPortIndex);
List<BufferInfo *> &outQueue = getPortQueue(kOutputPortIndex);
/* If input EOS is seen and decoder is not in flush mode,
* set the decoder in flush mode.
* There can be a case where EOS is sent along with last picture data
* In that case, only after decoding that input data, decoder has to be
* put in flush. This case is handled here */
if (mReceivedEOS && !mIsInFlush) {
setFlushMode();
}
while (!outQueue.empty()) {
BufferInfo *inInfo;
OMX_BUFFERHEADERTYPE *inHeader;
BufferInfo *outInfo;
OMX_BUFFERHEADERTYPE *outHeader;
size_t timeStampIx;
inInfo = NULL;
inHeader = NULL;
if (!mIsInFlush) {
if (!inQueue.empty()) {
inInfo = *inQueue.begin();
inHeader = inInfo->mHeader;
} else {
break;
}
}
outInfo = *outQueue.begin();
outHeader = outInfo->mHeader;
outHeader->nFlags = 0;
outHeader->nTimeStamp = 0;
outHeader->nOffset = 0;
if (inHeader != NULL && (inHeader->nFlags & OMX_BUFFERFLAG_EOS)) {
mReceivedEOS = true;
if (inHeader->nFilledLen == 0) {
inQueue.erase(inQueue.begin());
inInfo->mOwnedByUs = false;
notifyEmptyBufferDone(inHeader);
inHeader = NULL;
setFlushMode();
}
}
// When there is an init required and the decoder is not in flush mode,
// update output port's definition and reinitialize decoder.
if (mInitNeeded && !mIsInFlush) {
bool portWillReset = false;
handlePortSettingsChange(&portWillReset, mNewWidth, mNewHeight);
CHECK_EQ(reInitDecoder(), (status_t)OK);
return;
}
/* Get a free slot in timestamp array to hold input timestamp */
{
size_t i;
timeStampIx = 0;
for (i = 0; i < MAX_TIME_STAMPS; i++) {
if (!mTimeStampsValid[i]) {
timeStampIx = i;
break;
}
}
if (inHeader != NULL) {
mTimeStampsValid[timeStampIx] = true;
mTimeStamps[timeStampIx] = inHeader->nTimeStamp;
}
}
{
ivd_video_decode_ip_t s_dec_ip;
ivd_video_decode_op_t s_dec_op;
WORD32 timeDelay, timeTaken;
size_t sizeY, sizeUV;
setDecodeArgs(&s_dec_ip, &s_dec_op, inHeader, outHeader, timeStampIx);
// If input dump is enabled, then write to file
DUMP_TO_FILE(mInFile, s_dec_ip.pv_stream_buffer, s_dec_ip.u4_num_Bytes);
if (s_dec_ip.u4_num_Bytes > 0) {
char *ptr = (char *)s_dec_ip.pv_stream_buffer;
}
GETTIME(&mTimeStart, NULL);
/* Compute time elapsed between end of previous decode()
* to start of current decode() */
TIME_DIFF(mTimeEnd, mTimeStart, timeDelay);
IV_API_CALL_STATUS_T status;
status = ivdec_api_function(mCodecCtx, (void *)&s_dec_ip, (void *)&s_dec_op);
bool unsupportedDimensions = (IMPEG2D_UNSUPPORTED_DIMENSIONS == s_dec_op.u4_error_code);
bool resChanged = (IVD_RES_CHANGED == (s_dec_op.u4_error_code & 0xFF));
GETTIME(&mTimeEnd, NULL);
/* Compute time taken for decode() */
TIME_DIFF(mTimeStart, mTimeEnd, timeTaken);
ALOGV("timeTaken=%6d delay=%6d numBytes=%6d", timeTaken, timeDelay,
s_dec_op.u4_num_bytes_consumed);
if (s_dec_op.u4_frame_decoded_flag && !mFlushNeeded) {
mFlushNeeded = true;
}
if ((inHeader != NULL) && (1 != s_dec_op.u4_frame_decoded_flag)) {
/* If the input did not contain picture data, then ignore
* the associated timestamp */
mTimeStampsValid[timeStampIx] = false;
}
// This is needed to handle CTS DecoderTest testCodecResetsMPEG2WithoutSurface,
// which is not sending SPS/PPS after port reconfiguration and flush to the codec.
if (unsupportedDimensions && !mFlushNeeded) {
bool portWillReset = false;
handlePortSettingsChange(&portWillReset, s_dec_op.u4_pic_wd, s_dec_op.u4_pic_ht);
CHECK_EQ(reInitDecoder(), (status_t)OK);
setDecodeArgs(&s_dec_ip, &s_dec_op, inHeader, outHeader, timeStampIx);
ivdec_api_function(mCodecCtx, (void *)&s_dec_ip, (void *)&s_dec_op);
return;
}
// If the decoder is in the changing resolution mode and there is no output present,
// that means the switching is done and it's ready to reset the decoder and the plugin.
if (mChangingResolution && !s_dec_op.u4_output_present) {
mChangingResolution = false;
resetDecoder();
resetPlugin();
continue;
}
if (unsupportedDimensions || resChanged) {
mChangingResolution = true;
if (mFlushNeeded) {
setFlushMode();
}
if (unsupportedDimensions) {
mNewWidth = s_dec_op.u4_pic_wd;
mNewHeight = s_dec_op.u4_pic_ht;
mInitNeeded = true;
}
continue;
}
if ((0 < s_dec_op.u4_pic_wd) && (0 < s_dec_op.u4_pic_ht)) {
uint32_t width = s_dec_op.u4_pic_wd;
uint32_t height = s_dec_op.u4_pic_ht;
bool portWillReset = false;
handlePortSettingsChange(&portWillReset, width, height);
if (portWillReset) {
resetDecoder();
return;
}
}
if (s_dec_op.u4_output_present) {
size_t timeStampIdx;
outHeader->nFilledLen = (mWidth * mHeight * 3) / 2;
timeStampIdx = getMinTimestampIdx(mTimeStamps, mTimeStampsValid);
outHeader->nTimeStamp = mTimeStamps[timeStampIdx];
mTimeStampsValid[timeStampIdx] = false;
/* mWaitForI waits for the first I picture. Once made FALSE, it
has to remain false till explicitly set to TRUE. */
mWaitForI = mWaitForI && !(IV_I_FRAME == s_dec_op.e_pic_type);
if (mWaitForI) {
s_dec_op.u4_output_present = false;
} else {
ALOGV("Output timestamp: %lld, res: %ux%u",
(long long)outHeader->nTimeStamp, mWidth, mHeight);
DUMP_TO_FILE(mOutFile, outHeader->pBuffer, outHeader->nFilledLen);
outInfo->mOwnedByUs = false;
outQueue.erase(outQueue.begin());
outInfo = NULL;
notifyFillBufferDone(outHeader);
outHeader = NULL;
}
} else {
/* If in flush mode and no output is returned by the codec,
* then come out of flush mode */
mIsInFlush = false;
/* If EOS was recieved on input port and there is no output
* from the codec, then signal EOS on output port */
if (mReceivedEOS) {
outHeader->nFilledLen = 0;
outHeader->nFlags |= OMX_BUFFERFLAG_EOS;
outInfo->mOwnedByUs = false;
outQueue.erase(outQueue.begin());
outInfo = NULL;
notifyFillBufferDone(outHeader);
outHeader = NULL;
resetPlugin();
}
}
}
// TODO: Handle more than one picture data
if (inHeader != NULL) {
inInfo->mOwnedByUs = false;
inQueue.erase(inQueue.begin());
inInfo = NULL;
notifyEmptyBufferDone(inHeader);
inHeader = NULL;
}
}
}
DWORD WINAPI ContrlOfABCarThread(LPVOID p_lpVoid)
{
int ASeriportNum = 5;
int BSeriportNum = 2;
AKinectSerialPort.m_SerialPortType = KinectCar;
AKinectSerialPort.InitSuccess = false;
cout << "AKinectSerialPort 初始化" << ASeriportNum << endl;
if (AKinectSerialPort.InitPort(ASeriportNum, 115200))
{
cout << "AKinectSerialPort 初始化成功！" << endl;
if (AKinectSerialPort.OpenListenThread())
{
cout << "AKinectSerialPort 打开监听线程成功！" << endl;
if (AKinectSerialPort.OpenSendThread())
{
cout << "AKinectSerialPort 打开发送线程成功！" << endl;
}
else
{
cout << "AKinectSerialPort 打开发送线程失败**********************！" << endl;
}
}
else
{
cout << "AKinectSerialPort 打开监听线程失败*******************！" << endl;
}
}
else
{
cout << "AKinectSerialPort 初始化失败******************！" << endl;
}
AKinectSerialPort.InitSuccess = true;
/***********************************************************************/
BKinectSerialPort.m_SerialPortType = KinectCar;
BKinectSerialPort.InitSuccess = false;
cout << "BKinectSerialPort 初始化" << BSeriportNum << endl;
if (BKinectSerialPort.InitPort(BSeriportNum, 115200))
{
cout << "BKinectSerialPort 初始化成功！" << endl;
if (BKinectSerialPort.OpenListenThread())
{
cout << "BKinectSerialPort 打开监听线程成功！" << endl;
if (BKinectSerialPort.OpenSendThread())
{
cout << "BKinectSerialPort 打开发送线程成功！" << endl;
}
else
{
cout << "BKinectSerialPort 打开发送线程失败**********************！" << endl;
}
}
else
{
cout << "BKinectSerialPort 打开监听线程失败*******************！" << endl;
}
}
else
{
cout << "BKinectSerialPort 初始化失败******************！" << endl;
}
BKinectSerialPort.InitSuccess = true;
/***********************************************************************/
BKinectSerialPort.x = 6000;
BKinectSerialPort.y = 0;
BKinectSerialPort.x = -6000;
BKinectSerialPort.y = 0;
AKinectSerialPort.m_SerialPortType = KinectCar;
BKinectSerialPort.m_SerialPortType = UnKnow;//定义为 UnKnow ，可以在接收程序中分别AB，接收m_StateOfAB
//BKinectSerialPort.x = 2570;
//BKinectSerialPort.y = 0;
double Time;
//只管给双目，B车发送位置
while (1)
{
Time = GETTIME();
//决策机正常运行
if (Time - AKinectSerialPort.GetPositionTime < KeepConditionTime)
{
AKinectSerialPort.InitSuccess = true;
}
else
{
AKinectSerialPort.InitSuccess = false;
Sleep(100);
//cout << "A" ;
}
//决策机正常运行
if (Time - BKinectSerialPort.GetPositionTime < KeepConditionTime)
{
BKinectSerialPort.InitSuccess = true;
}
else
{
BKinectSerialPort.InitSuccess = false;
Sleep(100);
//cout << "B";
}
Sleep(30);
}
return 0;
}
//图片处理函数
void CImageHandleThread_R::ImageHandleThread_R_Message(WPARAM wParam, LPARAM lParam)
{
////记录图片的处理时间
//int Time_Temp = (int)(GETTIME() + 0.5);
static int Nframe = -1;
static int T_OF_TRACK = 0;
void *data = (void *)wParam;
static int NFrame = 1;
static IplImage* Image1 = cvCreateImage(cvSize(640, 480), 8, 1);
static IplImage* Image2 = cvCreateImage(cvSize(640, 480), 8, 1);
static IplImage* Image3 = cvCreateImage(cvSize(640, 480), 8, 1);
static IplImage* Imask1 = cvCreateImage(cvSize(640, 480), 8, 1);
static IplImage* Imask2 = cvCreateImage(cvSize(640, 480), 8, 1);
static IplImage* Result = cvCreateImage(cvSize(640, 480), 8, 1);
static IplImage* Temp;//用于转换储存地址，省去拷贝。
if (NFrame == 1)
{
memcpy(Image1->imageData, data, 640 * 480);
NFrame++;
}
else
{
if (NFrame == 2)
{
memcpy(Image2->imageData, data, 640 * 480);
cvAbsDiff(Image1, Image2, Imask1);
cvThreshold(Imask1, Imask1, BinaryTherod, 255, CV_THRESH_BINARY);
NFrame++;
}
else
{
memcpy(Image3->imageData, data, 640 * 480);
cvAbsDiff(Image2, Image3, Imask2);
cvThreshold(Imask2, Imask2, BinaryTherod, 255, CV_THRESH_BINARY);
cvAnd(Imask1, Imask2, Src_Right_Gray[T_OF_TRACK]);//会修改内存中的值
while (To_Handle_Right[T_OF_TRACK] == true)
{
cout << "R_配对线程处理不过来" << endl;
Sleep(1);
//system("pause");
}
//存时间
//时间也得写在处理完之后！！！
//Time_R[T_OF_TRACK] = int(lParam);
Time_R[T_OF_TRACK] = GETTIME();
//cvDilate(Src_Right_Gray[T_OF_TRACK], Src_Right_Gray[T_OF_TRACK]);
//cvDilate(Src_Right_Gray[T_OF_TRACK], Src_Right_Gray[T_OF_TRACK]);
//cvErode(Src_Right_Gray[T_OF_TRACK], Src_Right_Gray[T_OF_TRACK]);
//cvErode(Src_Right_Gray[T_OF_TRACK], Src_Right_Gray[T_OF_TRACK]);
////用2x2模板腐蚀，用3x3模板膨胀 可行
//static int Mask[4] = { 1, 1, 1, 1 };
//static IplConvKernel* THelementstatic = cvCreateStructuringElementEx(2, 2, 0, 0, CV_SHAPE_RECT, Mask);
//cvErode(Src_Right_Gray[T_OF_TRACK], Src_Right_Gray[T_OF_TRACK], THelementstatic);
//cvDilate(Src_Right_Gray[T_OF_TRACK], Src_Right_Gray[T_OF_TRACK]);
PointDensity_R(Src_Right_Gray[T_OF_TRACK], PointDensityTherod);
//一定要注意使用不同的函数找连通域
FindTheCounter_define_R(Src_Right_Gray[T_OF_TRACK], TheCounter_Right[T_OF_TRACK], &NumOfCounter_Right[T_OF_TRACK], KofCounter, MinCounter, MaxCounter, MaxCounterNum, GIRTHAREA);//9m的时候，球只有 6-7个像素点
if (NumOfCounter_Right[T_OF_TRACK])
{
//对找到连通域的中心进行畸变矫正
//数组赋值
for (int i = 0; i < NumOfCounter_Right[T_OF_TRACK]; i++)
{
CenterPointOfCounter_R[T_OF_TRACK][i][0] = TheCounter_Right[T_OF_TRACK][i].x;
CenterPointOfCounter_R[T_OF_TRACK][i][1] = TheCounter_Right[T_OF_TRACK][i].y;
}
//对找到连通域的中心进行畸变矫正
//数组赋值
for (int i = 0; i < NumOfCounter_Right[T_OF_TRACK]; i++)
{
CenterPointOfCounter_R[T_OF_TRACK][i][0] = TheCounter_Right[T_OF_TRACK][i].x;
CenterPointOfCounter_R[T_OF_TRACK][i][1] = TheCounter_Right[T_OF_TRACK][i].y;
}
static CvMat ORGpoint = cvMat(1, NumOfCounter_Right[T_OF_TRACK], CV_64FC2, &CenterPointOfCounter_R[T_OF_TRACK][0][0]);//double 对应CV_64FC2
ORGpoint.cols = NumOfCounter_Right[T_OF_TRACK];
ORGpoint.data.db = CenterPointOfCounter_R[T_OF_TRACK][0];
//点的畸变矫正
cvUndistortPoints(&ORGpoint, &ORGpoint, &CameraMatri2, &dist_coeffs2, Rr, Pr);
}
//置标志位
To_Handle_Right[T_OF_TRACK] = true;
//向上级线程发送图片处理完毕函数
::PostMessage(m_hWnd, WM_DEAL_WIHT_IMAGE_TEMP, T_OF_TRACK, 1);//1确定是R
//转换储存地址
//省去拷贝
Temp = Image1;
Image1 = Image2;
Image2 = Image3;
Image3 = Temp;
//Imask2可以作为下一次的Imask1，省去一帧的处理
Temp = Imask1;
Imask1 = Imask2;
Imask2 = Temp;
T_OF_TRACK++;
if (T_OF_TRACK == NUM_T)T_OF_TRACK = 0;
}
}
//double a = (GETTIME() - Time_Temp);
//if (a > 13)
//{
// cout << "右相机图片处理进程时间危险" << a << endl;
//}
}
