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) {
Camera camera(*camera_);
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();
}
int main(int argc, char** argv)
{
camera_ = Camera::Ptr(new Camera());
scene_ = Scene::Ptr(new Scene());
range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_, 640));
// range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
//range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));
window_width_ = range_likelihood_->width() * 2;
window_height_ = range_likelihood_->height();
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
glutInitWindowPosition(10,10);
glutInitWindowSize(window_width_, window_height_);
glutCreateWindow("OpenGL range likelihood");
initialize(argc, argv);
glutReshapeFunc(on_reshape);
glutDisplayFunc(display);
glutIdleFunc(display);
glutKeyboardFunc(on_keyboard);
glutMouseFunc(on_mouse);
glutMotionFunc(on_motion);
glutPassiveMotionFunc(on_passive_motion);
glutEntryFunc(on_entry);
glutMainLoop();
}
int main(int argc, char** argv)
{
print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
if (argc < 2){
printHelp (argc, argv);
return (-1);
}
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
camera_ = Camera::Ptr(new Camera());
scene_ = Scene::Ptr(new Scene());
range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_, 640));
// range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
//range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));
// Actually corresponds to default parameters:
range_likelihood_->set_CameraIntrinsicsParameters(640,480,576.09757860,
576.09757860, 321.06398107, 242.97676897);
window_width_ = range_likelihood_->width() * 2;
window_height_ = range_likelihood_->height();
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
glutInitWindowPosition(10,10);
glutInitWindowSize(window_width_, window_height_);
glutCreateWindow("OpenGL range likelihood");
initialize(argc, argv);
glutReshapeFunc(on_reshape);
glutDisplayFunc(display);
glutIdleFunc(display);
glutKeyboardFunc(on_keyboard);
glutMouseFunc(on_mouse);
glutMotionFunc(on_motion);
glutPassiveMotionFunc(on_passive_motion);
glutEntryFunc(on_entry);
glutMainLoop();
}
void display_depth_image(const float* depth_buffer)
{
int npixels = range_likelihood_->width() * range_likelihood_->height();
uint8_t* depth_img = new uint8_t[npixels * 3];
for (int i=0; i<npixels; i++) {
float zn = 0.7;
float zf = 20.0;
float d = depth_buffer[i];
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+0] = 255;
depth_img[3*i+1] = 255-lb;
depth_img[3*i+2] = 255-lb;
break;
case 1:
depth_img[3*i+0] = 255;
depth_img[3*i+1] = lb;
depth_img[3*i+2] = 0;
break;
case 2:
depth_img[3*i+0] = 255-lb;
depth_img[3*i+1] = 255;
depth_img[3*i+2] = 0;
break;
case 3:
depth_img[3*i+0] = 0;
depth_img[3*i+1] = 255;
depth_img[3*i+2] = lb;
break;
case 4:
depth_img[3*i+0] = 0;
depth_img[3*i+1] = 255-lb;
depth_img[3*i+2] = 255;
break;
case 5:
depth_img[3*i+0] = 0;
depth_img[3*i+1] = 0;
depth_img[3*i+2] = 255-lb;
break;
default:
depth_img[3*i+0] = 0;
depth_img[3*i+1] = 0;
depth_img[3*i+2] = 0;
break;
}
}
// glRasterPos2i(-1,-1);
// glDrawPixels(range_likelihood_->width(), range_likelihood_->height(), GL_LUMINANCE, GL_FLOAT, range_likelihood_->depth_buffer());
glRasterPos2i(-1,-1);
glDrawPixels(range_likelihood_->width(), range_likelihood_->height(), GL_RGB, GL_UNSIGNED_BYTE, depth_img);
delete [] depth_img;
}
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;
}
}
