void display_score_image(const float* score_buffer)
{
int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
uint8_t* score_img = new uint8_t[npixels * 3];
float min_score = score_buffer[0];
float max_score = score_buffer[0];
for (int i=1; i<npixels; i++)
{
if (score_buffer[i] < min_score) min_score = score_buffer[i];
if (score_buffer[i] > max_score) max_score = score_buffer[i];
}
for (int i=0; i<npixels; i++)
{
float d = (score_buffer[i]-min_score)/(max_score-min_score);
score_img[3*i+0] = 0;
score_img[3*i+1] = d*255;
score_img[3*i+2] = 0;
}
glRasterPos2i(-1,-1);
glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(), GL_RGB, GL_UNSIGNED_BYTE, score_img);
delete [] score_img;
}
void
depthBufferToMM(const float* depth_buffer,unsigned short* depth_img)
{
int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
// unsigned short * depth_img = new unsigned short[npixels ];
for (int y = 0; y < 480; ++y)
{
for (int x = 0; x < 640; ++x)
{
int i= y*640 + x ;
int i_in= (480-1 -y) *640 + x ; // flip up down
float zn = 0.7;
float zf = 20.0;
float d = depth_buffer[i_in];
unsigned short z_new = (unsigned short) floor( 1000*( -zf*zn/((zf-zn)*(d - zf/(zf-zn)))));
if (z_new < 0) z_new = 0;
// else if (z_new>65535) z_new = 65535;
else if (z_new>5000) z_new = 0;
// if ( z_new < 18000){
// cout << z_new << " " << d << " " << x << "\n";
// }
depth_img[i] = z_new;
}
}
}
void
write_rgb_image(const uint8_t* rgb_buffer)
{
int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
uint8_t* rgb_img = new uint8_t[npixels * 3];
for (int y = 0; y < 480; ++y)
{
for (int x = 0; x < 640; ++x)
{
int px= y*640 + x ;
int px_in= (480-1 -y) *640 + x ; // flip up down
rgb_img [3* (px) +0] = rgb_buffer[3*px_in+0];
rgb_img [3* (px) +1] = rgb_buffer[3*px_in+1];
rgb_img [3* (px) +2] = rgb_buffer[3*px_in+2];
}
}
// Write to file:
IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
cv::Mat cv_mat(cv_ipl);
cv_mat.data = rgb_img ;
// cv::imwrite("rgb_image.png", cv_mat);
std::stringstream ss;
ss <<"rgb_image.png" ;
cv::imwrite(ss.str() , cv_mat);
delete [] rgb_img;
}
void
display_score_image (const float* score_buffer)
{
int npixels = range_likelihood_->getWidth () * range_likelihood_->getHeight ();
uint8_t* score_img = new uint8_t[npixels * 3];
float min_score = score_buffer[0];
float max_score = score_buffer[0];
for (int i=1; i<npixels; i++)
{
if (score_buffer[i] < min_score) min_score = score_buffer[i];
if (score_buffer[i] > max_score) max_score = score_buffer[i];
}
for (int i=0; i < npixels; i++)
{
float d = (score_buffer[i]-min_score)/(max_score-min_score);
score_img[3*i+0] = 0;
score_img[3*i+1] = static_cast<unsigned char> (d*255);
score_img[3*i+2] = 0;
}
textured_quad_->setTexture (score_img);
textured_quad_->render ();
delete [] score_img;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//SIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTART
void
write_depth_image(const float* depth_buffer)
{
int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
uint8_t* depth_img = new uint8_t[npixels * 3];
float min_depth = depth_buffer[0];
float max_depth = depth_buffer[0];
for (int i=1; i<npixels; i++)
{
if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
}
for (int y = 0; y < 480; ++y)
{
for (int x = 0; x < 640; ++x)
{
int i= y*640 + x ;
int i_in= (480-1 -y) *640 + x ; // flip up down
float zn = 0.7;
float zf = 20.0;
float d = depth_buffer[i_in];
float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
float b = 0.075;
float f = 580.0;
uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
if (kd < 0) kd = 0;
else if (kd>2047) kd = 2047;
int pval = t_gamma[kd];
int lb = pval & 0xff;
switch (pval>>8) {
case 0:
depth_img[3*i+2] = 255;
depth_img[3*i+1] = 255-lb;
depth_img[3*i+0] = 255-lb;
break;
case 1:
depth_img[3*i+2] = 255;
depth_img[3*i+1] = lb;
depth_img[3*i+0] = 0;
break;
case 2:
depth_img[3*i+2] = 255-lb;
depth_img[3*i+1] = 255;
depth_img[3*i+0] = 0;
break;
case 3:
depth_img[3*i+2] = 0;
depth_img[3*i+1] = 255;
depth_img[3*i+0] = lb;
break;
case 4:
depth_img[3*i+2] = 0;
depth_img[3*i+1] = 255-lb;
depth_img[3*i+0] = 255;
break;
case 5:
depth_img[3*i+2] = 0;
depth_img[3*i+1] = 0;
depth_img[3*i+0] = 255-lb;
break;
default:
depth_img[3*i+2] = 0;
depth_img[3*i+1] = 0;
depth_img[3*i+0] = 0;
break;
}
}
}
// Write to file:
IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
cv::Mat cv_mat(cv_ipl);
cv_mat.data = depth_img;
std::stringstream ss;
ss <<"depth_image.png" ;
cv::imwrite(ss.str() , cv_mat);
delete [] depth_img;
}
int
main (int argc, char** argv)
{
int width = 640;
int height = 480;
window_width_ = width * 2;
window_height_ = height * 2;
int cols = 30;
int rows = 30;
int col_width = 20;
int row_height = 15;
print_info ("Range likelihood performance tests using pcl::simulation. For more information, use: %s -h\n", argv[0]);
if (argc < 2)
{
printHelp (argc, argv);
return (-1);
}
for (int i = 0; i < 2048; ++i)
{
float v = static_cast<float> (i / 2048.0);
v = powf(v, 3)* 6;
t_gamma[i] = static_cast<uint16_t> (v*6*256);
}
glutInit (&argc, argv);
glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
glutInitWindowPosition (10, 10);
glutInitWindowSize (window_width_, window_height_);
glutCreateWindow ("OpenGL range likelihood");
GLenum err = glewInit ();
if (GLEW_OK != err)
{
std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
exit (-1);
}
std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
if (glewIsSupported ("GL_VERSION_2_0"))
std::cout << "OpenGL 2.0 supported" << std::endl;
else
{
std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
exit (1);
}
std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
camera_ = Camera::Ptr (new Camera ());
scene_ = Scene::Ptr (new Scene ());
range_likelihood_visualization_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (rows, cols, row_height, col_width, scene_));
// Actually corresponds to default parameters:
range_likelihood_visualization_->setCameraIntrinsicsParameters (
640, 480, 576.09757860f, 576.09757860f, 321.06398107f, 242.97676897f);
range_likelihood_visualization_->setComputeOnCPU (false);
range_likelihood_visualization_->setSumOnCPU (false);
range_likelihood_visualization_->setUseColor (true);
range_likelihood_->setCameraIntrinsicsParameters (
640, 480, 576.09757860f, 576.09757860f, 321.06398107f, 242.97676897f);
range_likelihood_->setComputeOnCPU (false);
range_likelihood_->setSumOnCPU (false);
range_likelihood_->setUseColor (false);
textured_quad_ = TexturedQuad::Ptr (new TexturedQuad (range_likelihood_->getWidth (),
range_likelihood_->getHeight ()));
initialize (argc, argv);
glutDisplayFunc (display);
glutIdleFunc (display);
glutKeyboardFunc (on_keyboard);
glutMainLoop ();
}
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_->getRowHeight () * range_likelihood_->getRows () / 2) * range_likelihood_->getWidth ()
+ j + range_likelihood_->getColWidth () * range_likelihood_->getCols () / 2];
}
}
float* reference_vis = new float[range_likelihood_visualization_->getRowHeight () * range_likelihood_visualization_->getColWidth ()];
const float* depth_buffer_vis = range_likelihood_visualization_->getDepthBuffer ();
// Copy one image from our last as a reference.
for (int i = 0, n = 0; i < range_likelihood_visualization_->getRowHeight (); ++i)
{
for (int j = 0; j < range_likelihood_visualization_->getColWidth (); ++j)
{
reference_vis[n++] = depth_buffer_vis[i*range_likelihood_visualization_->getWidth () + j];
}
}
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
std::vector<float> scores;
// Render a single pose for visualization
poses.clear ();
poses.push_back (camera_->getPose ());
range_likelihood_visualization_->computeLikelihoods (reference_vis, poses, scores);
glDrawBuffer (GL_BACK);
glReadBuffer (GL_BACK);
// Draw the resulting images from the range_likelihood
glViewport (range_likelihood_visualization_->getWidth (), 0,
range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
// Draw the color image
glColorMask (true, true, true, true);
glClearColor (0, 0, 0, 0);
glClearDepth (1);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable (GL_DEPTH_TEST);
glRasterPos2i (-1,-1);
glDrawPixels (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_visualization_->getColorBuffer ());
// Draw the depth image
glViewport (0, 0, range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
display_depth_image (range_likelihood_visualization_->getDepthBuffer (),
range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
poses.clear ();
for (int i = 0; i < range_likelihood_->getRows (); ++i)
{
for (int j = 0; j < range_likelihood_->getCols (); ++j)
{
Camera camera (*camera_);
camera.move ((j - range_likelihood_->getCols () / 2.0) * 0.1,
(i - range_likelihood_->getRows () / 2.0) * 0.1,
0.0);
poses.push_back (camera.getPose ());
}
}
std::cout << std::endl;
TicToc tt;
tt.tic();
range_likelihood_->computeLikelihoods (reference, poses, scores);
tt.toc();
tt.toc_print();
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: RangeLikelihood::computeLikelihoods: finished" << std::endl;
}
#if 0
std::cout << "score: ";
for (size_t i = 0; i < scores.size (); ++i)
{
std::cout << " " << scores[i];
}
std::cout << std::endl;
#endif
std::cout << "camera: " << camera_->getX ()
<< " " << camera_->getY ()
<< " " << camera_->getZ ()
<< " " << camera_->getRoll ()
<< " " << camera_->getPitch ()
<< " " << camera_->getYaw ()
<< std::endl;
delete [] reference_vis;
delete [] reference;
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: before buffers" << std::endl;
}
glBindFramebuffer (GL_FRAMEBUFFER, 0);
glDrawBuffer (GL_BACK);
glReadBuffer (GL_BACK);
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: after buffers" << std::endl;
}
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: before viewport" << std::endl;
}
// Draw the score image for the particles
glViewport (0, range_likelihood_visualization_->getHeight (),
range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: after viewport" << std::endl;
}
display_score_image (range_likelihood_->getScoreBuffer ());
// Draw the depth image for the particles
glViewport (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
display_score_image (range_likelihood_->getDepthBuffer ());
glutSwapBuffers ();
}
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;
}
}