void View::doViewDraw()
{
ofPushMatrix();
//Flip image and re-adjust
ofScale(-1, 1, 1);
ofTranslate(-this->getWidth(), 0, 0);
m_texture.loadData(m_useDepth ? getKinectData().m_depthStream : getKinectData().m_videoStream);
m_texture.draw(0, 0, this->getWidth(), this->getHeight());
ofPopMatrix();
}
void drawKinectData() {
getKinectData();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glBegin(GL_POINTS);
for (int i = 0; i < kinect_width*kinect_height; ++i) {
glColor3f(colorarray[i * 3], colorarray[i * 3 + 1], colorarray[i * 3 + 2]);
glVertex3f(vertexarray[i * 3], vertexarray[i * 3 + 1], vertexarray[i * 3 + 2]);
}
glEnd();
}
void drawKinectData() {
glBindTexture(GL_TEXTURE_2D, textureId);
getKinectData(data);
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_BGRA_EXT, GL_UNSIGNED_BYTE, (GLvoid*)data);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f);
glVertex3f(0, 0, 0);
glTexCoord2f(1.0f, 0.0f);
glVertex3f(width, 0, 0);
glTexCoord2f(1.0f, 1.0f);
glVertex3f(width, height, 0.0f);
glTexCoord2f(0.0f, 1.0f);
glVertex3f(0, height, 0.0f);
glEnd();
}
void drawKinectData() {
getKinectData();
rotateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY);
glBindBuffer(GL_ARRAY_BUFFER, vboId);
glVertexPointer(3, GL_FLOAT, 0, NULL);
glBindBuffer(GL_ARRAY_BUFFER, cboId);
glColorPointer(3, GL_FLOAT, 0, NULL);
glPointSize(1.f);
glDrawArrays(GL_POINTS, 0, width*height);
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
// Draw some arms
const Vector4& lh = skeletonPosition[NUI_SKELETON_POSITION_HAND_LEFT];
const Vector4& le = skeletonPosition[NUI_SKELETON_POSITION_ELBOW_LEFT];
const Vector4& ls = skeletonPosition[NUI_SKELETON_POSITION_SHOULDER_LEFT];
const Vector4& rh = skeletonPosition[NUI_SKELETON_POSITION_HAND_RIGHT];
const Vector4& re = skeletonPosition[NUI_SKELETON_POSITION_ELBOW_RIGHT];
const Vector4& rs = skeletonPosition[NUI_SKELETON_POSITION_SHOULDER_RIGHT];
glBegin(GL_LINES);
glColor3f(1.f, 0.f, 0.f);
if (lh.w > 0 && le.w > 0 && ls.w > 0) {
glVertex3f(lh.x, lh.y, lh.z);
glVertex3f(le.x, le.y, le.z);
glVertex3f(le.x, le.y, le.z);
glVertex3f(ls.x, ls.y, ls.z);
}
if (rh.w > 0 && re.w > 0 && rs.w > 0) {
glVertex3f(rh.x, rh.y, rh.z);
glVertex3f(re.x, re.y, re.z);
glVertex3f(re.x, re.y, re.z);
glVertex3f(rs.x, rs.y, rs.z);
}
glEnd();
}
void MKinect::update(){
QImage actualFrame;
QTime updateTime;
updateTime.start();
getKinectData(actualFrame);
++_actual_frame;
renderer->initTexture(actualFrame);
if (_tracker_set) {
bool orientationIsValid = false;
int SIZE = KinectColorWidth*KinectColorHeight;
std::vector<PXCPoint3DF32> vertices(SIZE);
CloudType::Ptr xyz_cloud(new CloudType(KinectColorWidth, KinectColorHeight));
int validP = 0;
for (int i = 0; i < SIZE; i++) {
if (convertPoint(depth2xyz[i], xyz_cloud->points[i])) {
++validP;
}
}
if (validP != 0) {//Then DO SOMETHING
if (_tracker_set == false) { //Use built-in algorithm
QTime algorithmTime;
algorithmTime.start();
orientationIsValid = track();
int elapsed = algorithmTime.elapsed();
++_actual_frame;
app->addIncrementalTimeMean(elapsed);
}
else { //Use algorithm in tracker and filters
double radius_search = tracker->getParameter("radius_search").toDouble();
bool subsampling_none = tracker->getParameter("subsample") == "none";
bool subsampling_uniform = tracker->getParameter("subsample") == "uniform";
bool subsampling_random = tracker->getParameter("subsample") == "random";
QTime algorithmTime;
algorithmTime.start();
if (!_last_init) {
_last_init = true;
//pcl::copyPointCloud(*xyz_cloud, *last_cloud);
last_cloud->is_dense = false;
first_cloud->is_dense = false;
xyz_cloud->is_dense = false;
std::vector<int> indices2;
CloudType::Ptr cloud_filtered(new CloudType);
pcl::removeNaNFromPointCloud(*xyz_cloud, *cloud_filtered, indices2);
//Pass cloud to renderer (Or pass the filtered one to see differences)
if (subsampling_uniform) {
pcl::PointCloud<int> indicesIN;
uniform_sampling.setInputCloud(cloud_filtered);
uniform_sampling.setRadiusSearch(radius_search);
uniform_sampling.compute(indicesIN);
//pcl::copyPointCloud(*cloud_filtered, indicesIN.points, *first_cloud);
pcl::copyPointCloud(*cloud_filtered, indicesIN.points, *first_cloud);
pcl::copyPointCloud(*cloud_filtered, indicesIN.points, *last_cloud);
}
else if (subsampling_none) {
pcl::copyPointCloud(*cloud_filtered, *first_cloud);
pcl::copyPointCloud(*cloud_filtered, *last_cloud);
}
else if (subsampling_random) {
pcl::copyPointCloud(*cloud_filtered, *first_cloud);
pcl::copyPointCloud(*cloud_filtered, *last_cloud);
}
//Save in first_cloud the first frame, filtered and prepared :D
//pcl::io::savePCDFileASCII("../clouds/rs/init_cloud.pcd", *cloud_filtered);
}
//Filter NANS for ICP
xyz_cloud->is_dense = false;
std::vector<int> indices;
CloudType::Ptr _cloud1(new CloudType);
pcl::removeNaNFromPointCloud(*xyz_cloud, *_cloud1, indices);
//Filter points using filter object
if (subsampling_uniform) {
QTime filterTime;
filterTime.start();
pcl::PointCloud<int> indicesOUT;
uniform_sampling.setInputCloud(_cloud1);
uniform_sampling.setRadiusSearch(radius_search);
uniform_sampling.compute(indicesOUT);
pcl::copyPointCloud(*_cloud1.get(), indicesOUT.points, *actual_cloud);
++_actual_frame;
//pcl::io::savePCDFileASCII("../clouds/rs/actual_cloud_" + QString::number(_actual_frame).toStdString() + ".pcd", *actual_cloud);
//pcl::copyPointCloud(*_cloud1.get(), *actual_cloud);
int elapsedF = filterTime.elapsed();
//Save frame info to file for DOCUMENTATION
//QFile file("../"+QString::number(radius_search)+"_uniform_info.txt");
//if (file.open(QIODevice::WriteOnly | QIODevice::Append)) {
// QTextStream stream(&file);
// stream << radius_search << " " << _cloud1->size() - actual_cloud->size() << " " << elapsedF << endl;
//}
}
else if (subsampling_none) {
pcl::copyPointCloud(*_cloud1.get(), *actual_cloud);
}
else if (subsampling_random) {
pcl::copyPointCloud(*_cloud1.get(), *actual_cloud);
}
//Pass cloud to algorithm
if (_last_init) { //Compute if we have 2 frames (last_cloud is filled with the first frame)
app->setPointsAnalyzed(actual_cloud->size(), first_cloud->size());
if (actual_cloud->size() > 0) {
}
double fitness;
orientationIsValid = tracker->compute(*last_cloud.get(), *actual_cloud.get(), _roll, _pitch, _yaw, fitness);
app->setICPConverged(orientationIsValid, fitness);
}
int elapsed = algorithmTime.elapsed();
app->addIncrementalTimeMean(elapsed);
}
renderer->setFaceTracked(orientationIsValid);
app->setFaceTracked(orientationIsValid);
if (orientationIsValid) {
app->setFaceAngles(_yaw, _pitch, _roll);
}
}
}
else {
QTime algorithmTime;
algorithmTime.start();
track();
int elapsed = algorithmTime.elapsed();
app->addIncrementalTimeMean(elapsed);
}
int elapsedUpdate = updateTime.elapsed();
app->setFilterTime(elapsedUpdate);
app->setFaceAngles(_yaw, _pitch, _roll);
}
void drawKinectData() {
glBindTexture(GL_TEXTURE_2D, textureId);
getKinectData(data);
getSkeletalData();
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_BGRA_EXT, GL_UNSIGNED_BYTE, (GLvoid*)data);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glBegin(GL_QUADS);
glColor3f(1.f, 1.f, 1.f);
glTexCoord2f(0.0f, 0.0f);
glVertex3f(0, 0, 0);
glTexCoord2f(1.0f, 0.0f);
glVertex3f(width, 0, 0);
glTexCoord2f(1.0f, 1.0f);
glVertex3f(width, height, 0.0f);
glTexCoord2f(0.0f, 1.0f);
glVertex3f(0, height, 0.0f);
glEnd();
// Torso & Head
const Vector4& hip = skeletonPosition[NUI_SKELETON_POSITION_HIP_CENTER];
const Vector4& spn = skeletonPosition[NUI_SKELETON_POSITION_SPINE];
const Vector4& shc = skeletonPosition[NUI_SKELETON_POSITION_SHOULDER_CENTER];
const Vector4& head = skeletonPosition[NUI_SKELETON_POSITION_HEAD];
// Left Arm etc
const Vector4& lh = skeletonPosition[NUI_SKELETON_POSITION_HAND_LEFT];
const Vector4& le = skeletonPosition[NUI_SKELETON_POSITION_ELBOW_LEFT];
const Vector4& lw = skeletonPosition[NUI_SKELETON_POSITION_WRIST_LEFT];
const Vector4& ls = skeletonPosition[NUI_SKELETON_POSITION_SHOULDER_LEFT];
// Right Arm etc
const Vector4& rh = skeletonPosition[NUI_SKELETON_POSITION_HAND_RIGHT];
const Vector4& re = skeletonPosition[NUI_SKELETON_POSITION_ELBOW_RIGHT];
const Vector4& rw = skeletonPosition[NUI_SKELETON_POSITION_WRIST_RIGHT];
const Vector4& rs = skeletonPosition[NUI_SKELETON_POSITION_SHOULDER_RIGHT];
// Left Leg etc
const Vector4& lhip = skeletonPosition[NUI_SKELETON_POSITION_HIP_LEFT];
const Vector4& lk = skeletonPosition[NUI_SKELETON_POSITION_KNEE_LEFT];
const Vector4& la = skeletonPosition[NUI_SKELETON_POSITION_ANKLE_LEFT];
const Vector4& lf = skeletonPosition[NUI_SKELETON_POSITION_FOOT_LEFT];
// Right Leg etc
const Vector4& rhip = skeletonPosition[NUI_SKELETON_POSITION_HIP_RIGHT];
const Vector4& rk = skeletonPosition[NUI_SKELETON_POSITION_KNEE_RIGHT];
const Vector4& ra = skeletonPosition[NUI_SKELETON_POSITION_ANKLE_RIGHT];
const Vector4& rf = skeletonPosition[NUI_SKELETON_POSITION_FOOT_RIGHT];
glLineWidth(20.0);
glBegin(GL_LINES);
glColor3f(1.f, 0.f, 0.f);
if (testTracked){
// Torso & Head
if (hip.w == 1 && spn.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(hip)[0], SkeletonToScreen(hip)[1]);
glVertex2f(SkeletonToScreen(spn)[0], SkeletonToScreen(spn)[1]);
}
if (spn.w == 1 && shc.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(spn)[0], SkeletonToScreen(spn)[1]);
glVertex2f(SkeletonToScreen(shc)[0], SkeletonToScreen(shc)[1]);
}
if (shc.w == 1 && head.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(shc)[0], SkeletonToScreen(shc)[1]);
glVertex2f(SkeletonToScreen(head)[0], SkeletonToScreen(head)[1]);
}
// Left Arm etc
if (lh.w == 1 && lw.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lh)[0], SkeletonToScreen(lh)[1]);
glVertex2f(SkeletonToScreen(lw)[0], SkeletonToScreen(lw)[1]);
}
if (lw.w == 1 && le.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lw)[0], SkeletonToScreen(lw)[1]);
glVertex2f(SkeletonToScreen(le)[0], SkeletonToScreen(le)[1]);
}
if (le.w == 1 && ls.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(le)[0], SkeletonToScreen(le)[1]);
glVertex2f(SkeletonToScreen(ls)[0], SkeletonToScreen(ls)[1]);
}
if (ls.w == 1 && shc.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(ls)[0], SkeletonToScreen(ls)[1]);
glVertex2f(SkeletonToScreen(shc)[0], SkeletonToScreen(shc)[1]);
}
// Right Arm etc
if (rh.w == 1 && rw.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rh)[0], SkeletonToScreen(rh)[1]);
glVertex2f(SkeletonToScreen(rw)[0], SkeletonToScreen(rw)[1]);
}
if (rw.w == 1 && re.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rw)[0], SkeletonToScreen(rw)[1]);
glVertex2f(SkeletonToScreen(re)[0], SkeletonToScreen(re)[1]);
}
if (re.w == 1 && rs.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(re)[0], SkeletonToScreen(re)[1]);
glVertex2f(SkeletonToScreen(rs)[0], SkeletonToScreen(rs)[1]);
}
if (rs.w == 1 && shc.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rs)[0], SkeletonToScreen(rs)[1]);
glVertex2f(SkeletonToScreen(shc)[0], SkeletonToScreen(shc)[1]);
}
// Left Leg etc
if (lf.w == 1 && la.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lf)[0], SkeletonToScreen(lf)[1]);
glVertex2f(SkeletonToScreen(la)[0], SkeletonToScreen(la)[1]);
}
if (la.w == 1 && lk.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(la)[0], SkeletonToScreen(la)[1]);
glVertex2f(SkeletonToScreen(lk)[0], SkeletonToScreen(lk)[1]);
}
if (lk.w == 1 && lhip.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lk)[0], SkeletonToScreen(lk)[1]);
glVertex2f(SkeletonToScreen(lhip)[0], SkeletonToScreen(lhip)[1]);
}
if (lhip.w == 1 && hip.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lhip)[0], SkeletonToScreen(lhip)[1]);
glVertex2f(SkeletonToScreen(hip)[0], SkeletonToScreen(hip)[1]);
}
// Right Leg etc
if (rf.w == 1 && ra.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rf)[0], SkeletonToScreen(rf)[1]);
glVertex2f(SkeletonToScreen(ra)[0], SkeletonToScreen(ra)[1]);
}
if (ra.w == 1 && rk.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(ra)[0], SkeletonToScreen(ra)[1]);
glVertex2f(SkeletonToScreen(rk)[0], SkeletonToScreen(rk)[1]);
}
if (rk.w == 1 && rhip.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rk)[0], SkeletonToScreen(rk)[1]);
glVertex2f(SkeletonToScreen(rhip)[0], SkeletonToScreen(rhip)[1]);
}
if (rhip.w == 1 && hip.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rhip)[0], SkeletonToScreen(rhip)[1]);
glVertex2f(SkeletonToScreen(hip)[0], SkeletonToScreen(hip)[1]);
}
}
glEnd();
// Leg angles and their output to screen.
float fVal1 = 0;
float fVal2 = 0;
float facingAngle = 0;
if (testTracked && rhip.w == 1 && lhip.w == 1 && rk.w == 1 && lk.w == 1){ // checking to see if all the needed joints are tracked
Vector3D* UpperLegRotZ = NULL;
Vector3D* UpperLegRotX = NULL;
int screen_position[2];
if (active_leg == 1){ // Left leg is selected. Default value.
UpperLegRotZ = new Vector3D(lk.x - lhip.x, lk.y - lhip.y, 0);
UpperLegRotX = new Vector3D(0, lk.y - lhip.y, lk.z - lhip.z);
screen_position[0] = SkeletonToScreen(lhip)[0];
screen_position[1] = SkeletonToScreen(lhip)[1];
}
else if (active_leg == 2){ // Left leg is selected.
UpperLegRotZ = new Vector3D(rk.x - rhip.x, rk.y - rhip.y, 0);
UpperLegRotX = new Vector3D(0, rk.y - rhip.y, rk.z - rhip.z);
screen_position[0] = SkeletonToScreen(rhip)[0]+20; // 20 pixels moved to center it better above joint
screen_position[1] = SkeletonToScreen(rhip)[1];
}
// Creating vector to measure leg angle against and calculating the angles for rotation around z-achsis and x-achsis.
Vector3D& jointAttachedPendulum = Vector3D(0, -1, 0);
Vector3D& planeNormal = Vector3D(0, 0, 1);
fVal1 = twoVectorAngle(*UpperLegRotZ, jointAttachedPendulum, planeNormal);
planeNormal = Vector3D(-1, 0, 0);
fVal2 = twoVectorAngle(*UpperLegRotX, jointAttachedPendulum, planeNormal);
// Constructing the calculation for a camera facing vector
Vector3D& rightHipPointToLeft = Vector3D(lhip.x - rhip.x, 0, lhip.z - rhip.z);
planeNormal = Vector3D(0, -1, 0);
Vector3D& straightFacing = Vector3D(-1, 0, 0); // must align with rightHipPointToLeftVector for skeleton to be facing straight forward
facingAngle = twoVectorAngle(rightHipPointToLeft, straightFacing, planeNormal);
delete UpperLegRotZ;
UpperLegRotZ = NULL;
delete UpperLegRotX;
UpperLegRotX = NULL;
// Write angle of the rotation around z to screen by converting float to char array and printing to screen above the joints.
glColor3f(0.f, 1.f, 0.f);
glRasterPos2f(screen_position[0] - 30, screen_position[1] - 20);
char cVal[32];
sprintf_s(cVal, "%f", fVal1);
outputText = cVal;
for (int i = 0; i < 5; i++) {
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, outputText[i]);
}
// New screen position for angle of the rotation around x.
glRasterPos2f(screen_position[0] - 30, screen_position[1] - 40);
sprintf_s(cVal, "%f", fVal2);
outputText = cVal;
for (int i = 0; i < 5; i++) {
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, outputText[i]);
}
// screen position for facing angle
glRasterPos2f(545, 140);
sprintf_s(cVal, "%f", facingAngle);
outputText = cVal;
for (int i = 0; i < 5; i++) {
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, outputText[i]);
}
glLineWidth(5.0);
glBegin(GL_LINES);
// blue arrow for facing
glColor3f(0.f, 0.f, 1.f);
glLineWidth(5.0);
// arrow shaft
glVertex2f(570 + sin(facingAngle) * 50, 70 - cos(facingAngle) * 50);
glVertex2f(570 - sin(facingAngle) * 50 , 70 + cos(facingAngle) * 50);
// arrow tip part 1
glVertex2f(570 - sin(facingAngle+0.4) * 30, 70 + cos(facingAngle+0.4) * 30);
glVertex2f(570 - sin(facingAngle) * 50, 70 + cos(facingAngle) * 50);
// arrow tip part 2
glVertex2f(570 - sin(facingAngle - 0.4) * 30, 70 + cos(facingAngle - 0.4) * 30);
glVertex2f(570 - sin(facingAngle) * 50, 70 + cos(facingAngle) * 50);
glEnd();
}
// Nag message about menu appears on screen if menu has not been used yet.
if (menu_used == FALSE){
outputText = "Left click on screen to use menu!";
glColor3f(255, 0, 0);
glRasterPos2f(10, 20);
int len;
len = (int)strlen(outputText);
for (int i = 0; i < len; i++) {
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, outputText[i]);
}
}
// Guards:
// catch legs that are not fully tracked and bad angles
if (lk.w == 0 || lhip.w == 0){
guard_trap = TRUE;
}
if (fVal1 > 45.29){
guard_trap = TRUE;
}
if (guarded_data == FALSE){
guard_trap = FALSE;
}
// Program actions
if (record_data == TRUE && save_data == FALSE){
appendData(fVal1, fVal2, facingAngle);
}
else if (record_data == TRUE && save_data == TRUE && guard_trap == FALSE){
record_data = FALSE;
save_data = FALSE;
appendData(fVal1, fVal2, facingAngle);
writePickle();
}
else if (record_data == FALSE && save_data == TRUE && data_stream->next != NULL && guard_trap == FALSE){
save_data == FALSE;
writePickle();
}
}
