mrptでDepth画像を取得する
訳あって懲りずにmrptいじくってます。
KinectのDepthデータをOpenCVで扱いたいと思って、いろいろ試してみましたが、うまくいかなくて、結局mrptで書いたのが動いたのでソースコードを載せておきます。
首動かせるよ、みたいなソースコード書いてありますが、動かないのであしからず。
環境
ソースコード
/**
* @file main.cpp
*/
#include "LogCommon.h"
#include <opencv2/opencv.hpp>
#include <mrpt/hwdrivers.h>
#define LOG INF_LOG // Log Utiliry Macro.
//---------------------------------------------------------------
// constant
//---------------------------------------------------------------
static const int DEPTH_WIDTH = 640;
static const int DEPTH_HEIGHT = 480;
static const double MAX_TILT = 30.0;
static const double MIN_TILT = -30.0;
static const char* wndNameDepth = "DEPTH";
//---------------------------------------------------------------
// definition
//---------------------------------------------------------------
struct TThreadParam
{
TThreadParam() : bQuit(false), dTiltDeg(0.0), iKeyPushed(0) {}
volatile bool bQuit;
volatile double dTiltDeg;
volatile int iKeyPushed;
mrpt::synch::CThreadSafeVariable<mrpt::slam::CObservation3DRangeScanPtr> pObsSafeData;
};
//---------------------------------------------------------------
// prototype
//---------------------------------------------------------------
// Initialize
int init();
// Capture Depth
void thread_grab(TThreadParam& pThParam);
// Point Cloud To Image
long ToImage(const mrpt::slam::CObservation3DRangeScanPtr p3dData, cv::Mat& depthImage);
// Depth To Pixel
long DepthToPixel(float src, unsigned char& dst);
// Main Procedure
int main(void)
{
init();
// Create Thread of Capture
TThreadParam thParam;
mrpt::system::TThreadHandle threadHandle = mrpt::system::createThreadRef( thread_grab, thParam );
// Init Window
cv::Mat depthImage( DEPTH_HEIGHT, DEPTH_WIDTH, CV_8U );
// Main Loop.
int mainCout = 0;
while(!thParam.bQuit){
// Retrieve Data.
mrpt::slam::CObservation3DRangeScanPtr obs3dScanData = thParam.pObsSafeData.get();
if( (obs3dScanData == NULL) || (obs3dScanData->hasPoints3D == false) ){
continue;
}
// 3D Range to Depth Image.
ToImage(obs3dScanData, depthImage);
cv::imshow( wndNameDepth, depthImage );
int key = cv::waitKey(30);
if(key == 'q'){
// QUIT.
thParam.bQuit = true;
break;
}else if(key == 38){ // Up Key.
LOG(" Input Up Key.");
if(thParam.dTiltDeg <= MAX_TILT){
thParam.dTiltDeg += 2.0;
LOG(" >> Up");
}
}else if(key == 40){ // Down Key.
LOG(" Input Down Key.");
if(thParam.dTiltDeg <= MIN_TILT){
thParam.dTiltDeg -= 2.0;
LOG(" >> Down");
}
}
thParam.iKeyPushed = key;
mainCout++;
}
mrpt::system::joinThread(threadHandle);
return 0;
}
void thread_grab(TThreadParam& pThParam)
{
// Kinect Controller Class
mrpt::hwdrivers::CKinect cKinectSensor;
cKinectSensor.initialize();
LOG(" Kinect Sensor initialize OK");
// Capture Loop.
int cnt = 0;
while(!pThParam.bQuit){
mrpt::slam::CObservation3DRangeScanPtr p3dRangeScan = mrpt::slam::CObservation3DRangeScan::Create();
bool bObsResult = false;
bool bHardError = false;
cKinectSensor.getNextObservation( *p3dRangeScan, bObsResult, bHardError); // Get Point Cloud.
if(p3dRangeScan->hasPoints3D == true){
pThParam.pObsSafeData.set( p3dRangeScan );
}else{
LOG(" ERR: p3dRangeScan do NOT has Point 3D data.");
}
// Move Direction.
if(pThParam.iKeyPushed != 0){
if( (pThParam.dTiltDeg <= MAX_TILT) && (pThParam.dTiltDeg >= MIN_TILT) ){
cKinectSensor.setTiltAngleDegrees( pThParam.dTiltDeg );
}
pThParam.iKeyPushed = 0;
}
cnt++;
}
}
long ToImage(const mrpt::slam::CObservation3DRangeScanPtr p3dData, cv::Mat& depthImage)
{
// Check Size.
if( (p3dData->points3D_x.size() != p3dData->points3D_y.size() ) ||
(p3dData->points3D_y.size() != p3dData->points3D_z.size() ) )
{
LOG(" ERR: ScanData has Invalid Data.");
return -1;
}
if(p3dData->points3D_x.size() != (depthImage.cols * depthImage.rows)){
LOG(" ERR: ScanData size is Invalid. size=%d.", p3dData->points3D_x.size());
return -1;
}
// Loop for Each Pixels.
for(int y=0; y<depthImage.rows; y++){
for(int x=0; x<depthImage.cols; x++){
const int idx = y * depthImage.cols + x;
float srcVal = p3dData->points3D_x[ idx ];
DepthToPixel( srcVal, *(depthImage.ptr( y, x)) );
}
}
return 0;
}
long DepthToPixel(float src, unsigned char& dst)
{
static const double dCoeff = 100.0; // T.B.D.
const int iVal = (int) (src * dCoeff);
dst = (unsigned char)iVal;
return 0;
}
int init()
{
setLogPath("test.log", true); // Initialize Log Utility.
cv::namedWindow( wndNameDepth );
return 0;
}
Makefile
LogCommon.cppはオレオレ用ログユーティリティです。無視してください。
CC = g++ -O SRC = main.cpp \ Util/LogCommon.cpp TGT = KinectCapTest USR_DIR = /usr INCPATH = $(USR_DIR)/include LIBPATH = $(USR_DIR)/lib OPT = -lstdc++ \ -lopencv_core -lopencv_highgui \ -lopencv_imgproc -lopencv_legacy \ -lmrpt-base \ -lmrpt-obs -lmrpt-slam \ -lmrpt-hwdrivers \ -lmrpt-gui -lmrpt-vision CFLAGS = -I$(INCPATH) \ -I$(INCPATH)/mrpt/base/include \ -I$(INCPATH)/mrpt/slam/include \ -I$(INCPATH)/mrpt/maps/include \ -I$(INCPATH)/mrpt/detectors/include \ -I$(INCPATH)/mrpt/opengl/include \ -I$(INCPATH)/mrpt/mrpt-config \ -I$(INCPATH)/mrpt/hmtslam/include \ -I$(INCPATH)/mrpt/gui/include \ -I$(INCPATH)/mrpt/obs/include \ -I$(INCPATH)/mrpt/topography/include \ -I$(INCPATH)/mrpt/hwdrivers/include \ -I$(INCPATH)/mrpt/bayes/include \ -I$(INCPATH)/mrpt/vision/include \ -I$(INCPATH)/mrpt/reactivenav/include \ -I$(INCPATH)/mrpt/scanmatching/include \ -I./Util LDFLAGS = -L. -L$(LIBPATH) all: $(CC) $(SRC) -o $(TGT) $(CFLAGS) $(LDFLAGS) $(OPT) clean: rm $(TGT)
