Skip to content

Commit

Permalink
Merge pull request OpenKinect#409 from pAIgn10/master
Browse files Browse the repository at this point in the history 
Add cpp_pcview point cloud example

Reviewed-by: Benn Snyder <[email protected]>
  • Loading branch information
@piedar
piedar committed Aug 24, 2014
2 parents 8d95792 + 811950a commit 110a58b
Show file tree
Hide file tree
Showing 2 changed files with 340 additions and 0 deletions.
5 changes: 5 additions & 0 deletions wrappers/cpp/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ endif()
include_directories(.)

add_executable(freenect-cppview cppview.cpp)
add_executable(freenect-cpp_pcview cpp_pc_view.cpp)

# Mac just has everything already
if(APPLE)
Expand All @@ -26,9 +27,13 @@ else()
include_directories(${OPENGL_INCLUDE_DIR} ${GLUT_INCLUDE_DIR} ${USB_INCLUDE_DIRS})

target_link_libraries(freenect-cppview freenect ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${MATH_LIB})
target_link_libraries(freenect-cpp_pcview freenect ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${CMAKE_THREAD_LIBS_INIT})
endif()

install (TARGETS freenect-cppview
DESTINATION bin)

install (TARGETS freenect-cpp_pcview
DESTINATION bin)

ENDIF()
335 changes: 335 additions & 0 deletions wrappers/cpp/cpp_pc_view.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,335 @@
/*
* This file is part of the OpenKinect Project. http://www.openkinect.org
*
* Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
* for details.
*
* This code is licensed to you under the terms of the Apache License, version
* 2.0, or, at your option, the terms of the GNU General Public License,
* version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
* or the following URLs:
* http://www.apache.org/licenses/LICENSE-2.0
* http://www.gnu.org/licenses/gpl-2.0.txt
*
* If you redistribute this file in source form, modified or unmodified, you
* may:
* 1) Leave this header intact and distribute it under the same terms,
* accompanying it with the APACHE20 and GPL20 files, or
* 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
* 3) Delete the GPL v2 clause and accompany it with the APACHE20 file
* In all cases you must keep the copyright notice intact and include a copy
* of the CONTRIB file.
*
* Binary distributions must follow the binary distribution requirements of
* either License.
*/

#include <iostream>
#include <vector>
#include <pthread.h>
#include <libfreenect.hpp>

#if defined(__APPLE__)
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif


class Mutex
{
public:
Mutex()
{
pthread_mutex_init(&m_mutex, NULL);
}

void lock()
{
pthread_mutex_lock(&m_mutex);
}

void unlock()
{
pthread_mutex_unlock(&m_mutex);
}

class ScopedLock
{
public:
ScopedLock(Mutex &mutex) : _mutex(mutex)
{
_mutex.lock();
}

~ScopedLock()
{
_mutex.unlock();
}

private:
Mutex &_mutex;
};

private:
pthread_mutex_t m_mutex;
};


class MyFreenectDevice : public Freenect::FreenectDevice
{
public:
MyFreenectDevice(freenect_context *_ctx, int _index)
: Freenect::FreenectDevice(_ctx, _index),
m_buffer_video(freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB).bytes),
m_buffer_depth(freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED).bytes / 2),
m_new_rgb_frame(false), m_new_depth_frame(false)
{
setDepthFormat(FREENECT_DEPTH_REGISTERED);
}

// Do not call directly, even in child
void VideoCallback(void *_rgb, uint32_t timestamp)
{
Mutex::ScopedLock lock(m_rgb_mutex);
uint8_t* rgb = static_cast<uint8_t*>(_rgb);
copy(rgb, rgb+getVideoBufferSize(), m_buffer_video.begin());
m_new_rgb_frame = true;
}

// Do not call directly, even in child
void DepthCallback(void *_depth, uint32_t timestamp)
{
Mutex::ScopedLock lock(m_depth_mutex);
uint16_t* depth = static_cast<uint16_t*>(_depth);
copy(depth, depth+getDepthBufferSize()/2, m_buffer_depth.begin());
m_new_depth_frame = true;
}

bool getRGB(std::vector<uint8_t> &buffer)
{
Mutex::ScopedLock lock(m_rgb_mutex);

if (!m_new_rgb_frame)
return false;

buffer.swap(m_buffer_video);
m_new_rgb_frame = false;

return true;
}

bool getDepth(std::vector<uint16_t> &buffer)
{
Mutex::ScopedLock lock(m_depth_mutex);

if (!m_new_depth_frame)
return false;

buffer.swap(m_buffer_depth);
m_new_depth_frame = false;

return true;
}

private:
Mutex m_rgb_mutex;
Mutex m_depth_mutex;
std::vector<uint8_t> m_buffer_video;
std::vector<uint16_t> m_buffer_depth;
bool m_new_rgb_frame;
bool m_new_depth_frame;
};


Freenect::Freenect freenect;
MyFreenectDevice* device;

int window(0); // Glut window identifier
int mx = -1, my = -1; // Prevous mouse coordinates
float anglex = 0, angley = 0; // Panning angles
float zoom = 1; // Zoom factor
bool color = true; // Flag to indicate to use of color in the cloud


void DrawGLScene()
{
static std::vector<uint8_t> rgb(640*480*3);
static std::vector<uint16_t> depth(640*480);

device->getRGB(rgb);
device->getDepth(depth);

glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

glPointSize(1.0f);

glBegin(GL_POINTS);

if (!color) glColor3ub(255, 255, 255);
for (int i = 0; i < 480*640; ++i)
{
if (color)
glColor3ub( rgb[3*i+0], // R
rgb[3*i+1], // G
rgb[3*i+2] ); // B

float f = 595.f;
// Convert from image plane coordinates to world coordinates
glVertex3f( (i%640 - (640-1)/2.f) * depth[i] / f, // X = (x - cx) * d / fx
(i/640 - (480-1)/2.f) * depth[i] / f, // Y = (y - cy) * d / fy
depth[i] ); // Z = d
}

glEnd();

// Draw the world coordinate frame
glLineWidth(2.0f);
glBegin(GL_LINES);
glColor3ub(255, 0, 0); // X-axis
glVertex3f( 0, 0, 0);
glVertex3f( 50, 0, 0);
glColor3ub(0, 255, 0); // Y-axis
glVertex3f(0, 0, 0);
glVertex3f(0, 50, 0);
glColor3ub(0, 0, 255); // Z-axis
glVertex3f(0, 0, 0);
glVertex3f(0, 0, 50);
glEnd();

// Place the camera
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glScalef(zoom, zoom, 1);
gluLookAt( -7*anglex, -7*angley, -1000.0,
0.0, 0.0, 2000.0,
0.0, -1.0, 0.0 );

glutSwapBuffers();
}


void keyPressed(unsigned char key, int x, int y)
{
switch (key)
{
case 'C':
case 'c':
color = !color;
break;

case 'Q':
case 'q':
case 0x1B: // ESC
glutDestroyWindow(window);
device->stopDepth();
device->stopVideo();
exit(0);
}
}


void mouseMoved(int x, int y)
{
if (mx >= 0 && my >= 0)
{
anglex += x - mx;
angley += y - my;
}

mx = x;
my = y;
}


void mouseButtonPressed(int button, int state, int x, int y)
{
if (state == GLUT_DOWN)
{
switch (button)
{
case GLUT_LEFT_BUTTON:
mx = x;
my = y;
break;

case 3:
zoom *= 1.2f;
break;

case 4:
zoom /= 1.2f;
break;
}
}
else if (state == GLUT_UP && button == GLUT_LEFT_BUTTON)
{
mx = -1;
my = -1;
}
}


void resizeGLScene(int width, int height)
{
glViewport(0, 0, width, height);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(50.0, (float)width / height, 900.0, 11000.0);

glMatrixMode(GL_MODELVIEW);
}


void idleGLScene()
{
glutPostRedisplay();
}


void printInfo()
{
std::cout << "\nAvailable Controls:" << std::endl;
std::cout << "===================" << std::endl;
std::cout << "Rotate : Mouse Left Button" << std::endl;
std::cout << "Zoom : Mouse Wheel" << std::endl;
std::cout << "Toggle Color : C" << std::endl;
std::cout << "Quit : Q or Esc\n" << std::endl;
}


int main(int argc, char **argv)
{
device = &freenect.createDevice<MyFreenectDevice>(0);
device->startVideo();
device->startDepth();

glutInit(&argc, argv);

glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutInitWindowSize(640, 480);
glutInitWindowPosition(0, 0);

window = glutCreateWindow("LibFreenect");
glClearColor(0.45f, 0.45f, 0.45f, 0.0f);

glEnable(GL_DEPTH_TEST);
glEnable(GL_ALPHA_TEST);
glAlphaFunc(GL_GREATER, 0.0f);

glMatrixMode(GL_PROJECTION);
gluPerspective(50.0, 1.0, 900.0, 11000.0);

glutDisplayFunc(&DrawGLScene);
glutIdleFunc(&idleGLScene);
glutReshapeFunc(&resizeGLScene);
glutKeyboardFunc(&keyPressed);
glutMotionFunc(&mouseMoved);
glutMouseFunc(&mouseButtonPressed);

printInfo();

glutMainLoop();

return 0;
}

0 comments on commit 110a58b

Please sign in to comment.