/***********************************************************************
RawKinectViewer - Simple application to view color and depth images
captured from a Kinect device.
Copyright (c) 2010-2015 Oliver Kreylos
This file is part of the Kinect 3D Video Capture Project (Kinect).
The Kinect 3D Video Capture Project is free software; you can
redistribute it and/or modify it under the terms of the GNU General
Public License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version.
The Kinect 3D Video Capture Project is distributed in the hope that it
will be useful, but WITHOUT ANY WARRANTY; without even the implied
warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See
the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along
with the Kinect 3D Video Capture Project; if not, write to the Free
Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
02111-1307 USA
***********************************************************************/
#ifndef RAWKINECTVIEWER_INCLUDED
#define RAWKINECTVIEWER_INCLUDED
#include <Misc/FunctionCalls.h>
#include <Threads/TripleBuffer.h>
#include <USB/Context.h>
#include <Geometry/Plane.h>
#include <GL/gl.h>
#include <GL/GLObject.h>
#include <GLMotif/ToggleButton.h>
#include <GLMotif/FileSelectionDialog.h>
#include <Vrui/LocatorTool.h>
#include <Vrui/Application.h>
#include <Kinect/FrameBuffer.h>
#include <Kinect/FrameSource.h>
/* Forward declarations: */
namespace GLMotif {
class PopupMenu;
class PopupWindow;
}
namespace Kinect {
class Camera;
}
class RawKinectViewer:public Vrui::Application,public GLObject
{
/* Embedded classes: */
private:
typedef Geometry::Plane<float,3> Plane; // Type for planes in depth camera or world space
typedef Kinect::FrameSource::DepthPixel DepthPixel; // Type for depth frame pixels
typedef Kinect::FrameSource::ColorComponent ColorComponent; // Type for color frame pixel components
typedef Kinect::FrameSource::ColorPixel ColorPixel; // Type for color frame pixels
typedef Kinect::FrameSource::DepthCorrection::PixelCorrection PixelCorrection; // Type for per-pixel depth correction factors
typedef Kinect::FrameSource::IntrinsicParameters IntrinsicParameters; // Type for camera intrinsic parameters
typedef Misc::FunctionCall<int> AverageFrameReadyCallback; // Type for callbacks when an average depth frame has been captured; int argument is a dummy
struct DataItem:public GLObject::DataItem
{
/* Elements: */
public:
unsigned int colorTextureSize[2]; // Padded size of the color texture
GLuint colorTextureId; // ID of texture object holding color image
unsigned int colorFrameVersion; // Version number of color currently in texture object
unsigned int depthTextureSize[2]; // Padded size of the depth texture
GLuint depthTextureId; // ID of texture object holding depth image
unsigned int depthFrameVersion; // Version number of frame currently texture object
/* Constructors and destructors: */
DataItem(void);
virtual ~DataItem(void);
};
friend class PauseTool;
friend class MeasurementTool;
friend class TiePointTool;
friend class LineTool;
friend class DepthCorrectionTool;
friend class GridTool;
friend class PlaneTool;
friend class PointPlaneTool;
friend class CalibrationCheckTool;
/* Elements: */
USB::Context usbContext; // USB device context
Kinect::Camera* camera; // Pointer to camera aspect of Kinect device
const unsigned int* colorFrameSize; // Size of color frames in pixels
Threads::TripleBuffer<Kinect::FrameBuffer> colorFrames; // Triple buffer of color frames received from the camera
unsigned int colorFrameVersion; // Version number of current color frame
const unsigned int* depthFrameSize; // Size of depth frames in pixels
PixelCorrection* depthCorrection; // Buffer containing per-pixel depth correction coefficients
IntrinsicParameters intrinsicParameters; // Intrinsic parameters of the Kinect camera
float depthValueRange[2]; // Range of depth values mapped to the depth color map
float depthPlaneDistMax; // Range of depth plane color map around depth plane
Threads::TripleBuffer<Kinect::FrameBuffer> depthFrames; // Triple buffer of depth frames received from the camera
unsigned int depthFrameVersion; // Version number of current depth frame
bool paused; // Flag whether the video stream display is paused
unsigned int averageNumFrames; // Number of depth frames to average
unsigned int averageFrameCounter; // Number of depth frames still to accumulate
std::vector<AverageFrameReadyCallback*> averageFrameReadyCallbacks; // Functions called when a new average depth frame has been captured
float* averageFrameDepth; // Average depth values in depth frame
float* averageFrameForeground; // Ratio of foreground vs background in depth frame
bool averageFrameValid; // Flag whether the average depth frame buffer is currently valid
bool showAverageFrame; // Flag whether to show the averaged frame
bool depthPlaneValid; // Flag whether a depth plane has been defined
Plane camDepthPlane; // Depth plane equation in depth camera image space
Plane worldDepthPlane; // Depth plane equation in world space
unsigned int selectedPixel[2]; // Coordinates of the selected depth image pixel
DepthPixel selectedPixelPulse[128]; // EKG of depth value of selected pixel
int selectedPixelCurrentIndex; // Index of most recent value in selected pixel's EKG
GLMotif::PopupMenu* mainMenu; // The program's main menu
GLMotif::PopupWindow* averageDepthFrameDialog; // A dialog window indicating that an average depth frame is being captured
/* Private methods: */
void mapDepth(unsigned int x,unsigned int y,float depth,GLubyte* colorPtr) const; // Maps a depth value to a color
Vrui::Point calcImagePoint(const Vrui::Ray& physicalRay) const; // Returns image-space point at which the given physical-space ray intersects the image plane
void colorStreamingCallback(const Kinect::FrameBuffer& frameBuffer); // Callback receiving color frames from the Kinect camera
void depthStreamingCallback(const Kinect::FrameBuffer& frameBuffer); // Callback receiving depth frames from the Kinect camera
void requestAverageFrame(AverageFrameReadyCallback* callback); // Requests collection of an average depth frame; given function will be called when it's ready
void locatorButtonPressCallback(Vrui::LocatorTool::ButtonPressCallbackData* cbData); // Callback when a locator tool's button is pressed
void resetNavigationCallback(Misc::CallbackData* cbData);
void captureBackgroundCallback(Misc::CallbackData* cbData);
void removeBackgroundCallback(GLMotif::ToggleButton::ValueChangedCallbackData* cbData);
void averageFramesCallback(GLMotif::ToggleButton::ValueChangedCallbackData* cbData);
void saveAverageFrameOKCallback(GLMotif::FileSelectionDialog::OKCallbackData* cbData);
void saveAverageFrameCallback(Misc::CallbackData* cbData);
void saveColorFrameOKCallback(GLMotif::FileSelectionDialog::OKCallbackData* cbData);
void saveColorFrameCallback(Misc::CallbackData* cbData);
GLMotif::PopupMenu* createMainMenu(void); // Creates the program's main menu
GLMotif::PopupWindow* createAverageDepthFrameDialog(void); // Creates the depth frame averaging dialog
/* Constructors and destructors: */
public:
RawKinectViewer(int& argc,char**& argv,char**& appDefaults);
virtual ~RawKinectViewer(void);
/* Methods from Vrui::Application: */
virtual void toolCreationCallback(Vrui::ToolManager::ToolCreationCallbackData* cbData);
virtual void frame(void);
virtual void display(GLContextData& contextData) const;
/* Methods from GLObject: */
virtual void initContext(GLContextData& contextData) const;
};
#endif
/***********************************************************************
RawKinectViewer - Simple application to view color and depth images
captured from a Kinect device.
Copyright (c) 2010-2015 Oliver Kreylos
This file is part of the Kinect 3D Video Capture Project (Kinect).
The Kinect 3D Video Capture Project is free software; you can
redistribute it and/or modify it under the terms of the GNU General
Public License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version.
The Kinect 3D Video Capture Project is distributed in the hope that it
will be useful, but WITHOUT ANY WARRANTY; without even the implied
warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See
the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along
with the Kinect 3D Video Capture Project; if not, write to the Free
Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
02111-1307 USA
***********************************************************************/
#include "Viewer.h"
#include <stdexcept>
#include <iostream>
#include <Misc/SelfDestructPointer.h>
#include <Misc/FunctionCalls.h>
#include <IO/File.h>
#include <IO/Directory.h>
#include <Geometry/Point.h>
#include <Geometry/Ray.h>
#include <Geometry/OrthogonalTransformation.h>
#include <GL/gl.h>
#include <GL/Extensions/GLARBTextureNonPowerOfTwo.h>
#include <GL/GLContextData.h>
#include <Images/RGBImage.h>
#include <Images/WriteImageFile.h>
#include <GLMotif/PopupMenu.h>
#include <GLMotif/Menu.h>
#include <GLMotif/Button.h>
#include <Vrui/Vrui.h>
#include <Vrui/ToolManager.h>
#include <Vrui/OpenFile.h>
#include <Kinect/Camera.h>
/******************************************
Methods of class RawKinectViewer::DataItem:
******************************************/
RawKinectViewer::DataItem::DataItem(void)
:colorTextureId(0),colorFrameVersion(0),
depthTextureId(0),depthFrameVersion(0)
{
/* Allocate texture objects: */
glGenTextures(1,&depthTextureId);
glGenTextures(1,&colorTextureId);
}
RawKinectViewer::DataItem::~DataItem(void)
{
/* Destroy texture objects: */
glDeleteTextures(1,&depthTextureId);
glDeleteTextures(1,&colorTextureId);
}
/********************************
Methods of class RawKinectViewer:
********************************/
void RawKinectViewer::mapDepth(unsigned int x,unsigned int y,float depth,GLubyte* result) const
{
if(depthPlaneValid)
{
/* Color depth pixels by distance to the depth plane: */
float dist=camDepthPlane.calcDistance(Plane::Point(float(x)+0.5f,float(y)+0.5f,depth));
if(dist>=0.0f)
{
GLubyte col=dist<depthPlaneDistMax?255U-GLubyte((dist*255.0f)/depthPlaneDistMax+0.5f):0U;
result[0]=col;
result[1]=col;
result[2]=255U;
}
else
{
GLubyte col=-dist<depthPlaneDistMax?255U-GLubyte((-dist*255.0f)/depthPlaneDistMax+0.5f):0U;
result[0]=255U;
result[1]=col;
result[2]=col;
}
}
else
{
/* Color depth pixels by depth value: */
static const GLubyte mapColors[6][3]=
{
{255,0,0},
{255,255,0},
{0,255,0},
{0,255,255},
{0,0,255},
{255,0,255}
};
float d=(depth-depthValueRange[0])*5.0f/(depthValueRange[1]-depthValueRange[0]);
if(d<=0.0f)
{
for(int i=0;i<3;++i)
result[i]=GLubyte(mapColors[0][i]*0.2f);
}
else if(d>=5.0f)
{
for(int i=0;i<3;++i)
result[i]=mapColors[5][i];
}
else
{
int i0=int(d);
d-=float(i0);
for(int i=0;i<3;++i)
result[i]=GLubyte((mapColors[i0][i]*(1.0f-d)+mapColors[i0+1][i]*d)*(d*0.8f+0.2f));
}
}
}
Vrui::Point RawKinectViewer::calcImagePoint(const Vrui::Ray& physicalRay) const
{
/* Transform the ray to navigational space: */
Vrui::Ray navRay=physicalRay;
navRay.transform(Vrui::getInverseNavigationTransformation());
if(navRay.getDirection()[2]!=Vrui::Scalar(0))
{
Vrui::Scalar lambda=-navRay.getOrigin()[2]/navRay.getDirection()[2];
return navRay(lambda);
}
else
return Vrui::Point::origin;
}
void RawKinectViewer::colorStreamingCallback(const Kinect::FrameBuffer& frameBuffer)
{
if(!paused)
{
#if 0
/* Normalize the color frame: */
Kinect::FrameBuffer normalizedFrame(frameBuffer.getSize(0),frameBuffer.getSize(1),frameBuffer.getSize(1)*frameBuffer.getSize(0)*sizeof(ColorPixel));
const ColorPixel* fPtr=static_cast<const ColorPixel*>(frameBuffer.getBuffer());
ColorPixel* nfPtr=static_cast<ColorPixel*>(normalizedFrame.getBuffer());
for(int y=0;y<frameBuffer.getSize(1);++y)
for(int x=0;x<frameBuffer.getSize(0);++x,++fPtr,++nfPtr)
{
#if 1
/* Divide all color components by the largest component: */
unsigned int max=fPtr->rgb[0];
for(int i=1;i<3;++i)
if(max<fPtr->rgb[i])
max=fPtr->rgb[i];
for(int i=0;i<3;++i)
nfPtr->rgb[i]=ColorComponent(((unsigned int)fPtr->rgb[i]*256U)/(max+1));
#endif
#if 0
/* Classify the unprocessed color: */
if(fPtr->rgb[0]<32U&&fPtr->rgb[1]<32U&&fPtr->rgb[2]<32U)
nfPtr->rgb[0]=nfPtr->rgb[1]=nfPtr->rgb[2]=255U;
else
nfPtr->rgb[0]=nfPtr->rgb[1]=nfPtr->rgb[2]=0U;
#elif 0
/* Classify the normalized color: */
if(nfPtr->rgb[0]>=240U&&nfPtr->rgb[1]<80U&&nfPtr->rgb[2]>=80U)
nfPtr->rgb[0]=nfPtr->rgb[1]=nfPtr->rgb[2]=255U;
else
nfPtr->rgb[0]=nfPtr->rgb[1]=nfPtr->rgb[2]=0U;
#endif
}
colorFrames.postNewValue(normalizedFrame);
#else
/* Post the new frame into the color frame triple buffer: */
colorFrames.postNewValue(frameBuffer);
#endif
/* Update application state: */
Vrui::requestUpdate();
}
}
void RawKinectViewer::depthStreamingCallback(const Kinect::FrameBuffer& frameBuffer)
{
if(!paused)
{
/* Post the new frame into the depth frame triple buffer: */
depthFrames.postNewValue(frameBuffer);
/* Update application state: */
Vrui::requestUpdate();
}
}
void RawKinectViewer::requestAverageFrame(RawKinectViewer::AverageFrameReadyCallback* callback)
{
/* Check if there already is an average frame: */
if(averageFrameValid)
{
/* Just call the callback immediately and forget about it: */
if(callback!=0)
{
(*callback)(0);
delete callback;
}
}
else
{
/* Check if there is already an average frame capture underway: */
if(averageFrameCounter==0)
{
/* Start averaging frames: */
float* afdPtr=averageFrameDepth;
float* affPtr=averageFrameForeground;
for(unsigned int y=0;y<depthFrameSize[1];++y)
for(unsigned int x=0;x<depthFrameSize[0];++x,++afdPtr,++affPtr)
{
*afdPtr=0.0f;
*affPtr=0.0f;
}
averageFrameCounter=averageNumFrames;
/* Show a progress dialog: */
Vrui::popupPrimaryWidget(averageDepthFrameDialog);
}
/* Add the callback to the callback list: */
if(callback!=0)
averageFrameReadyCallbacks.push_back(callback);
}
}
void RawKinectViewer::locatorButtonPressCallback(Vrui::LocatorTool::ButtonPressCallbackData* cbData)
{
Vrui::Point pos=cbData->currentTransformation.getOrigin();
if(pos[0]>=-depthFrameSize[0]&&pos[0]<0.0&&pos[1]>=0.0&&pos[1]<depthFrameSize[1])
{
/* Select the pixel under the locator: */
selectedPixel[0]=int(pos[0]+double(depthFrameSize[0]));
selectedPixel[1]=int(pos[1]);
/* Start the selected pixel's EKG: */
selectedPixelCurrentIndex=0;
const DepthPixel* dfPtr=static_cast<const DepthPixel*>(depthFrames.getLockedValue().getBuffer());
selectedPixelPulse[0]=dfPtr[selectedPixel[1]*depthFrames.getLockedValue().getSize(0)+selectedPixel[0]];
for(int i=1;i<128;++i)
selectedPixelPulse[i]=0;
}
else
{
/* Select an invalid pixel: */
selectedPixel[0]=selectedPixel[1]=~0x0U;
}
}
void RawKinectViewer::resetNavigationCallback(Misc::CallbackData* cbData)
{
/* Reset the navigation transformation: */
Vrui::setNavigationTransformation(Vrui::Point::origin,Vrui::Scalar(1024),Vrui::Vector(0,1,0));
}
void RawKinectViewer::captureBackgroundCallback(Misc::CallbackData* cbData)
{
/* Capture five seconds worth of background frames: */
camera->captureBackground(150,true);
}
void RawKinectViewer::removeBackgroundCallback(GLMotif::ToggleButton::ValueChangedCallbackData* cbData)
{
/* Set the background removal flag: */
camera->setRemoveBackground(cbData->set);
/* Set the toggle button's state to the actual new flag value: */
cbData->toggle->setToggle(camera->getRemoveBackground());
}
void RawKinectViewer::averageFramesCallback(GLMotif::ToggleButton::ValueChangedCallbackData* cbData)
{
showAverageFrame=cbData->set;
if(cbData->set)
{
/* Request a new average frame: */
requestAverageFrame(0);
}
else
{
/* Invalidate the current average frame: */
averageFrameValid=false;
depthPlaneValid=false;
}
}
void RawKinectViewer::saveAverageFrameOKCallback(GLMotif::FileSelectionDialog::OKCallbackData* cbData)
{
try
{
/* Open the average frame file: */
IO::FilePtr frameFile(cbData->selectedDirectory->openFile(cbData->selectedFileName,IO::File::WriteOnly));
/* Write the averaged frame: */
for(int i=0;i<2;++i)
frameFile->write<Misc::UInt32>(depthFrameSize[i]);
float cutoff=float(averageNumFrames)*0.5f;
float* afdPtr=averageFrameDepth;
float* affPtr=averageFrameForeground;
const PixelCorrection* dcPtr=depthCorrection;
for(unsigned int y=0;y<depthFrameSize[1];++y)
for(unsigned int x=0;x<depthFrameSize[0];++x,++afdPtr,++affPtr,++dcPtr)
frameFile->write<Misc::Float32>(*affPtr>=cutoff?dcPtr->correct((*afdPtr)/(*affPtr)):2047.0f);
}
catch(std::runtime_error err)
{
/* Show an error message: */
Vrui::showErrorMessage("Save Average Depth Frame...",Misc::printStdErrMsg("Could not write depth frame file %s due to exception %s",cbData->getSelectedPath().c_str(),err.what()));
}
/* Destroy the file selection dialog: */
cbData->fileSelectionDialog->close();
}
void RawKinectViewer::saveAverageFrameCallback(Misc::CallbackData* cbData)
{
if(!averageFrameValid)
{
/* Show an error message: */
Vrui::showErrorMessage("Save Average Depth Frame...","No valid average depth frame to save");
return;
}
try
{
/* Create a uniquely-named depth image file in the current directory: */
IO::DirectoryPtr currentDir=Vrui::openDirectory(".");
std::string depthFrameFileName=currentDir->createNumberedFileName("DepthFrame.dat",4);
/* Create a file selection dialog to select an alternative depth frame file name: */
Misc::SelfDestructPointer<GLMotif::FileSelectionDialog> saveAverageFrameDialog(new GLMotif::FileSelectionDialog(Vrui::getWidgetManager(),"Save Average Depth Frame...",currentDir,depthFrameFileName.c_str(),".dat"));
saveAverageFrameDialog->getOKCallbacks().add(this,&RawKinectViewer::saveAverageFrameOKCallback);
saveAverageFrameDialog->deleteOnCancel();
/* Show the file selection dialog: */
Vrui::popupPrimaryWidget(saveAverageFrameDialog.releaseTarget());
}
catch(std::runtime_error err)
{
/* Show an error message: */
Vrui::showErrorMessage("Save Average Depth Frame...",Misc::printStdErrMsg("Could not save average depth frame due to exception %s",err.what()));
}
}
void RawKinectViewer::saveColorFrameOKCallback(GLMotif::FileSelectionDialog::OKCallbackData* cbData)
{
try
{
/* Convert the current color frame into an RGB image: */
const ColorPixel* sPtr=static_cast<const ColorPixel*>(colorFrames.getLockedValue().getBuffer());
Images::RGBImage colorImage(colorFrameSize[0],colorFrameSize[1]);
Images::RGBImage::Color* dPtr=colorImage.modifyPixels();
for(unsigned int y=0;y<colorFrameSize[1];++y)
for(unsigned int x=0;x<colorFrameSize[0];++x,sPtr+=3,++dPtr)
for(int i=0;i<3;++i)
(*dPtr)[i]=sPtr->rgb[i];
/* Write the RGB image to the file: */
Images::writeImageFile(colorImage,cbData->selectedFileName);
}
catch(std::runtime_error err)
{
/* Show an error message: */
Vrui::showErrorMessage("Save Color Frame...",Misc::printStdErrMsg("Could not write color frame file %s due to exception %s",cbData->getSelectedPath().c_str(),err.what()));
}
/* Destroy the file selection dialog: */
cbData->fileSelectionDialog->close();
}
void RawKinectViewer::saveColorFrameCallback(Misc::CallbackData* cbData)
{
try
{
/* Create a uniquely-named color image file in the current directory: */
IO::DirectoryPtr currentDir=Vrui::openDirectory(".");
std::string colorFrameFileName=currentDir->createNumberedFileName("ColorFrame.png",4);
/* Create a file selection dialog to select an alternative color frame file name: */
Misc::SelfDestructPointer<GLMotif::FileSelectionDialog> saveColorFrameDialog(new GLMotif::FileSelectionDialog(Vrui::getWidgetManager(),"Save Color Frame...",currentDir,colorFrameFileName.c_str(),".png"));
saveColorFrameDialog->getOKCallbacks().add(this,&RawKinectViewer::saveColorFrameOKCallback);
saveColorFrameDialog->deleteOnCancel();
/* Show the file selection dialog: */
Vrui::popupPrimaryWidget(saveColorFrameDialog.releaseTarget());
}
catch(std::runtime_error err)
{
/* Show an error message: */
Vrui::showErrorMessage("Save Color Frame...",Misc::printStdErrMsg("Could not save color frame due to exception %s",err.what()));
}
}
GLMotif::PopupMenu* RawKinectViewer::createMainMenu(void)
{
/* Create a popup shell to hold the main menu: */
GLMotif::PopupMenu* mainMenuPopup=new GLMotif::PopupMenu("MainMenuPopup",Vrui::getWidgetManager());
mainMenuPopup->setTitle("Raw Kinect Viewer");
/* Create the main menu itself: */
GLMotif::Menu* mainMenu=new GLMotif::Menu("MainMenu",mainMenuPopup,false);
/* Create a button to reset navigation: */
GLMotif::Button* resetNavigationButton=new GLMotif::Button("ResetNavigationButton",mainMenu,"Reset Navigation");
resetNavigationButton->getSelectCallbacks().add(this,&RawKinectViewer::resetNavigationCallback);
/* Create a button to capture a background frame: */
GLMotif::Button* captureBackgroundButton=new GLMotif::Button("CaptureBackgroundButton",mainMenu,"Capture Background");
captureBackgroundButton->getSelectCallbacks().add(this,&RawKinectViewer::captureBackgroundCallback);
/* Create a toggle button to enable/disable background removal: */
GLMotif::ToggleButton* removeBackgroundToggle=new GLMotif::ToggleButton("RemoveBackgroundToggle",mainMenu,"Remove Background");
removeBackgroundToggle->setToggle(camera->getRemoveBackground());
removeBackgroundToggle->getValueChangedCallbacks().add(this,&RawKinectViewer::removeBackgroundCallback);
/* Create a toggle button to calculate and show an averaged depth frame: */
GLMotif::ToggleButton* averageFramesButton=new GLMotif::ToggleButton("AverageFramesButton",mainMenu,"Average Frames");
averageFramesButton->getValueChangedCallbacks().add(this,&RawKinectViewer::averageFramesCallback);
/* Create a button to save the current averaged depth frame: */
GLMotif::Button* saveAverageFrameButton=new GLMotif::Button("SaveAverageFrameButton",mainMenu,"Save Average Frame");
saveAverageFrameButton->getSelectCallbacks().add(this,&RawKinectViewer::saveAverageFrameCallback);
/* Create a button to save the current color frame: */
GLMotif::Button* saveColorFrameButton=new GLMotif::Button("SaveColorFrameButton",mainMenu,"Save Color Frame");
saveColorFrameButton->getSelectCallbacks().add(this,&RawKinectViewer::saveColorFrameCallback);
/* Finish building the main menu: */
mainMenu->manageChild();
return mainMenuPopup;
}
GLMotif::PopupWindow* RawKinectViewer::createAverageDepthFrameDialog(void)
{
/* Create the average depth frame dialog window: */
GLMotif::PopupWindow* averageDepthFrameDialogPopup=new GLMotif::PopupWindow("AverageDepthFrameDialogPopup",Vrui::getWidgetManager(),"RawKinectViewer");
new GLMotif::Label("AverageDepthFrameLabel",averageDepthFrameDialogPopup,"Capturing average depth frame...");
return averageDepthFrameDialogPopup;
}
RawKinectViewer::RawKinectViewer(int& argc,char**& argv,char**& appDefaults)
:Vrui::Application(argc,argv,appDefaults),
camera(0),
colorFrameSize(0),colorFrameVersion(0),
depthFrameSize(0),depthCorrection(0),depthPlaneDistMax(10.0),depthFrameVersion(0),
paused(false),
averageNumFrames(150),averageFrameCounter(0),
averageFrameDepth(0),averageFrameForeground(0),
averageFrameValid(false),showAverageFrame(false),
depthPlaneValid(false),
mainMenu(0),averageDepthFrameDialog(0)
{
/*********************************************************************
Register the custom tool classes with the Vrui tool manager:
*********************************************************************/
/* Parse the command line: */
bool printHelp=false;
int cameraIndex=0; // Use first Kinect camera device on USB bus
Kinect::Camera::FrameSize selectedColorFrameSize=Kinect::Camera::FS_640_480;
Kinect::Camera::FrameSize selectedDepthFrameSize=Kinect::Camera::FS_640_480;
bool compressDepthFrames=false;
depthValueRange[0]=300.0f;
depthValueRange[1]=1100.0f; // float(Kinect::FrameSource::invalidDepth);
for(int i=1;i<argc;++i)
{
if(argv[i][0]=='-')
{
if(strcasecmp(argv[i]+1,"h")==0)
printHelp=true;
else if(strcasecmp(argv[i]+1,"high")==0)
{
selectedColorFrameSize=Kinect::Camera::FS_1280_1024;
// selectedDepthFrameSize=Kinect::Camera::FS_1280_1024;
}
else if(strcasecmp(argv[i]+1,"compress")==0)
compressDepthFrames=true;
else if(strcasecmp(argv[i]+1,"gridSize")==0)
{
// GridTool::setGridSize(atoi(argv[i+1]),atoi(argv[i+2]));
// i+=2;
}
else if(strcasecmp(argv[i]+1,"tileSize")==0)
{
// GridTool::setTileSize(atof(argv[i+1]),atof(argv[i+2]));
// i+=2;
}
else if(strcasecmp(argv[i]+1,"depthRange")==0)
{
for(int j=0;j<2;++j)
depthValueRange[j]=float(atof(argv[i+1+j]));
i+=2;
}
}
else
cameraIndex=atoi(argv[i]);
}
if(printHelp)
{
std::cout<<"Usage: RawKinectViewer [option 1] ... [option n] <camera index>"<<std::endl;
std::cout<<" <camera index>"<<std::endl;
std::cout<<" Selects the local Kinect camera of the given index (0: first camera on USB bus)"<<std::endl;
std::cout<<" Default: 0"<<std::endl;
std::cout<<" Options:"<<std::endl;
std::cout<<" -h"<<std::endl;
std::cout<<" Prints this help message"<<std::endl;
std::cout<<" -high"<<std::endl;
std::cout<<" Sets color frame size for the selected Kinect camera to 1280x1024 @ 15Hz"<<std::endl;
std::cout<<" -compress"<<std::endl;
std::cout<<" Requests compressed depth frames from the selected Kinect camera"<<std::endl;
std::cout<<" -gridSize <grid width> <grid height>"<<std::endl;
std::cout<<" Sets the number of tiles of the semi-transparent calibration grid"<<std::endl;
std::cout<<" Default: 7 5"<<std::endl;
std::cout<<" -tileSize <tile width> <tile height>"<<std::endl;
std::cout<<" Sets the size of each tile of the semi-transparent calibration grid"<<std::endl;
std::cout<<" Default: 3.5 3.5 (assumed to be inches)"<<std::endl;
std::cout<<" -depthRange <min depth> <max depth>"<<std::endl;
std::cout<<" Sets the range of depth values mapped to the full color range"<<std::endl;
std::cout<<" Default: 300 1100"<<std::endl;
}
/* Enable background USB event handling: */
usbContext.startEventHandling();
/* Connect to the given Kinect camera device on the host: */
camera=new Kinect::Camera(usbContext,cameraIndex);
/* Set the color camera's frame size: */
camera->setFrameSize(Kinect::FrameSource::COLOR,selectedColorFrameSize);
camera->setFrameSize(Kinect::FrameSource::DEPTH,selectedDepthFrameSize);
/* Get the cameras' actual frame sizes: */
colorFrameSize=camera->getActualFrameSize(Kinect::FrameSource::COLOR);
depthFrameSize=camera->getActualFrameSize(Kinect::FrameSource::DEPTH);
/* Get the camera's depth correction parameters: */
Kinect::FrameSource::DepthCorrection* dc=camera->getDepthCorrectionParameters();
/* Evaluate the camera's depth correction parameters into a per-pixel offset array: */
depthCorrection=dc->getPixelCorrection(depthFrameSize);
/* Clean up: */
delete dc;
/* Get the camera's intrinsic parameters: */
intrinsicParameters=camera->getIntrinsicParameters();
/* Allocate the average depth frame buffer: */
averageFrameDepth=new float[depthFrameSize[0]*depthFrameSize[1]];
averageFrameForeground=new float[depthFrameSize[0]*depthFrameSize[1]];
/* Set depth frame compression: */
camera->setCompressDepthFrames(compressDepthFrames);
/* Create the main menu: */
mainMenu=createMainMenu();
Vrui::setMainMenu(mainMenu);
averageDepthFrameDialog=createAverageDepthFrameDialog();
/* Start streaming: */
camera->startStreaming(Misc::createFunctionCall(this,&RawKinectViewer::colorStreamingCallback),Misc::createFunctionCall(this,&RawKinectViewer::depthStreamingCallback));
/* Select an invalid pixel: */
selectedPixel[0]=selectedPixel[1]=~0x0U;
/* Initialize navigation transformation: */
resetNavigationCallback(0);
}
RawKinectViewer::~RawKinectViewer(void)
{
delete mainMenu;
delete averageDepthFrameDialog;
delete[] averageFrameDepth;
delete[] averageFrameForeground;
/* Stop streaming: */
camera->stopStreaming();
/* Disconnect from the Kinect camera device: */
delete camera;
}
void RawKinectViewer::toolCreationCallback(Vrui::ToolManager::ToolCreationCallbackData* cbData)
{
/* Call the base class method: */
Vrui::Application::toolCreationCallback(cbData);
/* Check if the new tool is a locator tool: */
Vrui::LocatorTool* lt=dynamic_cast<Vrui::LocatorTool*>(cbData->tool);
if(lt!=0)
{
/* Register callbacks with the locator tool: */
lt->getButtonPressCallbacks().add(this,&RawKinectViewer::locatorButtonPressCallback);
}
}
void RawKinectViewer::frame(void)
{
/* Lock the most recent frame in the color frame triple buffer: */
if(colorFrames.lockNewValue())
++colorFrameVersion;
/* Lock the most recent frame in the depth frame triple buffer: */
if(depthFrames.lockNewValue())
{
++depthFrameVersion;
if(selectedPixel[0]!=~0x0U&&selectedPixel[1]!=~0x0U)
{
/* Update the selected pixel's EKG: */
++selectedPixelCurrentIndex;
if(selectedPixelCurrentIndex==128)
selectedPixelCurrentIndex=0;
const DepthPixel* dfPtr=static_cast<const DepthPixel*>(depthFrames.getLockedValue().getBuffer());
selectedPixelPulse[selectedPixelCurrentIndex]=dfPtr[selectedPixel[1]*depthFrames.getLockedValue().getSize(0)+selectedPixel[0]];
}
if(averageFrameCounter>0)
{
/* Accumulate the new depth frame into the averaging buffer: */
const DepthPixel* dfPtr=static_cast<const DepthPixel*>(depthFrames.getLockedValue().getBuffer());
float* afdPtr=averageFrameDepth;
float* affPtr=averageFrameForeground;
for(unsigned int y=0;y<depthFrameSize[1];++y)
for(unsigned int x=0;x<depthFrameSize[0];++x,++dfPtr,++afdPtr,++affPtr)
{
if(*dfPtr!=0x7ffU)
{
*afdPtr+=float(*dfPtr);
*affPtr+=1.0f;
}
}
--averageFrameCounter;
if(averageFrameCounter==0)
{
/* Mark the average frame buffer as valid: */
averageFrameValid=true;
/* Call all registered callbacks: */
for(std::vector<AverageFrameReadyCallback*>::iterator afrcIt=averageFrameReadyCallbacks.begin();afrcIt!=averageFrameReadyCallbacks.end();++afrcIt)
{
(**afrcIt)(0);
delete *afrcIt;
}
averageFrameReadyCallbacks.clear();
/* Hide the progress dialog: */
Vrui::popdownPrimaryWidget(averageDepthFrameDialog);
/* Invalidate the average depth frame immediately if it wasn't requested directly by the user: */
averageFrameValid=showAverageFrame;
}
}
}
}
void RawKinectViewer::display(GLContextData& contextData) const
{
/* Get the context data item: */
DataItem* dataItem=contextData.retrieveDataItem<DataItem>(this);
/* Save and set up OpenGL state: */
glPushAttrib(GL_ENABLE_BIT|GL_TEXTURE_BIT);
glDisable(GL_LIGHTING);
glEnable(GL_TEXTURE_2D);
glTexEnvi(GL_TEXTURE_ENV,GL_TEXTURE_ENV_MODE,GL_REPLACE);
glColor3f(1.0f,1.0f,1.0f);
/* Bind the depth texture object: */
glBindTexture(GL_TEXTURE_2D,dataItem->depthTextureId);
/* Check if the cached depth frame needs to be updated: */
if(showAverageFrame&&averageFrameValid)
{
/* Convert the averaged depth image to RGB: */
unsigned int width=depthFrameSize[0];
unsigned int height=depthFrameSize[1];
GLubyte* byteFrame=new GLubyte[height*width*3];
const float* afdPtr=averageFrameDepth;
const float* affPtr=averageFrameForeground;
float foregroundCutoff=float(averageNumFrames)*0.5f;
const PixelCorrection* dcPtr=depthCorrection;
GLubyte* bfPtr=byteFrame;
for(unsigned int y=0;y<height;++y)
for(unsigned int x=0;x<width;++x,++afdPtr,++affPtr,++dcPtr,bfPtr+=3)
{
if(*affPtr>=foregroundCutoff)
{
float d=dcPtr->correct((*afdPtr)/(*affPtr));
mapDepth(x,y,d,bfPtr);
}
else
bfPtr[0]=bfPtr[1]=bfPtr[2]=GLubyte(0);
}
/* Set up the texture parameters: */
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_BASE_LEVEL,0);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAX_LEVEL,0);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_CLAMP);
/* Upload the depth texture image: */
glTexSubImage2D(GL_TEXTURE_2D,0,0,0,width,height,GL_RGB,GL_UNSIGNED_BYTE,byteFrame);
delete[] byteFrame;
}
else
{
if(dataItem->depthFrameVersion!=depthFrameVersion)
{
/* Upload the depth frame into the texture object: */
const Kinect::FrameBuffer& depthFrame=depthFrames.getLockedValue();
unsigned int width=depthFrameSize[0];
unsigned int height=depthFrameSize[1];
const GLushort* framePtr=static_cast<const GLushort*>(depthFrame.getBuffer());
/* Convert the depth image to unsigned byte: */
GLubyte* byteFrame=new GLubyte[height*width*3];
const GLushort* fPtr=framePtr;
const PixelCorrection* dcPtr=depthCorrection;
GLubyte* bfPtr=byteFrame;
for(unsigned int y=0;y<height;++y)
for(unsigned int x=0;x<width;++x,++fPtr,++dcPtr,bfPtr+=3)
{
if(*fPtr!=Kinect::FrameSource::invalidDepth)
{
float d=dcPtr->correct(*fPtr);
mapDepth(x,y,d,bfPtr);
}
else
bfPtr[0]=bfPtr[1]=bfPtr[2]=GLubyte(0);
}
/* Set up the texture parameters: */
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_BASE_LEVEL,0);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAX_LEVEL,0);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_CLAMP);
/* Upload the depth texture image: */
glTexSubImage2D(GL_TEXTURE_2D,0,0,0,width,height,GL_RGB,GL_UNSIGNED_BYTE,byteFrame);
delete[] byteFrame;
/* Mark the cached depth frame as up-to-date: */
dataItem->depthFrameVersion=depthFrameVersion;
}
}
/* Draw the depth image: */
glBegin(GL_QUADS);
glTexCoord2f(0.0f,0.0f);
glVertex2f(-GLfloat(depthFrameSize[0]),0.0f);
glTexCoord2f(GLfloat(depthFrameSize[0])/GLfloat(dataItem->depthTextureSize[0]),0.0f);
glVertex2f(0.0f,0.0f);
glTexCoord2f(GLfloat(depthFrameSize[0])/GLfloat(dataItem->depthTextureSize[0]),GLfloat(depthFrameSize[1])/GLfloat(dataItem->depthTextureSize[1]));
glVertex2f(0.0f,GLfloat(depthFrameSize[1]));
glTexCoord2f(0.0f,GLfloat(depthFrameSize[1])/GLfloat(dataItem->depthTextureSize[1]));
glVertex2f(-GLfloat(depthFrameSize[0]),GLfloat(depthFrameSize[1]));
glEnd();
/* Bind the color texture object: */
glBindTexture(GL_TEXTURE_2D,dataItem->colorTextureId);
/* Check if the cached color frame needs to be updated: */
if(dataItem->colorFrameVersion!=colorFrameVersion)
{
/* Upload the color frame into the texture object: */
const Kinect::FrameBuffer& colorFrame=colorFrames.getLockedValue();
unsigned int width=colorFrameSize[0];
unsigned int height=colorFrameSize[1];
const GLubyte* framePtr=static_cast<const GLubyte*>(colorFrame.getBuffer());
/* Set up the texture parameters: */
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_BASE_LEVEL,0);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAX_LEVEL,0);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_CLAMP);
/* Upload the color texture image: */
glTexSubImage2D(GL_TEXTURE_2D,0,0,0,width,height,GL_RGB,GL_UNSIGNED_BYTE,framePtr);
/* Mark the cached color frame as up-to-date: */
dataItem->colorFrameVersion=colorFrameVersion;
}
/* Draw the color image: */
glBegin(GL_QUADS);
glTexCoord2f(0.0f,0.0f);
glVertex2f(0.0f,0.0f);
glTexCoord2f(GLfloat(colorFrameSize[0])/GLfloat(dataItem->colorTextureSize[0]),0.0f);
glVertex2f(GLfloat(colorFrameSize[0]),0.0f);
glTexCoord2f(GLfloat(colorFrameSize[0])/GLfloat(dataItem->colorTextureSize[0]),GLfloat(colorFrameSize[1])/GLfloat(dataItem->colorTextureSize[1]));
glVertex2f(GLfloat(colorFrameSize[0]),GLfloat(colorFrameSize[1]));
glTexCoord2f(0.0f,GLfloat(colorFrameSize[1])/GLfloat(dataItem->colorTextureSize[1]));
glVertex2f(0.0f,GLfloat(colorFrameSize[1]));
glEnd();
/* Protect the texture objects: */
glBindTexture(GL_TEXTURE_2D,0);
if(selectedPixel[0]!=~0x0U&&selectedPixel[1]!=~0x0U)
{
/* Draw the selected pixel: */
glDisable(GL_TEXTURE_2D);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(0.0f,1.0f,0.0f);
GLfloat spx=GLfloat(selectedPixel[0])-GLfloat(depthFrameSize[0])+0.5f;
GLfloat spy=GLfloat(selectedPixel[1])+0.5f;
glVertex3f(spx-5.0f,spy,0.1f);
glVertex3f(spx+5.0f,spy,0.1f);
glVertex3f(spx,spy-5.0f,0.1f);
glVertex3f(spx,spy+5.0f,0.1f);
glEnd();
/* Draw the selected pixel's EKG: */
glBegin(GL_LINE_STRIP);
for(int i=0;i<128;++i)
glVertex3f(GLfloat(i)*depthFrameSize[0]/128.0f-depthFrameSize[0],GLfloat(selectedPixelPulse[i])*0.25-512.0f,0.1f);
glEnd();
}
/* Restore OpenGL state: */
glPopAttrib();
}
void RawKinectViewer::initContext(GLContextData& contextData) const
{
/* Create and register the data item: */
DataItem* dataItem=new DataItem;
contextData.addDataItem(this,dataItem);
/* Check for NPOTD texture support and initialize the padded texture sizes: */
if(GLARBTextureNonPowerOfTwo::isSupported())
{
/* Initialize the extension: */
GLARBTextureNonPowerOfTwo::initExtension();
/* Use actual image sizes as texture sizes: */
for(int i=0;i<2;++i)
{
dataItem->colorTextureSize[i]=colorFrameSize[i];
dataItem->depthTextureSize[i]=depthFrameSize[i];
}
}
else
{
/* Pad image sizes to the next-larger power of two: */
for(int i=0;i<2;++i)
{
for(dataItem->colorTextureSize[i]=1;dataItem->colorTextureSize[i]<colorFrameSize[i];dataItem->colorTextureSize[i]<<=1)
;
for(dataItem->depthTextureSize[i]=1;dataItem->depthTextureSize[i]<depthFrameSize[i];dataItem->depthTextureSize[i]<<=1)
;
}
}
/* Prepare the depth texture: */
glBindTexture(GL_TEXTURE_2D,dataItem->depthTextureId);
glTexImage2D(GL_TEXTURE_2D,0,GL_RGB8,dataItem->depthTextureSize[0],dataItem->depthTextureSize[1],0,GL_RGB,GL_UNSIGNED_BYTE,0);
/* Prepare the color texture: */
glBindTexture(GL_TEXTURE_2D,dataItem->colorTextureId);
glTexImage2D(GL_TEXTURE_2D,0,GL_RGB8,dataItem->colorTextureSize[0],dataItem->colorTextureSize[1],0,GL_RGB,GL_UNSIGNED_BYTE,0);
/* Protect the texture images: */
glBindTexture(GL_TEXTURE_2D,0);
}