void capture (Eigen::Isometry3d pose_in) { // No reference image - but this is kept for compatibility 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>); }
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 () { 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; } }