/**
* @function saveData
* @brief Save rgb and depth matrix + pointcloud + rgb with different overlay colors on the object
*/
void saveData() {
int i = gSelectedCluster;
const int num_overlays = 7;
uchar r[num_overlays] = {255,255,0,0,255,255,0};
uchar g[num_overlays] = {255,0,255,0,255,0,255};
uchar b[num_overlays] = {255,0,0,255,0,255,255};
// Save rgb sub-img
cv::Mat img( gRgbImg, cv::Rect(gClustersBB[i](0), gClustersBB[i](1),
gClustersBB[i](2) - gClustersBB[i](0),
gClustersBB[i](3) - gClustersBB[i](1) ) );
char name[50];
sprintf( name, "%s_rgb_%d.png",gName.c_str(), gCounter );
cv::imwrite( name, img );
// Save depth sub-img
cv::Mat dimg( gXyzImg, cv::Rect(gClustersBB[i](0), gClustersBB[i](1),
gClustersBB[i](2) - gClustersBB[i](0),
gClustersBB[i](3) - gClustersBB[i](1) ) );
sprintf( name, "%s_xyz_%d.yml", gName.c_str(), gCounter );
cv::FileStorage fs( name, cv::FileStorage::WRITE );
fs << "xyzMatrix" << dimg; fs.release();
// Save pointcloud
sprintf( name, "%s_%d.pcd", gName.c_str(), gCounter );
pcl::io::savePCDFileASCII(name, *gTts.getCluster(i));
// Optionally, save rgb img with different overlays
pcl::PointIndices ind = gTts.getClusterIndices(i);
cv::Mat m = gRgbImg.clone();
for( int j = 0; j < num_overlays; ++j ) {
// Overlay color
for( int k = 0; k < ind.indices.size(); ++k ) {
int row, col;
row = ind.indices[k] / 640;
col = ind.indices[k] % 640;
uchar ri, gi, bi;
ri = (uchar)( ( gRgbImg.at<cv::Vec3b>(row,col)(2) + r[j] ) / 2 );
gi = (uchar)( ( gRgbImg.at<cv::Vec3b>(row,col)(1) + g[j] ) / 2 );
bi = (uchar)( ( gRgbImg.at<cv::Vec3b>(row,col)(0) + b[j] ) / 2 );
m.at<cv::Vec3b>(row, col) = cv::Vec3b(bi,gi,ri);
}
sprintf( name, "%s_rgb_overlay_%d_%d.png", gName.c_str(), j, gCounter );
cv::imwrite( name, cv::Mat(m,cv::Rect(gClustersBB[i](0),
gClustersBB[i](1),
gClustersBB[i](2) - gClustersBB[i](0),
gClustersBB[i](3) - gClustersBB[i](1) )) );
}
printf("Saved %s counter %d \n", gName.c_str(), gCounter );
// Augment the counter
gCounter++;
}
void KinectDataMan::ShowColorDepth()
{
// init kinect and connect
if( m_cvhelper.IsInitialized() )
return;
m_cvhelper.SetColorFrameResolution(color_reso);
m_cvhelper.SetDepthFrameResolution(depth_reso);
HRESULT hr;
// Get number of Kinect sensors
int sensorCount = 0;
hr = NuiGetSensorCount(&sensorCount);
if (FAILED(hr))
{
return;
}
// If no sensors, update status bar to report failure and return
if (sensorCount == 0)
{
cerr<<"No kinect"<<endl;
}
// Iterate through Kinect sensors until one is successfully initialized
for (int i = 0; i < sensorCount; ++i)
{
INuiSensor* sensor = NULL;
hr = NuiCreateSensorByIndex(i, &sensor);
if (SUCCEEDED(hr))
{
hr = m_cvhelper.Initialize(sensor);
if (SUCCEEDED(hr))
{
// Report success
cerr<<"Kinect initialized"<<endl;
break;
}
else
{
// Uninitialize KinectHelper to show that Kinect is not ready
m_cvhelper.UnInitialize();
return;
}
}
}
DWORD width, height;
m_cvhelper.GetColorFrameSize(&width, &height);
Size colorsz(width, height);
Mat cimg(colorsz, m_cvhelper.COLOR_TYPE);
m_cvhelper.GetDepthFrameSize(&width, &height);
Size depthsz(width, height);
Mat dimg(depthsz, m_cvhelper.DEPTH_RGB_TYPE);
// start processing
while(true)
{
// get color frame
if( SUCCEEDED(m_cvhelper.UpdateColorFrame()) )
{
HRESULT hr = m_cvhelper.GetColorImage(&cimg);
if(FAILED(hr))
break;
imshow("color", cimg);
if( waitKey(10) == 'q' )
break;
}
if( SUCCEEDED(m_cvhelper.UpdateDepthFrame()) )
{
HRESULT hr = m_cvhelper.GetDepthImageAsArgb(&dimg);
if(FAILED(hr))
break;
imshow("depth", dimg);
if( waitKey(10) == 'q' )
break;
}
}
destroyAllWindows();
}
