int main (int argc, char** argv){
//determine number of frames to write to .pcd
int range = 1;
if(argc > 1){
range = atoi(argv[1]);
}
static std::vector<uint16_t> mdepth(640*480);
static std::vector<uint8_t> mrgb(640*480*4);
// Fill in the cloud data
cloud->width = 640;
cloud->height = 480;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
device = &freenect.createDevice<okpcl::MyFreenectDevice>(0);
device->startVideo();
device->startDepth();
boost::this_thread::sleep (boost::posix_time::seconds (1));
//Grab until clean returns
int DepthCount = 0;
while (DepthCount == 0) {
device->updateState();
device->getDepth(mdepth);
device->getRGB(mrgb);
for (size_t i = 0;i < 480*640;i++) DepthCount+=mdepth[i];
}
device->setVideoFormat(requested_format);
//--------------------
// -----Main loop-----
//--------------------
double x = NULL;
double y = NULL;
int iRealDepth = 0;
int timestamp=0;
int counter=1;
int frame;
for (frame=0;frame<range;frame++)
{
device->updateState();
device->getDepth(mdepth);
device->getRGB(mrgb);
size_t i = 0;
size_t cinput = 0;
for (size_t v=0 ; v<480 ; v++)
{
for ( size_t u=0 ; u<640 ; u++, i++)
{
iRealDepth = mdepth[i];
freenect_camera_to_world(device->getDevice(), u, v, iRealDepth, &x, &y);
cloud->points[i].x = x;
cloud->points[i].y = y;
cloud->points[i].z = iRealDepth;
cloud->points[i].r = mrgb[i*3];
cloud->points[i].g = mrgb[(i*3)+1];
cloud->points[i].b = mrgb[(i*3)+2];
}
}
timestamp = std::time(0);
std::string filename = "../../../../data/pcl/pcd/";
filename.append(boost::lexical_cast<std::string>(timestamp));
filename.append("_"+boost::lexical_cast<std::string>(cloud_id));
filename.append(".pcd");
pcl::io::savePCDFileASCII (filename, *cloud);
cloud_id++;
}
printf("render loop finished\n");
device->stopVideo();
device->stopDepth();
return 0;
}
//------------------------------------
ofVec3f ofxKinect::getWorldCoordinateAt(float cx, float cy, float wz) {
double wx, wy;
freenect_camera_to_world(kinectDevice, cx, cy, wz, &wx, &wy);
return ofVec3f(wx, wy, wz);
}
/*---------------------------------------------*-
* MAIN
* Steak?
-*---------------------------------------------*/
int main (int argc, char** argv)
{
printf("Hello\n");
// Some more Kinect setup
static std::vector<uint16_t> mdepth(640*480);
static std::vector<uint8_t> mrgb(640*480*4);
// Fill in the cloud data
printf("Make a cloud\n");
cloud->width = 640;
cloud->height = 480;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
// Create and setup the viewer
printf("Make a viewer\n");
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);
viewer->setBackgroundColor (255, 255, 255);
viewer->addPointCloud<pcl::PointXYZRGB> (cloud, "Kinect Cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1, "Kinect Cloud");
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
// Actually starting up the Kinect
printf("Start the Kinect\n");
device = &freenect.createDevice<MyFreenectDevice>(0);
device->startVideo();
device->startDepth();
boost::this_thread::sleep (boost::posix_time::seconds (1));
// Grab until clean returns...
printf("Start Kinect checks\n");
int DepthCount = 0;
int check = 0;
while (DepthCount == 0) {
printf("\tchecking... %d\n", check++);
device->updateState();
device->getDepth(mdepth);
device->getRGB(mrgb);
for (size_t i = 0;i < 480*640;i++) DepthCount+=mdepth[i];
}
device->setVideoFormat(requested_format);
// Main Loop. Mmmmm continuous steak.
printf("Main Loop Time\n");
double x = NULL;
double y = NULL;
int iRealDepth = 0;
while (!viewer->wasStopped ())
{
device->updateState(); // Use this to make sure the Kinect is still alive.
device->getDepth(mdepth);
device->getRGB(mrgb);
size_t i = 0;
size_t cinput = 0;
for (size_t v=0 ; v<480 ; v++)
{
for ( size_t u=0 ; u<640 ; u++, i++)
{
iRealDepth = mdepth[i];
freenect_camera_to_world(device->getDevice(), u, v, iRealDepth, &x, &y);
cloud->points[i].x = x;
cloud->points[i].y = y;
cloud->points[i].z = iRealDepth;
cloud->points[i].r = mrgb[i*3];
cloud->points[i].g = mrgb[(i*3)+1];
cloud->points[i].b = mrgb[(i*3)+2];
}
}
// Table & Object Detection
if (do_detection) {
printf("Doing some detection...\n");
done_ransac = false;
done_detection = false;
// Create the segmentation object and do RANSAC
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
std::cerr << "PointCloud after segmentation has: " << inliers->indices.size () << " inliers." << std::endl;
std::cerr << "Plane coefficients: a:" << coefficients->values[0] <<
" b:" << coefficients->values[1] <<
" c:" << coefficients->values[2] <<
" d:" << coefficients->values[3] << std::endl;
done_ransac = true;
double boxSize = 300.0;
double bl_x = -boxSize, bl_y = -boxSize;
double tl_x = -boxSize, tl_y = boxSize;
double tr_x = boxSize, tr_y = boxSize;
double br_x = boxSize, br_y = -boxSize;
// ax + by + cz + d = 0
// ax + by + d = -cz
// -(ax + by + d)/c = z
double bl_z = -((coefficients->values[0]*bl_x) + (coefficients->values[1]*bl_y) + coefficients->values[3])/(coefficients->values[2]*1.0);
double tl_z = -((coefficients->values[0]*tl_x) + (coefficients->values[1]*tl_y) + coefficients->values[3])/(coefficients->values[2]*1.0);
double tr_z = -((coefficients->values[0]*tr_x) + (coefficients->values[1]*tr_y) + coefficients->values[3])/(coefficients->values[2]*1.0);
double br_z = -((coefficients->values[0]*br_x) + (coefficients->values[1]*br_y) + coefficients->values[3])/(coefficients->values[2]*1.0);
std::cerr << "Plane in 3D is BL: x:" << bl_x << " y:" << bl_y << " z:" << bl_z << std::endl;
std::cerr << " TL: x:" << tl_x << " y:" << tl_y << " z:" << tl_z << std::endl;
std::cerr << " TR: x:" << tr_x << " y:" << tr_y << " z:" << tr_z << std::endl;
std::cerr << " BR: x:" << br_x << " y:" << br_y << " z:" << br_z << std::endl;
std::cerr << "Off-Mid Point (" << (size_t)(cloud->size()/2+(640*100+46)) <<
") is x:" << cloud->points[(size_t)(cloud->size()/2+(640*100+46))].x <<
" y:" << cloud->points[(size_t)(cloud->size()/2+(640*100+46))].y <<
" z:" << cloud->points[(size_t)(cloud->size()/2+(640*100+46))].z << std::endl;
done_detection = true;
do_detection = false;
viewer->addPlane(*coefficients, "plane", 0);
printf("Detection complete.\n");
}
viewer->updatePointCloud (cloud, "Kinect Cloud");
viewer->spinOnce ();
}
printf("render loop finished\n");
device->stopVideo();
device->stopDepth();
printf("Goodbye\n");
return 0;
}
// --------------
// -----Main-----
// --------------
int main (int argc, char** argv)
{
//More Kinect Setup
static std::vector<uint16_t> mdepth(640*480);
static std::vector<uint8_t> mrgb(640*480*4);
static std::vector<uint16_t> tdepth(640*480);
static std::vector<uint8_t> trgb(640*480*4);
// Fill in the cloud data
cloud->width = 640;
cloud->height = 480;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
// Fill in the cloud data
cloud2->width = 640;
cloud2->height = 480;
cloud2->is_dense = false;
cloud2->points.resize (cloud2->width * cloud2->height);
//Calibrate the new camera position over
//-53.5 cm in X
//+45 degrees about Y
// Eigen::Affine3f twotrans = pcl::getTransformation(378.3, 0.0, 378.3, 0.0, -0.785398163, 0.0);
Eigen::Affine3f twotrans = pcl::getTransformation(535.0, 0.0, 0.0, 0.0, -0.785398163, 0.0);
// Create and setup the viewer
printf("Create the viewer.\n");
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);
viewer->setBackgroundColor (0, 0, 0);
viewer->addPointCloud<pcl::PointXYZRGB> (cloud, "Kinect Cloud");
viewer->addPointCloud<pcl::PointXYZRGB> (cloud2, "Second Cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "Kinect Cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "Second Cloud");
viewer->addCoordinateSystem (1.0, 0);
viewer->initCameraParameters ();
//Voxelizer Setup
pcl::VoxelGrid<pcl::PointXYZRGB> vox;
printf("Create the devices.\n");
device = &freenect.createDevice<MyFreenectDevice>(0);
devicetwo = &freenect.createDevice<MyFreenectDevice>(1);
device->startVideo();
device->startDepth();
boost::this_thread::sleep (boost::posix_time::seconds (1));
devicetwo->startVideo();
devicetwo->startDepth();
boost::this_thread::sleep (boost::posix_time::seconds (1));
//Grab until clean returns
int DepthCount = 0;
while (DepthCount == 0) {
device->updateState();
device->getDepth(mdepth);
device->getRGB(mrgb);
for (size_t i = 0;i < 480*640;i++)
DepthCount+=mdepth[i];
}
//--------------------
// -----Main loop-----
//--------------------
double x = NULL;
double y = NULL;
double tx = NULL;
double ty = NULL;
int iRealDepth = 0;
int iTDepth = 0;
device->setVideoFormat(requested_format);
devicetwo->setVideoFormat(requested_format);
printf("Start the main loop.\n");
while (!viewer->wasStopped ())
{
device->updateState();
device->getDepth(mdepth);
device->getRGB(mrgb);
devicetwo->updateState();
devicetwo->getDepth(tdepth);
devicetwo->getRGB(trgb);
size_t i = 0;
size_t cinput = 0;
for (size_t v=0 ; v<480 ; v++)
{
for ( size_t u=0 ; u<640 ; u++, i++)
{
//pcl::PointXYZRGB result;
iRealDepth = mdepth[i];
iTDepth = tdepth[i];
//DepthCount+=iRealDepth;
//printf("fRealDepth = %f\n",fRealDepth);
//fflush(stdout);
freenect_camera_to_world(device->getDevicePtr(), u, v, iRealDepth, &x, &y);
freenect_camera_to_world(devicetwo->getDevicePtr(), u, v, iTDepth, &tx, &ty);
cloud->points[i].x = x;//1000.0;
cloud->points[i].y = y;//1000.0;
cloud->points[i].z = iRealDepth;//1000.0;
cloud->points[i].r = mrgb[i*3];
cloud->points[i].g = mrgb[(i*3)+1];
cloud->points[i].b = mrgb[(i*3)+2];
cloud2->points[i].x = tx;//1000.0;
cloud2->points[i].y = ty;//1000.0;
cloud2->points[i].z = iTDepth;//1000.0;
cloud2->points[i].r = trgb[i*3];
cloud2->points[i].g = trgb[(i*3)+1];
cloud2->points[i].b = trgb[(i*3)+2];
//cloud->points[i] = result;
//printf("x,y,z = %f,%f,%f\n",x,y,iRealDepth);
//printf("RGB = %d,%d,%d\n", mrgb[i*3],mrgb[(i*3)+1],mrgb[(i*3)+2]);
}
}
pcl::transformPointCloud (*cloud2, *cloud2, twotrans);
if (BackgroundSub) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr fgcloud (new pcl::PointCloud<pcl::PointXYZRGB>);
octree.deleteCurrentBuffer();
// Add points from background to octree
octree.setInputCloud (bgcloud);
octree.addPointsFromInputCloud ();
// Switch octree buffers: This resets octree but keeps previous tree structure in memory.
octree.switchBuffers ();
// Add points from the mixed data to octree
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();
std::vector<int> newPointIdxVector;
// Get vector of point indices from octree voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
for (size_t i = 0; i < newPointIdxVector.size(); ++i) {
fgcloud->push_back(cloud->points[newPointIdxVector[i]]);
}
viewer->updatePointCloud (fgcloud, "Kinect Cloud");
}
else if (Voxelize) {
vox.setInputCloud (cloud);
vox.setLeafSize (50.0f, 50.0f, 50.0f);
vox.filter (*voxcloud);
viewer->updatePointCloud (voxcloud, "Kinect Cloud");
}
else
{
viewer->updatePointCloud (cloud, "Kinect Cloud");
viewer->updatePointCloud (cloud2, "Second Cloud");
}
viewer->spinOnce ();
}
device->stopVideo();
device->stopDepth();
devicetwo->stopVideo();
devicetwo->stopDepth();
return 0;
}
int main (int argc, char** argv)
{
//More Kinect Setup
static std::vector<uint16_t> mdepth(640*480);
static std::vector<uint8_t> mrgb(640*480*4);
// Fill in the cloud data
cloud->width = 640;
cloud->height = 480;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
// Create and setup the viewer
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);
viewer->setBackgroundColor (0, 0, 0);
viewer->addPointCloud<pcl::PointXYZRGB> (cloud, "Kinect Cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1, "Kinect Cloud");
viewer->addCoordinateSystem (1.0, 0);
viewer->initCameraParameters ();
device = &freenect.createDevice<MyFreenectDevice>(0);
device->startVideo();
device->startDepth();
boost::this_thread::sleep (boost::posix_time::seconds (1));
//Grab until clean returns
int DepthCount = 0;
while (DepthCount == 0) {
device->updateState();
device->getDepth(mdepth);
device->getRGB(mrgb);
for (size_t i = 0;i < 480*640;i++) DepthCount+=mdepth[i];
}
device->setVideoFormat(requested_format);
//--------------------
// -----Main loop-----
//--------------------
double x = NULL;
double y = NULL;
int iRealDepth = 0;
while (!viewer->wasStopped ())
{
device->updateState();
device->getDepth(mdepth);
device->getRGB(mrgb);
size_t i = 0;
size_t cinput = 0;
for (size_t v=0 ; v<480 ; v++)
{
for ( size_t u=0 ; u<640 ; u++, i++)
{
iRealDepth = mdepth[i];
freenect_camera_to_world(device->getDevicePtr(), u, v, iRealDepth, &x, &y);
cloud->points[i].x = x;
cloud->points[i].y = y;
cloud->points[i].z = iRealDepth;
cloud->points[i].r = mrgb[i*3];
cloud->points[i].g = mrgb[(i*3)+1];
cloud->points[i].b = mrgb[(i*3)+2];
}
}
viewer->updatePointCloud (cloud, "Kinect Cloud");
viewer->spinOnce ();
}
printf("render loop finished\n");
device->stopVideo();
device->stopDepth();
return 0;
}
int freenect_sync_camera_to_world(int cx, int cy, int wz, double* wx, double* wy, int index) {
if (runloop_enter(index)) return -1;
freenect_camera_to_world(kinects[index]->dev, cx, cy, wz, wx, wy);
runloop_exit();
return 0;
}
