void
view (const CloudConstPtr &src, const CloudConstPtr &tgt, const CorrespondencesPtr &correspondences)
{
if (!visualize || !vis) return;
PointCloudColorHandlerCustom<PointT> green (tgt, 0, 255, 0);
if (!vis->updatePointCloud<PointT> (src, "source"))
{
vis->addPointCloud<PointT> (src, "source");
vis->resetCameraViewpoint ("source");
}
if (!vis->updatePointCloud<PointT> (tgt, green, "target")) vis->addPointCloud<PointT> (tgt, green, "target");
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.5, "source");
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.7, "target");
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 6, "source");
TicToc tt;
tt.tic ();
if (!vis->updateCorrespondences<PointT> (src, tgt, *correspondences, 1))
vis->addCorrespondences<PointT> (src, tgt, *correspondences, 1, "correspondences");
tt.toc_print ();
vis->setShapeRenderingProperties (PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences");
//vis->setShapeRenderingProperties (PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "correspondences");
vis->spin ();
}
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 ();
}
