void
capture (Eigen::Isometry3d pose_in,unsigned short* depth_buffer_mm,const uint8_t* color_buffer)//, string point_cloud_fname)
{
// No reference image - but this is kept for compatability with range_test_v2:
float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
//const float* depth_buffer = range_likelihood_->getDepthBuffer();
// Copy one image from our last as a reference.
/*
for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
{
for (int j=0; j<range_likelihood_->getColWidth(); ++j)
{
reference[n++] = 0;//depth_buffer[i*range_likelihood_->getWidth() + j];
}
}
*/
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
std::vector<float> scores;
int n = 1;
poses.push_back (pose_in);
// HACK: mfallon modified computeLikelihoods to only call render() (which is currently private)
// need to make render public and use it.
// For simulation it is used alone from the reset of range_likelihood_
range_likelihood_->computeLikelihoods (reference, poses, scores);
color_buffer =range_likelihood_->getColorBuffer();
const float* db_ptr= range_likelihood_->getDepthBuffer ();
range_likelihood_->addNoise ();
depthBufferToMM (db_ptr,depth_buffer_mm);
// Writing these images is a smaller computation draw relative to KinFu:
write_depth_image (db_ptr);
//write_depth_image_uint(depth_buffer_mm);
write_rgb_image (color_buffer);
/*
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
bool write_cloud=true;
if (write_cloud)
{
// Read Color Buffer from the GPU before creating PointCloud:
// By default the buffers are not read back from the GPU
range_likelihood_->getColorBuffer ();
range_likelihood_->getDepthBuffer ();
// Add noise directly to the CPU depth buffer
range_likelihood_->addNoise ();
// Optional argument to save point cloud in global frame:
// Save camera relative:
//range_likelihood_->getPointCloud(pc_out);
// Save in global frame - applying the camera frame:
//range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
// Save in local frame
range_likelihood_->getPointCloud (pc_out,false,camera_->pose ());
// TODO: what to do when there are more than one simulated view?
std::cout << pc_out->points.size() << " points written to file\n";
pcl::PCDWriter writer;
//writer.write (point_cloud_fname, *pc_out, false); /// ASCII
writer.writeBinary (point_cloud_fname, *pc_out);
//cout << "finished writing file\n";
}
*/
delete [] reference;
}
void display ()
{
float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
const float* depth_buffer = range_likelihood_->getDepthBuffer();
// Copy one image from our last as a reference.
for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
{
for (int j=0; j<range_likelihood_->getColWidth(); ++j)
{
reference[n++] = depth_buffer[i*range_likelihood_->getWidth() + j];
}
}
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
std::vector<float> scores;
int n = range_likelihood_->getRows ()*range_likelihood_->getCols ();
for (int i = 0; i < n; ++i)
{
Camera camera(*camera_);
camera.move(0.0,i*0.02,0.0);
//camera.move(0.0,i*0.02,0.0);
poses.push_back (camera.getPose ());
}
range_likelihood_->computeLikelihoods (reference, poses, scores);
range_likelihood_->computeLikelihoods (reference, poses, scores);
std::cout << "score: ";
for (size_t i = 0; i<scores.size (); ++i)
{
std::cout << " " << scores[i];
}
std::cout << std::endl;
std::cout << "camera: " << camera_->getX ()
<< " " << camera_->getY ()
<< " " << camera_->getZ ()
<< " " << camera_->getRoll ()
<< " " << camera_->getPitch ()
<< " " << camera_->getYaw ()
<< std::endl;
delete [] reference;
glDrawBuffer (GL_BACK);
glReadBuffer (GL_BACK);
// Draw the resulting images from the range_likelihood
glViewport (range_likelihood_->getWidth (), 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
// Draw the color image
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColorMask (true, true, true, true);
glDisable (GL_DEPTH_TEST);
glRasterPos2i (-1,-1);
glDrawPixels (range_likelihood_->getWidth (), range_likelihood_->getHeight (),
GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_->getColorBuffer ());
// Draw the depth image
glViewport (0, 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
// display_depth_image (range_likelihood_->getDepthBuffer ());
display_depth_image (range_likelihood_->getDepthBuffer (),
range_likelihood_->getWidth (), range_likelihood_->getHeight ());
// Draw the score image
glViewport (0, range_likelihood_->getHeight (),
range_likelihood_->getWidth (), range_likelihood_->getHeight ());
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
display_score_image (range_likelihood_->getScoreBuffer ());
glutSwapBuffers ();
if (write_file_)
{
range_likelihood_->addNoise ();
pcl::RangeImagePlanar rangeImage;
range_likelihood_->getRangeImagePlanar (rangeImage);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
// Optional argument to save point cloud in global frame:
// Save camera relative:
//range_likelihood_->getPointCloud(pc_out);
// Save in global frame - applying the camera frame:
//range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
// Save in local frame
range_likelihood_->getPointCloud (pc_out,false,camera_->getPose ());
// TODO: what to do when there are more than one simulated view?
pcl::PCDWriter writer;
writer.write ("simulated_range_image.pcd", *pc_out, false);
cout << "finished writing file\n";
// pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
// viewer.showCloud (pc_out);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = simpleVis(pc_out);
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
// doesnt work:
// viewer->~PCLVisualizer();
// viewer.reset();
cout << "done\n";
// Problem: vtk and opengl dont seem to play very well together
// vtk seems to misbehave after a little while and wont keep the window on the screen
// method1: kill with [x] - but eventually it crashes:
//while (!viewer.wasStopped ()){
//}
// method2: eventually starts ignoring cin and pops up on screen and closes almost
// immediately
// cout << "enter 1 to cont\n";
// cin >> pause;
// viewer.wasStopped ();
// method 3: if you interact with the window with keys, the window is not closed properly
// TODO: use pcl methods as this time stuff is probably not cross playform
// struct timespec t;
// t.tv_sec = 100;
// //t.tv_nsec = (time_t)(20000000); // short sleep
// t.tv_nsec = (time_t)(0); // long sleep - normal speed
// nanosleep (&t, NULL);
write_file_ = 0;
}
}
void capture (Eigen::Isometry3d pose_in, string point_cloud_fname)
{
// No reference image - but this is kept for compatability with range_test_v2:
float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
const float* depth_buffer = range_likelihood_->getDepthBuffer();
// Copy one image from our last as a reference.
for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
{
for (int j=0; j<range_likelihood_->getColWidth(); ++j)
{
reference[n++] = depth_buffer[i*range_likelihood_->getWidth() + j];
}
}
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
std::vector<float> scores;
poses.push_back (pose_in);
range_likelihood_->computeLikelihoods (reference, poses, scores);
std::cout << "score: ";
for (size_t i = 0; i<scores.size (); ++i)
{
std::cout << " " << scores[i];
}
std::cout << std::endl;
std::cout << "camera: " << camera_->getX ()
<< " " << camera_->getY ()
<< " " << camera_->getZ ()
<< " " << camera_->getRoll ()
<< " " << camera_->getPitch ()
<< " " << camera_->getYaw ()
<< std::endl;
delete [] reference;
// Benchmark Values for
// 27840 triangle faces
// 13670 vertices
// 45.00Hz: simuation only
// 1.28Hz: simuation, addNoise? , getPointCloud, writeASCII
// 33.33Hz: simuation, getPointCloud
// 23.81Hz: simuation, getPointCloud, writeBinary
// 14.28Hz: simuation, addNoise, getPointCloud, writeBinary
// MODULE TIME FRACTION
// simuation 0.02222 31%
// addNoise 0.03 41%
// getPointCloud 0.008 11%
// writeBinary 0.012 16%
// total 0.07222
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
bool write_cloud = true;
if (write_cloud)
{
// Read Color Buffer from the GPU before creating PointCloud:
// By default the buffers are not read back from the GPU
range_likelihood_->getColorBuffer ();
range_likelihood_->getDepthBuffer ();
// Add noise directly to the CPU depth buffer
range_likelihood_->addNoise ();
// Optional argument to save point cloud in global frame:
// Save camera relative:
//range_likelihood_->getPointCloud(pc_out);
// Save in global frame - applying the camera frame:
//range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
// Save in local frame
range_likelihood_->getPointCloud (pc_out,false,camera_->getPose ());
// TODO: what to do when there are more than one simulated view?
std::cout << pc_out->points.size() << " points written to file\n";
pcl::PCDWriter writer;
//writer.write (point_cloud_fname, *pc_out, false); /// ASCII
writer.writeBinary (point_cloud_fname, *pc_out);
//cout << "finished writing file\n";
}
// Disabled all OpenCV stuff for now: dont want the dependency
/*
bool demo_other_stuff = false;
if (demo_other_stuff && write_cloud)
{
write_score_image (range_likelihood_->getScoreBuffer ());
write_rgb_image (range_likelihood_->getColorBuffer ());
write_depth_image (range_likelihood_->getDepthBuffer ());
// Demo interacton with RangeImage:
pcl::RangeImagePlanar rangeImage;
range_likelihood_->getRangeImagePlanar (rangeImage);
// display viewer: (currently seqfaults on exit of viewer)
if (1==0){
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = simpleVis(pc_out);
while (!viewer->wasStopped ()){
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
}
*/
}
void display() {
glViewport(range_likelihood_->width(), 0, range_likelihood_->width(), range_likelihood_->height());
float* reference = new float[range_likelihood_->row_height() * range_likelihood_->col_width()];
const float* depth_buffer = range_likelihood_->depth_buffer();
// Copy one image from our last as a reference.
for (int i=0, n=0; i<range_likelihood_->row_height(); ++i) {
for (int j=0; j<range_likelihood_->col_width(); ++j) {
reference[n++] = depth_buffer[i*range_likelihood_->width() + j];
}
}
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
std::vector<float> scores;
int n = range_likelihood_->rows()*range_likelihood_->cols();
for (int i=0; i<n; ++i) { // This is used when there is
Camera camera(*camera_);
camera.move(0.0,0.0,0.0);
//camera.move(0.0,i*0.02,0.0);
poses.push_back(camera.pose());
}
float* depth_field =NULL;
bool do_depth_field =false;
range_likelihood_->compute_likelihoods(reference, poses, scores,depth_field,do_depth_field);
// range_likelihood_->compute_likelihoods(reference, poses, scores);
delete [] reference;
delete [] depth_field;
std::cout << "score: ";
for (size_t i=0; i<scores.size(); ++i) {
std::cout << " " << scores[i];
}
std::cout << std::endl;
// Draw the depth image
// glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// glColorMask(true, true, true, true);
glDisable(GL_DEPTH_TEST);
glViewport(0, 0, range_likelihood_->width(), range_likelihood_->height());
//glViewport(0, 0, range_likelihood_->width(), range_likelihood_->height());
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//glRasterPos2i(-1,-1);
//glDrawPixels(range_likelihood_->width(), range_likelihood_->height(), GL_LUMINANCE, GL_FLOAT, range_likelihood_->depth_buffer());
display_depth_image(range_likelihood_->depth_buffer());
glutSwapBuffers();
if (write_file_){
range_likelihood_->addNoise();
pcl::RangeImagePlanar rangeImage;
range_likelihood_->getRangeImagePlanar(rangeImage);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
// Optional argument to save point cloud in global frame:
// Save camera relative:
//range_likelihood_->getPointCloud(pc_out);
// Save in global frame - applying the camera frame:
//range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
// Save in local frame
range_likelihood_->getPointCloud(pc_out,false,camera_->pose());
// TODO: what to do when there are more than one simulated view?
pcl::PCDWriter writer;
writer.write ("simulated_range_image.pcd", *pc_out, false);
cout << "finished writing file\n";
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (pc_out);
// Problem: vtk and opengl dont seem to play very well together
// vtk seems to misbehavew after a little while and wont keep the window on the screen
// method1: kill with [x] - but eventually it crashes:
//while (!viewer.wasStopped ()){
//}
// method2: eventually starts ignoring cin and pops up on screen and closes almost
// immediately
// cout << "enter 1 to cont\n";
// cin >> pause;
// viewer.wasStopped ();
// method 3: if you interact with the window with keys, the window is not closed properly
// TODO: use pcl methods as this time stuff is probably not cross playform
struct timespec t;
t.tv_sec = 100;
//t.tv_nsec = (time_t)(20000000); // short sleep
t.tv_nsec = (time_t)(0); // long sleep - normal speed
nanosleep(&t, NULL);
write_file_ =0;
}
}