int main (int argc,char* argv[]){
if (argc != 2 && argc != 3){
printf("usage:\n %s /path/to/recoding/filename.oni\n",argv[0]);
return 0;
}
Xn_sensor sensor(WIDTH,HEIGHT);
sensor.play(argv[1],false);
cvNamedWindow( "Model Extractor Viewer", 1 );
IplImage* rgb_image = cvCreateImageHeader(cvSize(WIDTH,HEIGHT), 8, 3);
IplImage* test = cvCreateImageHeader(cvSize(WIDTH,HEIGHT), 8, 3);
IplImage* gray = cvCreateImage(cvSize(WIDTH,HEIGHT), 8, 1);
Mat img;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr model (new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::visualization::CloudViewer viewer("Model Extractor Viewer");
//Read Fiducial from file
Fiducial fiducial("fiducial.yml");
Pose pose;
while(/*!viewer.wasStopped() && */!sensor.endPlaying()){
//Get the frame
sensor.update();
sensor.getPCL(cloud);
cvSetData(rgb_image,sensor.rgb,rgb_image->widthStep);
//Estimate Camera Pose from fiducial
cvCvtColor(rgb_image,gray,CV_BGR2GRAY);
if (fiducial.find(gray,true)){
pose.estimate(gray,fiducial);
//fiducial.draw(rgb_image);
}
if (pose.isFound()){
printf("Rotation");
printMat<double>(pose.getR());
printf("Translation");
printMat<double>(pose.getT());
//Segment volume around the fiducial
boxFilter(cloud,pose);
//Create 3D model
buildModel(cloud,model);
}
//viewer.showCloud (model);
}
pcl::io::savePCDFileBinary ("model.pcd", *model);
sensor.shutdown();
return 0;
}
