UrlDialog::~UrlDialog()
{
delete m_pUrlList;
UrlDlgList * tmpitem = findFrame();
tmpitem->dlg = nullptr;
}
static void reportUnsafeJavaScriptAccess(v8::Local<v8::Object> host, v8::AccessType type, v8::Local<v8::Value> data)
{
Frame* target = findFrame(host, data);
if (!target)
return;
DOMWindow* targetWindow = target->document()->domWindow();
targetWindow->printErrorMessage(targetWindow->crossDomainAccessErrorMessage(activeDOMWindow(BindingState::instance())));
}
static void failedAccessCheckCallbackInMainThread(v8::Local<v8::Object> host, v8::AccessType type, v8::Local<v8::Value> data)
{
Frame* target = findFrame(host, data, v8::Isolate::GetCurrent());
if (!target)
return;
DOMWindow* targetWindow = target->domWindow();
setDOMException(SecurityError, targetWindow->crossDomainAccessErrorMessage(activeDOMWindow()), v8::Isolate::GetCurrent());
}
void FramestackWidget::slotSelectFrame(int frameNo, int threadNo)
{
FrameStackItem * frame = findFrame(frameNo, threadNo);
if (frame != 0) {
setSelected(frame, true);
emit selectFrame(frameNo, threadNo, frame->frameName());
} else {
emit selectFrame(frameNo, threadNo, QString());
}
}
static void failedAccessCheckCallbackInMainThread(v8::Local<v8::Object> host, v8::AccessType type, v8::Local<v8::Value> data)
{
v8::Isolate* isolate = v8::Isolate::GetCurrent();
LocalFrame* target = findFrame(host, data, isolate);
if (!target)
return;
// FIXME: We should modify V8 to pass in more contextual information (context, property, and object).
ExceptionState exceptionState(ExceptionState::UnknownContext, 0, 0, isolate->GetCurrentContext()->Global(), isolate);
exceptionState.throwSecurityError("failedAccessCheckCallbackInMainThread", "failedAccessCheckCallbackInMainThread");
exceptionState.throwIfNeeded();
}
static void failedAccessCheckCallbackInMainThread(v8::Local<v8::Object> host, v8::AccessType type, v8::Local<v8::Value> data)
{
v8::Isolate* isolate = v8::Isolate::GetCurrent();
Frame* target = findFrame(isolate, host, data);
if (!target)
return;
DOMWindow* targetWindow = target->domWindow();
// FIXME: We should modify V8 to pass in more contextual information (context, property, and object).
ExceptionState exceptionState(ExceptionState::UnknownContext, 0, 0, isolate->GetCurrentContext()->Global(), isolate);
exceptionState.throwSecurityError(targetWindow->sanitizedCrossDomainAccessErrorMessage(currentDOMWindow(isolate)), targetWindow->crossDomainAccessErrorMessage(currentDOMWindow(isolate)));
exceptionState.throwIfNeeded();
}
static bool url_kvs_cmd_list(KviKvsModuleCommandCall *)
{
UrlDlgList *tmpitem = findFrame();
if (tmpitem->dlg) return false;
tmpitem->dlg = new UrlDialog(g_pList);
g_pMainWindow->addWindow(tmpitem->dlg);
for(KviUrl *tmp=g_pList->first();tmp;tmp=g_pList->next())
{
QString tmpCount;
tmpCount.setNum(tmp->count);
tmpitem->dlg->addUrl(QString(tmp->url), QString(tmp->window), tmpCount, QString(tmp->timestamp));
}
return true;
}
APETAG::Iterator::Iterator(const APETAG &parent, const char *id, uint16_t len)
: mParent(parent),
mID(NULL),
///mOffset(mParent.mFirstFrameOffset),
mOffset(0),
mFrameData(NULL),
mFrameSize(0)
{
if (id)
{
mID = strdup(id);
}
mIDLen = len;
findFrame();
}
// VarFrameRoot frames in the Variable Tree from the previous program pause,
// are set active here. Notified by the Frame Stack widget when it parses the
// backtrace from the 'where' command after a pause.
//
// After that, any frames which aren't marked as active must have gone
// out of scope and will end up pruned.
void VariableTree::slotFrameActive(int frameNo, int threadNo, const QString& frameName)
{
VarFrameRoot * frame = findFrame(frameNo, threadNo);
if (frameNo == 1) {
// If the current frame 1 doesn't exist, create it
if (frame == 0) {
frame = new VarFrameRoot(this, frameNo, threadNo);
}
frame->setFrameName(frameName);
}
if (frame != 0 && frame->text(VAR_NAME_COLUMN) == frameName) {
frame->setActivationId();
}
}
static bool url_kvs_cmd_list(KviKvsModuleCommandCall *)
{
UrlDlgList * tmpitem = findFrame();
if(tmpitem->dlg) {
tmpitem->dlg->delayedAutoRaise();
return false;
}
tmpitem->dlg = new UrlDialog(g_List);
g_pMainWindow->addWindow(tmpitem->dlg);
for(auto tmp : g_List)
{
QString tmpCount;
tmpCount.setNum(tmp->count);
tmpitem->dlg->addUrl(QString(tmp->url), QString(tmp->window), tmpCount, QString(tmp->timestamp));
}
return true;
}
UrlDialog::~UrlDialog()
{
/*
KviConfigurationFile cfg(szConfigPath,KviConfigurationFile::Write);
cfg.setGroup("ConfigDialog");
if (cfg.readBoolEntry("SaveColumnWidthOnClose",false)) {
cfg.setGroup("ColsWidth");
cfg.writeEntry("Url",m_pUrlList->columnWidth(0));
cfg.writeEntry("Window",m_pUrlList->columnWidth(1));
cfg.writeEntry("Count",m_pUrlList->columnWidth(2));
cfg.writeEntry("Timestamp",m_pUrlList->columnWidth(3));
}
*/
delete m_pUrlList;
/* if (m_pListPopup) delete m_pListPopup;
m_pListPopup = 0;
if (m_pMenuBar) delete m_pMenuBar;
m_pMenuBar = 0;*/
UrlDlgList *tmpitem = findFrame();
tmpitem->dlg = 0;
}
static pj_bool_t first_packet_in_frame(Jitter_Buffer *jitter_buffer, JTPacket *packet) {
//find prev frame
Frame_Buffer *prev_frame = findFrame(&jitter_buffer->frameList, packet->ts, '<');
//check
if(prev_frame != NULL) {
if(prev_frame->session_info.packetList.next == &prev_frame->session_info.packetList) //empty
return PJ_FALSE;
else {
int lastHighSeq = frame_buffer_getHighSeq(prev_frame);
if(lastHighSeq == -1) {
pj_assert(PJ_FALSE); //bad frame
return PJ_FALSE;
}
if(InSequence(lastHighSeq, packet->seq))
return PJ_TRUE;
}
} else {
if(InSequence(jitter_buffer->decode_state.seq, packet->seq))
return PJ_TRUE;
}
return PJ_FALSE;
}
bool addData(double **data, double **pos_data) {
//double data[JOINT_NUM][JOINT_DATA_NUM], double pos_data[POS_JOINT_NUM][POS_JOINT_DATA_NUM]) {
// advance one frame
currentFramePointer=findFrame(1);
// store current data
for (int i=0;i<JOINT_NUM;i++) {
for (int j=0;j<JOINT_DATA_NUM;j++) {
prevFrame[currentFramePointer][i][j] = data[i][j];
}
}
for (int i=0;i<POS_JOINT_NUM;i++) {
for (int j=0;j<POS_JOINT_DATA_NUM;j++) {
prevFrame_posOnly[currentFramePointer][i][j] = pos_data[i][j];
}
}
if (currentFramePointer >= (frameStoreNum-1)) {
frameComparable = true;
}
}
bool VariableTree::schedule()
{
QListViewItem * child = firstChild();
VarFrameRoot * frame = 0;
while (child != 0) {
if (child->rtti() == RTTI_VAR_FRAME_ROOT) {
frame = (VarFrameRoot *) child;
Q_ASSERT( !frame->isWaitingForData() );
if (frame->needsVariables()) {
if (QApplication::overrideCursor() == 0) {
QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
}
// Tell the controller to fetch the variable values
emit selectFrame(frame->frameNo(), frame->threadNo());
return true;
}
}
child = child->nextSibling();
}
frame = findFrame(1, currentThread_);
Q_ASSERT( frame != 0 );
Q_ASSERT( !frame->needsVariables() );
// All over, nothing left to fetch.
// Return to frame 1, and prune the inactive items
// from the variable tree..
QApplication::restoreOverrideCursor();
emit selectFrame(1, currentThread_);
prune();
return false;
}
int computeHandPosition(double **data, double **pos_data, int variableNum) {
//double data[JOINT_NUM][JOINT_DATA_NUM], double pos_data[POS_JOINT_NUM][POS_JOINT_DATA_NUM], int variableNum) {
// compute hand loc
double left_hand_pos[3];
double right_hand_pos[3];
double head_ori[9];
double head_pos[3];
double torso_ori[9];
double torso_pos[3];
for (int i = 0;i<9;i++) {
head_ori[i]=data[HEAD_JOINT_NUM][i];
}
for (int i=0;i<3;i++) {
head_pos[i]=data[HEAD_JOINT_NUM][i+9];
}
for (int i = 0;i<9;i++) {
torso_ori[i]=data[TORSO_JOINT_NUM][i];
}
for (int i=0;i<3;i++) {
torso_pos[i]=data[TORSO_JOINT_NUM][i+9];
}
for (int i=0;i<3;i++) {
left_hand_pos[i]=pos_data[POS_LEFT_HAND_NUM][i];
}
for (int i=0;i<3;i++) {
right_hand_pos[i]=pos_data[POS_RIGHT_HAND_NUM][i];
}
double* left_hand = computeLocalHandLoc(head_ori, head_pos, left_hand_pos);
for (int i=0;i<3;i++) {
// fprintf(pRecFile, "%.7f,",left_hand[i]);
featureValues.push_back(left_hand[i]);
variableNum++;
}
left_hand = computeLocalHandLoc(torso_ori, torso_pos, left_hand_pos);
for (int i=0;i<3;i++) {
// fprintf(pRecFile, "%.7f,", left_hand[i]);
featureValues.push_back(left_hand[i]);
variableNum++;
}
double* right_hand = computeLocalHandLoc(head_ori, head_pos, right_hand_pos);
for (int i=0;i<3;i++) {
// fprintf(pRecFile, "%.7f,", right_hand[i]);
featureValues.push_back(right_hand[i]);
variableNum++;
}
right_hand = computeLocalHandLoc(torso_ori, torso_pos, right_hand_pos);
for (int i=0;i<3;i++) {
// fprintf(pRecFile, "%.7f,", right_hand[i]);
featureValues.push_back(right_hand[i]);
variableNum++;
}
if (DEBUG_numFeature) printf("hand: %d\n", variableNum);
// Maybe try collecting hand motion.
if (USE_HAND_MOTION) {
double left_hand_pos_prev[3];
double right_hand_pos_prev[3];
double diff_left[3];
double diff_right[3];
for (int i=0; i<compareFrameNum; i++) {
int frameCompareTo = findFrame(compareFrame[i]);
for (int j=0;j<3;j++) {
left_hand_pos_prev[j] = prevFrame_posOnly[frameCompareTo][POS_LEFT_HAND_NUM][j];
right_hand_pos_prev[j] = prevFrame_posOnly[frameCompareTo][POS_RIGHT_HAND_NUM][j];
diff_left[j] = left_hand_pos[j] - left_hand_pos_prev[j];
diff_right[j] = right_hand_pos[j] - right_hand_pos_prev[j];
}
// fprintf(pRecFile, "%.7f,%.7f,%.7f,", diff_left[0], diff_left[1], diff_left[2]);
// fprintf(pRecFile, "%.7f,%.7f,%.7f,", diff_right[0], diff_right[1], diff_right[2]);
for(int num=0; num<3; num++)
{
featureValues.push_back(diff_left[num]);
}
for(int num=0; num<3; num++)
{
featureValues.push_back(diff_right[num]);
}
variableNum+=6;
}
if (DEBUG_numFeature) printf("hand motion: %d\n", variableNum);
}
return variableNum;
} // end computeHandPosition
int computeBodyPoseAndMotionInfo(double **data, double **pos_data, int variableNum) {
//double data[JOINT_NUM][JOINT_DATA_NUM], double pos_data[POS_JOINT_NUM][POS_JOINT_DATA_NUM], int variableNum) {
for (int i=0; i<compareFrameNum; i++) {
for (int j=0; j<JOINT_NUM; j++) {
int frameCompareTo = findFrame(compareFrame[i]);
if (currentFramePointer == frameCompareTo) {
if (j != TORSO_JOINT_NUM) {
// rot mat
double m[3][3];
for (int k =0;k<3;k++) {
m[0][k]= prevFrame[currentFramePointer][TORSO_JOINT_NUM][0]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][3]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][6]*prevFrame[currentFramePointer][j][6+k];
m[1][k]= prevFrame[currentFramePointer][TORSO_JOINT_NUM][1]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][4]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][7]*prevFrame[currentFramePointer][j][6+k];
m[2][k]= prevFrame[currentFramePointer][TORSO_JOINT_NUM][2]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][5]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[currentFramePointer][TORSO_JOINT_NUM][8]*prevFrame[currentFramePointer][j][6+k];
/*printf("%.10f %.10f %.10f %.10f\n", prevFrame[currentFramePointer][TORSO_JOINT_NUM][0]*prevFrame[currentFramePointer][j][0+k]
, prevFrame[currentFramePointer][TORSO_JOINT_NUM][3]*prevFrame[currentFramePointer][j][3+k]
, prevFrame[currentFramePointer][TORSO_JOINT_NUM][6]*prevFrame[currentFramePointer][j][6+k], m[0][k]);*/
}
/*for (int ii = 0;ii<3;ii++)
for(int jj=0;jj<3;jj++)
printf("%d %d %d %.10f\n",frameCompareTo, ii,jj,m[ii][jj]); */
// quat
double q1 = 1.0/2.0 * sqrt(1 + m[0][0] - m[1][1] - m[2][2]);
double q2 = 1.0/(4.0*q1) * (m[0][1] + m[1][0]);
double q3 = 1.0/(4.0*q1) * (m[0][2] + m[2][0]);
double q4 = 1.0/(4.0*q1) * (m[2][1] - m[1][2]);
if (!isnan(q1) && !isnan(q2) && !isnan(q3) && !isnan(q4)
&& !isinf(q1) && !isinf(q2) && !isinf(q3) && !isinf(q4)) {
// fprintf(pRecFile, "%.7f,%.7f,%.7f,%.7f,",q1,q2,q3,q4);
featureValues.push_back(q1);
featureValues.push_back(q2);
featureValues.push_back(q3);
featureValues.push_back(q4);
} else {
double zero = 0.0;
// fprintf(pRecFile, "%.1f,%.1f,%.1f,%.1f,",zero,zero,zero,zero);;
for(int num =0; num<4; num++)
featureValues.push_back(zero);
}
variableNum+=4;
if(DEBUG_numFeature) printf("current frame : %d\n", variableNum);
}
} else {
// rot mat
double m[3][3];
for (int k =0;k<3;k++) {
m[0][k]= prevFrame[frameCompareTo][j][0]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[frameCompareTo][j][3]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[frameCompareTo][j][6]*prevFrame[currentFramePointer][j][6+k];
m[1][k]= prevFrame[frameCompareTo][j][1]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[frameCompareTo][j][4]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[frameCompareTo][j][7]*prevFrame[currentFramePointer][j][6+k];
m[2][k]= prevFrame[frameCompareTo][j][2]*prevFrame[currentFramePointer][j][0+k]
+ prevFrame[frameCompareTo][j][5]*prevFrame[currentFramePointer][j][3+k]
+ prevFrame[frameCompareTo][j][8]*prevFrame[currentFramePointer][j][6+k];
}
/*for (int ii = 0;ii<3;ii++)
for(int jj=0;jj<3;jj++)
printf("%d %d %d %.10f\n",frameCompareTo, ii,jj,m[ii][jj]); */
// quat
double q1 = 1.0/2.0 * sqrt(1 + m[0][0] - m[1][1] - m[2][2]);
double q2 = 1.0/(4.0*q1) * (m[0][1] + m[1][0]);
double q3 = 1.0/(4.0*q1) * (m[0][2] + m[2][0]);
double q4 = 1.0/(4.0*q1) * (m[2][1] - m[1][2]);
if (!isnan(q1) && !isnan(q2) && !isnan(q3) && !isnan(q4)
&&!isinf(q1) && !isinf(q2) && !isinf(q3) && !isinf(q4)) {
// fprintf(pRecFile, "%.7f,%.7f,%.7f,%.7f,",q1,q2,q3,q4);
featureValues.push_back(q1);
featureValues.push_back(q2);
featureValues.push_back(q3);
featureValues.push_back(q4);
} else {
double zero = 0.0;
// fprintf(pRecFile, "%.1f,%.1f,%.1f,%.1f,",zero,zero,zero,zero);;
for(int num = 0; num < 4; num++)
featureValues.push_back(zero);
}
variableNum+=4;
if(DEBUG_numFeature) printf("comparing against frame : %d (against:%d)\n", variableNum, compareFrame[i]);
}
} // end j
} // end i
return variableNum;
} // end computeBodyPoseAndMotionInfo
int computeHead(int variableNum) {
//double data[JOINT_NUM][JOINT_DATA_NUM], double pos_data[POS_JOINT_NUM][POS_JOINT_DATA_NUM], int variableNum) {
int curFrame = findFrame(0);
// compute hand loc
double heap_pos[3];
double head_pos[3];
double head_up[3];
double he_fs = 0; // head from sensor
double hp_fs = 0; // heap from sensor
//double head_ori[9];
/*for (int i=0;i<9;i++) {
head_ori[i] = prevFrame[curFrame][HEAD_JOINT_NUM][i];
}*/
for (int i=0;i<3;i++) {
double ltemp = prevFrame[curFrame][7][i+9];
double rtemp = prevFrame[curFrame][9][i+9];
heap_pos[i] = (ltemp+rtemp)/2;
head_pos[i] = prevFrame[curFrame][HEAD_JOINT_NUM][i+9];
head_up[i] = heap_pos[i];
if (i==1) { // y val
head_up[i] = head_pos[i];
}
if (i!=1) { // except y val
he_fs += (head_pos[i]*head_pos[i]);
hp_fs += (heap_pos[i]*heap_pos[i]);
}
//printf("%d: head: %.1f heap: %.1f up: %.1f\n", i+1, head_pos[i], heap_pos[i], head_up[i]);
}
double v1[3]; // heap to head
double v2[3]; // heap to up
double v1_det = 0;
double v2_det = 0;
double dot = 0;
for (int i=0;i<3;i++) {
v1[i] = head_pos[i] - heap_pos[i];
v2[i] = head_up[i] - heap_pos[i];
dot += (v1[i]*v2[i]);
v1_det += (v1[i]*v1[i]);
v2_det += (v2[i]*v2[i]);
}
v1_det = sqrt(v1_det);
v2_det = sqrt(v2_det);
double ang = acos(dot / (v1_det * v2_det));
ang = ang * 180 / 3.14159265; // to degree
if (he_fs > hp_fs) { // if heap is closer
ang = -ang;
}
if (ang>80) {
// printf("computeHead angle not right.. %.1f \n", ang); exit(1);
}
// fprintf(pRecFile, "%.1f,", ang);
featureValues.push_back(ang);
variableNum++;
/*double* headLoc = computeLocalHandLoc(head_ori, head_pos, heap_pos);
for (int i=0;i<3;i++) {
fprintf(pRecFile, "%.7f,", headLoc[i]);
variableNum++;
}*/
if (DEBUG_numFeature) printf("HEAD angle feature: %d\n", variableNum);
return variableNum;
} // end computeHead
bool processData(double **data, double **pos_data) {
//double data[JOINT_NUM][JOINT_DATA_NUM], double pos_data[POS_JOINT_NUM][POS_JOINT_DATA_NUM]) {
featureValues.clear();
// advance one frame
currentFramePointer=findFrame(1);
// store current data
for (int i=0;i<JOINT_NUM;i++) {
for (int j=0;j<JOINT_DATA_NUM;j++) {
prevFrame[currentFramePointer][i][j] = data[i][j];
}
}
for (int i=0;i<POS_JOINT_NUM;i++) {
for (int j=0;j<POS_JOINT_DATA_NUM;j++) {
prevFrame_posOnly[currentFramePointer][i][j] = pos_data[i][j];
}
}
// compute diff
if (currentFramePointer >= (frameStoreNum-1)) {
frameComparable = true;
}
if (!frameComparable) {
cout << "current frame pointer " << currentFramePointer << endl;
if (currentFramePointer >= (frameStoreNum-1)) {
frameComparable = true;
}
} else {
int variableNum = 0;
// each row starts with fileName
// add L or M at the beginning to tell it is label (helps reading from matlab)
/* if (!mirrored) {
fprintf(pRecFile, "L%s,", curFileName);
} else {
fprintf(pRecFile, "M%s,", curFileName);
}*/
//fprintf(pRecFile, "%d,", currentFramePointer);
// feature 1 & 3 of AAAI paper
variableNum = computeBodyPoseAndMotionInfo(data,pos_data,variableNum);
// feature 2 of AAAI paper + some extra Hand motion (hand motion fails)
variableNum = computeHandPosition(data,pos_data,variableNum);
// foot location
variableNum = computeFootPosition(data,pos_data,variableNum);
// hand percentile
variableNum = computeMoreHandFeatures(variableNum);
if (HEAD_AGAINST_MID_HEAP) {
variableNum = computeHead(variableNum);
}
if(DEBUG_numFeature) {
printf("\t\tfeatures: %d\n", variableNum);
}
return true;
}
return false;
}
void Frame::updateFrames(CRI * boss)
{
// Todo: Need to interpolate rotation aswell!!!
bool found;
vector<string> listFromTF;
vector<string>::iterator listIt;
vector<Frame>::iterator frameIt;
tf::StampedTransform transform;
GFrame * tframe;
tfListener->getFrameStrings(listFromTF);
// Check for the appearance of any new frames...
for (listIt = listFromTF.begin(); listIt != listFromTF.end(); listIt++)
{
found = false;
for (frameIt = availableFrames.begin(); frameIt != availableFrames.end(); frameIt++)
{
if ((*frameIt).name == *listIt)
{
found = true;
break;
}
}
if (!found)
{
Frame newFrame = Frame(*listIt);
// We assume the frame is valid
newFrame.valid = true;
availableFrames.push_back(newFrame);
// Add a representative frame
tframe = new GFrame((char *)(*listIt).c_str());
boss->addGadget(tframe);
tframe->applyProperties();
tframe->show(true);
tframe->sourceFrame = availableFrames.size() - 1;
}
}
double timeDiff, timeFut;
ros::Time tTime;
tf::StampedTransform tTran;
int framePosition = -1;
// Update frame locations by extrapolating
// Todo: second order extrapolation, please
for (frameIt = availableFrames.begin(); frameIt != availableFrames.end(); frameIt++)
{
Frame & tFr = (*frameIt);
framePosition++;
if (tfListener->canTransform(fixedFrame, tFr.name, ros::Time(0), NULL))
{
// Check if the frame has become valid
if (!tFr.valid)
{
tFr.valid = true;
// Add a renewed representative frame
tframe = new GFrame((char *)tFr.name.c_str());
boss->addGadget(tframe);
tframe->applyProperties();
tframe->show(true);
tframe->sourceFrame = framePosition;
}
tfListener->lookupTransform(fixedFrame, tFr.name, ros::Time(0), tTran);
}
else
{
// TODO: error msg
// The frame is not connected to the fixed frame, or we're extrapolating, either way,
// the frame is useless.
tFr.valid = false;
continue;
}
//cout << "Beer: " << ros::Time::now() << "\n";
//cout << tTran.stamp_ << "\n";
if (tFr.gotLast == false)
{
tFr.curTransform = tTran;
tFr.curTime = tTime;
tFr.extrapTransform = tTran;
tFr.gotLast = true;
}
else
{
// Do we have a new sample? If so, update the pair of samples
if (tTran.stamp_ > tFr.curTime)
{
tFr.lastTransform = tFr.curTransform;
tFr.lastTime = tFr.curTime;
tFr.curTransform = tTran;
tFr.curTime = tTran.stamp_;
}
// Calculate the extrapolated position
timeDiff = (tFr.curTime - tFr.lastTime).toSec();
timeFut = (ros::Time::now() - tFr.curTime).toSec();
tf::Vector3 & lPos = tFr.lastTransform.getOrigin();
tf::Quaternion lRot = tFr.lastTransform.getRotation();
tf::Vector3 & cPos = tFr.curTransform.getOrigin();
tf::Quaternion cRot = tFr.curTransform.getRotation();
// Todo: No magic numbers
if (timeDiff > 0.001 && timeFut < 0.5)
{
tFr.extrapTransform.setOrigin(tf::Vector3((cPos.x() - lPos.x()) / timeDiff * timeFut + cPos.x(),
(cPos.y() - lPos.y()) / timeDiff * timeFut + cPos.y(),
(cPos.z() - lPos.z()) / timeDiff * timeFut + cPos.z()));
tFr.extrapTransform.setRotation(tFr.curTransform.getRotation());
}
else
{
tFr.extrapTransform.setOrigin(tFr.curTransform.getOrigin());
tFr.extrapTransform.setRotation(tFr.curTransform.getRotation());
}
}
}
// If we need to draw the frame tree, do so
if (drawFrameTree)
{
tf::StampedTransform endPoint;
tf::StampedTransform startPoint;
string startFrameName;
if (treeNode && !treeWasVisible)
{
boss->rootSceneNode->addChild(treeNode);
}
else if (treeNode == NULL)
{
treeNode = boss->rootSceneNode->createChildSceneNode();
}
if (!treeManual)
{
treeManual = boss->scene->createManualObject("TreeManual");
treeNode->attachObject(treeManual);
treeManual->setDynamic(true);
treeManual->estimateVertexCount(1000);
treeManual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
}
else
{
treeManual->beginUpdate(0);
}
treeManual->colour(Ogre::ColourValue(1, 1, 0, 1));
for (frameIt = availableFrames.begin(); frameIt != availableFrames.end(); frameIt++)
{
if ((*frameIt).valid)
{
//tfListener->lookupTransform(fixedFrame, tFr.name, ros::Time(0), endPoint);
tf::Vector3 startPos = (*frameIt).extrapTransform.getOrigin();
tfListener->getParent((*frameIt).name, ros::Time(), startFrameName);
int fNum = findFrame(startFrameName);
if (fNum >= 0)
{
tf::Vector3 endPos = availableFrames[fNum].extrapTransform.getOrigin();
treeManual->position(startPos.x(), startPos.y(), startPos.z());
treeManual->position(endPos.x(), endPos.y(), endPos.z());
}
}
}
// A dummy to keep things sane, can't have zero length manual object
treeManual->position(0, 0, 0);
treeManual->position(0, 0, 0);
treeManual->end();
treeWasVisible = 1;
}
else
{
if (treeWasVisible)
{
boss->rootSceneNode->removeChild(treeNode);
}
treeWasVisible = 0;
}
}
/**
* @isFirstPacket first packet in a frame
* @isMarketPacket last packet in a frame
*/
PJ_DEF(int) jitter_buffer_insert_packet(Jitter_Buffer *jitter_buffer,
pj_uint16_t seq, pj_uint32_t ts, pjmedia_frame_type frame_type,
char *payload, int size,
pj_bool_t isFirstPacket, pj_bool_t isMarketPacket) {
int ret = 0;
if(pj_mutex_lock(jitter_buffer->jb_mutex) != PJ_SUCCESS) {
return -1;
}
pj_ssize_t current;
JTPacket *packet = NULL;
Frame_Buffer *frame;
// is running
if(!jitter_buffer->running) {
ret = -2;
goto ON_RET;
}
// sanity check
if(ts == 0 || (frame_type != PJMEDIA_FRAME_TYPE_EMPTY && size == 0)) {
ret = -3;
goto ON_RET;
}
if(!jitter_buffer->first_packet) {
isFirstPacket = PJ_TRUE;
jitter_buffer->first_packet = PJ_TRUE;
}
//clean old frames
//CleanOldFrames(jitter_buffer);
//update jitter
current = GetCurrentTimeMs();
if(frame_type != PJMEDIA_FRAME_TYPE_EMPTY && frame_type != PJMEDIA_FRAME_TYPE_NONE) {
if(jitter_buffer->waiting_for_completed_frame.timestamp == ts) {
jitter_buffer->waiting_for_completed_frame.frame_size += size;
jitter_buffer->waiting_for_completed_frame.latest_packet_timestamp = current;
} else if(jitter_buffer->waiting_for_completed_frame.latest_packet_timestamp > 0 &&
current - jitter_buffer->waiting_for_completed_frame.latest_packet_timestamp > 2000) {
//too old
UpdateJitterEstimatorForWaitingFrame(jitter_buffer, &jitter_buffer->waiting_for_completed_frame);
jitter_buffer->waiting_for_completed_frame.frame_size = 0;
jitter_buffer->waiting_for_completed_frame.latest_packet_timestamp = -1;
jitter_buffer->waiting_for_completed_frame.timestamp = 0;
}
}
//create packet
packet = NULL;
if(jt_packet_create(&packet, &jitter_buffer->packet_alloc, seq, ts, frame_type,
isFirstPacket, isMarketPacket, payload, size) != 0) {
if(packet)
list_alloc_insert(packet, &jitter_buffer->packet_alloc);
ret = -1;
goto ON_RET;
}
//GetCurrentTimeInLocal(packet->time_in_jb, 60);
if(!isFirstPacket) {
//is first packet in frame
isFirstPacket = first_packet_in_frame(jitter_buffer, packet);
packet->isFirst = isFirstPacket;
}
if(isMarketPacket) {
//check if next packet is first packet
Frame_Buffer *next_frame = findFrame(&jitter_buffer->frameList, packet->ts, '>');
if(next_frame != NULL) {
JTPacket * first_packet = next_frame->session_info.packetList.next;
if(packet != &next_frame->session_info.packetList) {
if(InSequence(packet->seq, first_packet->seq)) {
first_packet->isFirst = PJ_TRUE;
}
}
}
}
//clean old frames
CleanOldFrames(jitter_buffer);
// is old packet
if(decode_state_isOldPacket(packet, &jitter_buffer->decode_state)) {
decode_state_updateOldPacket(packet, &jitter_buffer->decode_state);
list_alloc_insert(packet, &jitter_buffer->packet_alloc);
ret = -1;
goto ON_RET;
}
// find or alloc a frame
frame = findFrame(&jitter_buffer->frameList, packet->ts, '=');
if(frame == NULL) { //alloc one
//if(jitter_buffer->number_of_frames > jitter_buffer->max_number_of_frames) {
if(jitter_buffer->number_of_frames > jitter_buffer->max_number_of_frames) {
//clean old frames at least one
Frame_Buffer *oldestFrame = jitter_buffer->frameList.next;
if(oldestFrame != &jitter_buffer->frameList)
RealseFrame(jitter_buffer, oldestFrame);
}
list_alloc_alloc(Frame_Buffer, &frame, &jitter_buffer->frame_alloc);
//init
frame_buffer_init(frame, &jitter_buffer->packet_alloc);
}
//insert packet into the frame
ret = frame_buffer_insert_packet(frame, packet);
if(ret > 0) {
list_alloc_insert(packet, &jitter_buffer->packet_alloc);
frame_buffer_reset(frame);
list_alloc_insert(frame, &jitter_buffer->frame_alloc);
ret = -1;
goto ON_RET;
} else if (ret < 0) {
frame_buffer_reset(frame);
list_alloc_insert(frame, &jitter_buffer->frame_alloc);
ret = -1;
goto ON_RET;
} else {
event_set(jitter_buffer->packet_event);
if(packet->isRetrans)
frame_buffer_IncrementNackCount(frame);
}
//insert frame to frame list
if(findFrame(&jitter_buffer->frameList, frame->ts, '=') == NULL) {
Frame_Buffer *prev_frame = findFrame(&jitter_buffer->frameList, frame->ts, '<');
prev_frame = (prev_frame == NULL ? &jitter_buffer->frameList : prev_frame);
pj_list_insert_after(prev_frame, frame);
event_set(jitter_buffer->frame_event);
jitter_buffer->number_of_frames++;
}
ON_RET:
pj_mutex_unlock(jitter_buffer->jb_mutex);
return ret;
}
static Frame_Buffer* findFrameByTs(Jitter_Buffer *jitter_buffer, pj_uint32_t ts) {
return findFrame(&jitter_buffer->frameList, ts, '=');
}
int computeMoreHandFeatures(int variableNum) {
//double data[JOINT_NUM][JOINT_DATA_NUM], double pos_data[POS_JOINT_NUM][POS_JOINT_DATA_NUM], int variableNum) {
// compute hand loc
double left_hand_pos[3];
double right_hand_pos[3];
double head_ori[9];
double head_pos[3];
int latest = compareFrame[compareFrameNum-1];
vector<double> lhand(30);
vector<double> rhand(30);
int vCount=0;
for (int last=-2;last>latest;last--) {
int frameN = findFrame(last);
double ltemp = prevFrame_posOnly[frameN][POS_LEFT_HAND_NUM][1]-prevFrame[frameN][HEAD_JOINT_NUM][9+1]; // y value
double rtemp = prevFrame_posOnly[frameN][POS_RIGHT_HAND_NUM][1]-prevFrame[frameN][HEAD_JOINT_NUM][9+1]; // y value
lhand.push_back(ltemp);
rhand.push_back(rtemp);
vCount++;
//cout << "l " << ltemp << " " << rtemp << endl;
/*for (int i = 0;i<9;i++) {
head_ori[i]=prevFrame[frameN][HEAD_JOINT_NUM][i];
}
for (int i=0;i<3;i++) {
head_pos[i]=prevFrame[frameN][HEAD_JOINT_NUM][i+9];
}
for (int i=0;i<3;i++) {
left_hand_pos[i]=prevFrame_posOnly[frameN][POS_LEFT_HAND_NUM][i];
}
for (int i=0;i<3;i++) {
right_hand_pos[i]=prevFrame_posOnly[frameN][POS_RIGHT_HAND_NUM][i];
}
double* left_hand = computeLocalHandLoc(head_ori, head_pos, left_hand_pos);
lhand.push_back(left_hand[2]);
double* right_hand = computeLocalHandLoc(head_ori, head_pos, right_hand_pos);
rhand.push_back(right_hand[2]);
for (int j=0;j<3;j++) {
cout << right_hand[j] << " ";
}
cout << endl;*/
}
sort(lhand.begin(), lhand.end());
sort(rhand.begin(), rhand.end());
int numValue = vCount;
for (int i=0;i<sizeof(percentile)/sizeof(percentile[0]);i++) {
int temp = numValue * percentile[i] / 100;
// fprintf(pRecFile, "%.1f,%.1f,", lhand[temp], rhand[temp]);
featureValues.push_back(lhand[temp]);
featureValues.push_back(rhand[temp]);
variableNum += 2;
//cout << i << " " << temp << " " << numValue << " " << latest << " " << lhand[temp] << " " << rhand[temp] << endl;
if (DEBUG_numFeature) printf("hand location (percentile: %d): %d\n", percentile[i], variableNum);
}
if (USE_BIGGEST_UPDOWN_HAND_MOVE) {
int temp1 = numValue * 5 / 100;
int temp2 = numValue * 95 / 100;
double moveL = lhand[temp2]-lhand[temp1];
double moveR = rhand[temp2]-rhand[temp1];
// fprintf(pRecFile, "%.1f,%.1f,", moveL, moveR);
featureValues.push_back(moveL);
featureValues.push_back(moveR);
variableNum += 2;
if (DEBUG_numFeature) printf("hand biggest move (up/down): %d\n", variableNum);
}
if (DEBUG_numFeature) printf("extra hand features (percentile+move): %d\n", variableNum);
return variableNum;
} // end computeMoreHandFeatures
