/** * @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(); }