Exemple #1
0
template<typename PointInT> int
pcl::TextureMapping<PointInT>::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pcl::TextureMesh &sorted_mesh,
                                                  const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size,
                                                  PointCloud &visible_pts)
{
  if (tex_mesh.tex_polygons.size () != 1)
  {
    PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n");
    return (-1);
  }

  if (cameras.size () == 0)
  {
    PCL_ERROR ("Must provide at least one camera info!\n");
    return (-1);
  }

  // copy mesh
  sorted_mesh = tex_mesh;
  // clear polygons from cleaned_mesh
  sorted_mesh.tex_polygons.clear ();

  pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);

  // load points into a PCL format
  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud);

  // for each camera
  for (size_t cam = 0; cam < cameras.size (); ++cam)
  {
    // get camera pose as transform
    Eigen::Affine3f cam_trans = cameras[cam].pose;

    // transform original cloud in camera coordinates
    pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ());

    // find occlusions on transformed cloud
    std::vector<int> visible, occluded;
    removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
    visible_pts = *filtered_cloud;

    // find visible faces => add them to polygon N for camera N
    // add polygon group for current camera in clean
    std::vector<pcl::Vertices> visibleFaces_currentCam;
    // iterate over the faces of the current mesh
    for (size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces)
    {
      // check if all the face's points are visible
      bool faceIsVisible = true;
      std::vector<int>::iterator it;

      // iterate over face's vertex
      for (size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice)
      {
        // TODO this is far too long! Better create an helper function that raycasts here.
        it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]);

        if (it == occluded.end ())
        {
          // point is not occluded
          // does it land on the camera's image plane?
          pcl::PointXYZ pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]];
          Eigen::Vector2f dummy_UV;
          if (!getPointUVCoordinates (pt, cameras[cam], dummy_UV))
          {
            // point is not visible by the camera
            faceIsVisible = false;
          }
        }
        else
        {
          faceIsVisible = false;
        }
      }

      if (faceIsVisible)
      {
        // push current visible face into the sorted mesh
        visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]);
        // remove it from the unsorted mesh
        tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces);
        faces--;
      }

    }
    sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam);
  }

  // we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0]
  // we need to add them as an extra polygon in the sorted mesh
  sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]);
  return (0);
}
Exemple #2
0
int
main (int argc, char** argv)
{
  bool second_rotation_performed = false;
  bool visualize_results = true;

  boost::posix_time::ptime t_s = boost::posix_time::microsec_clock::local_time();

  std::vector<int> filenames;
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  if (filenames.size () != 1)
  {
    std::cout << "Usage: input_file_path.pcd\n";
    exit (-1);
  }
  std::string input_filename = argv[filenames.at(0)];

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr original_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

  // pcl::PointCloud<pcl::PointXYZRGB>::Ptr final (new pcl::PointCloud<pcl::PointXYZRGB>);

  if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (input_filename, *original_cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read input file\n");
    exit (-1);
  }
  std::cout << "Loaded "
            << original_cloud->width * original_cloud->height
            << " data points from input pcd" << std::endl;

  boost::posix_time::ptime t_file_loaded = boost::posix_time::microsec_clock::local_time();
  // Copy the original cloud to input cloud, which can be modified later during plane extraction
  pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZRGB>(*original_cloud, *input_cloud);

  CuboidMatcher cm;
  cm.setInputCloud(input_cloud);
  cm.setDebug(true);
  cm.setSaveIntermediateResults(true);
  Cuboid cuboid;
  cm.execute(cuboid);
  boost::posix_time::ptime t_algorithm_done = boost::posix_time::microsec_clock::local_time();
  printDuration(t_file_loaded, t_algorithm_done, "Algorithm Runtime");

  std::cout << "Algorithm done. Visualize results" << std::endl;

  
  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> intermediate_clouds;
  intermediate_clouds = cm.getIntermediateClouds();

  if(!visualize_results)
    exit(0);
  // Atleast one intermediate clouds means, that the user want's to display them here.
  
  std::cout << "The algorithm did " << intermediate_clouds.size() << " transformation(s)" << std::endl;

  // Create the viewports for the rotated object
  pcl::visualization::PCLVisualizer viewer;
  // Visualize original pointcloud
  int v1(0);
  viewer.createViewPort(0.0, 0.0, 0.3, 1.0, v1);
  viewer.addCoordinateSystem(1.0,v1);

  // Visualize the found inliers
  int v2(1);
  viewer.createViewPort(0.3, 0.0, 0.6, 1.0, v2);
  viewer.addCoordinateSystem(1.0,v2);

  int v3(2);
  viewer.createViewPort(0.6, 0.0, 1.0, 1.0, v3);
  viewer.addCoordinateSystem(0.5,v3);

  // Fill the viewpoints with the desired clouds
  // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(original_cloud);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red_pts (original_cloud, 255,0,0);
  // viewer.addPointCloud<pcl::PointXYZRGB> (original_cloud, rgb, "sample cloud1", v1);
  viewer.addPointCloud<pcl::PointXYZRGB> (original_cloud, red_pts, "sample cloud1", v1);

  if( intermediate_clouds.size() >= 1) 
  {
    // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_v2(intermediate_clouds.at(0));
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red_v2 (intermediate_clouds.at(0), 255,0,0);
    // viewer.addPointCloud<pcl::PointXYZRGB> (intermediate_clouds.at(0), rgb_v2, "first rotated cloud", v2);
    viewer.addPointCloud<pcl::PointXYZRGB> (intermediate_clouds.at(0), red_v2, "first rotated cloud", v2);
  }


  if( intermediate_clouds.size() >= 2) 
  {
    // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_v3(intermediate_clouds.at(1));
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red_v3 (intermediate_clouds.at(1), 255,0,0);
    viewer.addPointCloud<pcl::PointXYZRGB> (intermediate_clouds.at(1), red_v3, "second rotated cloud", v3);
    // viewer.addPointCloud<pcl::PointXYZRGB> (intermediate_clouds.at(1), rgb_v3, "second rotated cloud", v3);
  }

  // Draw the bounding box from the given corner points
  drawBoundingBoxLines(viewer, cuboid.corner_points, v1);

  viewer.addCube	(	Eigen::Vector3f(0,0,0),
      Eigen::Quaternion<float>::Identity(),
      cuboid.length1,
      cuboid.length2,
      cuboid.length3,
      "matched_cuboid",
      v2
    );

 
  
  viewer.addCube	(	cuboid.center,
      cuboid.orientation,
      cuboid.length1,
      cuboid.length2,
      cuboid.length3,
      "matched_cuboid_centered",
      v1
    );
  

  viewer.spin();




  /*
  boost::posix_time::ptime t_extraction_done = boost::posix_time::microsec_clock::local_time();
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  //
  // Store the possible different colors for the segmented planes
  std::vector<std::vector<int> > color_sequence;
  std::vector<int> green;
  green.push_back(0);
  green.push_back(255);
  green.push_back(0);
  std::vector<int> red;
  red.push_back(255);
  red.push_back(0);
  red.push_back(0);
  std::vector<int> blue;
  blue.push_back(0);
  blue.push_back(0);
  blue.push_back(255);
  color_sequence.push_back(green);
  color_sequence.push_back(red);
  color_sequence.push_back(blue);
  
  pcl::visualization::PCLVisualizer viewer;
  // Visualize original pointcloud
  int v1(0);
  viewer.createViewPort(0.0, 0.0, 0.2, 1.0, v1);
  viewer.addCoordinateSystem(1.0,v1);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(original_cloud);
  viewer.addPointCloud<pcl::PointXYZRGB> (original_cloud, rgb, "sample cloud1", v1);

  // Visualize the found inliers
  int v2(1);
  viewer.createViewPort(0.2, 0.0, 0.6, 1.0, v2);
  viewer.addCoordinateSystem(1.0,v2);

  int v3(2);
  viewer.createViewPort(0.6, 0.0, 1.0, 1.0, v3);
  viewer.addCoordinateSystem(0.5,v3);

  // For each segmented plane
  for (int i = 0; i < detected_planes.size(); i++) 
  {
    // Select a color from the color_sequence vector for the discovered plane
    int color_idx = i % 3;
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color (detected_planes.at(i).getPoints(), color_sequence.at(color_idx).at(0), color_sequence.at(color_idx).at(1), color_sequence.at(color_idx).at(2));

    std::stringstream cloud_name("plane_cloud_");
    cloud_name << i;
    std::stringstream plane_name("model_plane_");
    plane_name << i;
    viewer.addPointCloud<pcl::PointXYZRGB> (detected_planes.at(i).getPoints(), single_color, cloud_name.str(), v2);
    viewer.addPlane (*detected_planes.at(i).getCoefficients(), plane_name.str(), v2);

    // Display the centroid of the planes
    pcl::PointXYZRGB c;
    c.x = detected_planes.at(i).getCentroid()(0);
    c.y = detected_planes.at(i).getCentroid()(1);
    c.z = detected_planes.at(i).getCentroid()(2);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr centroid_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    centroid_cloud->push_back(c);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> centroid_yellow (centroid_cloud, 
        255,255,0);
    std::stringstream centroid_name("centroid_");
    centroid_name << i;
    viewer.addPointCloud<pcl::PointXYZRGB> (centroid_cloud, centroid_yellow, centroid_name.str(), v2);
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
        5, centroid_name.str());

    // Display the plane normal with the camera origin as the origin
    {
      pcl::PointXYZRGB origin;
      origin.x=0;
      origin.y=0;
      origin.z=0;

      // Get the normal from the plane
      pcl::PointXYZRGB dest;
      dest.x = detected_planes.at(i).getCoefficients()->values.at(0);
      dest.y = detected_planes.at(i).getCoefficients()->values.at(1);
      dest.z = detected_planes.at(i).getCoefficients()->values.at(2);

      pcl::PointCloud<pcl::PointXYZRGB>::Ptr origin_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
      origin_cloud->push_back(origin);
      pcl::PointCloud<pcl::PointXYZRGB>::Ptr origin_cloud_projected (new pcl::PointCloud<pcl::PointXYZRGB>);
      // Project the origin point to the plane
      pcl::ProjectInliers<pcl::PointXYZRGB> proj;
      proj.setModelType (pcl::SACMODEL_PLANE);
      proj.setInputCloud (origin_cloud);
      proj.setModelCoefficients (detected_planes.at(i).getCoefficients());
      proj.filter (*origin_cloud_projected);
      if(origin_cloud_projected->points.size()!=1)
      {
        std::cout << "Projection fail" << std::endl;
        return 0;
      }

      std::stringstream plane_name("norm_plane_");
      plane_name << i;
      viewer.addLine(origin_cloud_projected->points.at(0),dest, color_sequence.at(color_idx).at(0), color_sequence.at(color_idx).at(1), color_sequence.at(color_idx).at(2), plane_name.str(),v2);
      Eigen::Vector3f norm_origin(
origin_cloud_projected->points.at(0).x,
origin_cloud_projected->points.at(0).y,
origin_cloud_projected->points.at(0).z
          );
      detected_planes.at(i).setNormOrigin(norm_origin);
    }

  }
  // Build a coordinate system for biggest plane
  // Step 1) Visualize the coordinate system
  //
  // Transform the destination of the norm vector, to let it point in a 90° Angle from the
  // centroid to its destination
  Eigen::Vector3f newDest = moveVectorBySubtraction( 
       detected_planes.at(0).getCoefficientsAsVector3f(),
       // The Centroid of the plane cloud
       detected_planes.at(0).getCentroidAsVector3f(),
       // And the center of the norm origin
       detected_planes.at(0).getNormOrigin()
      );
  // Translate the norm vector of the second plane to the centroid of the first plane
  viewer.addLine(
      getPointXYZFromVector4f(detected_planes.at(0).getCentroid()),
      getPointXYZFromVector3f(newDest),
        0, 255, 0, "f1",v2);

  Eigen::Vector3f secondDest = moveVectorBySubtraction( 
       detected_planes.at(1).getCoefficientsAsVector3f(),
       // The Centroid of the plane cloud
       detected_planes.at(0).getCentroidAsVector3f(),
       // And the center of the norm origin
       detected_planes.at(1).getNormOrigin()
      );
  // Translate the norm vector of the second plane to the centroid of the first plane
  viewer.addLine(
      getPointXYZFromVector4f(detected_planes.at(0).getCentroid()),
      getPointXYZFromVector3f(secondDest),
        255, 0, 0, "f2",v2);

  // Create the third axis by using the cross product
  Eigen::Vector3f thirdDest = newDest.cross(secondDest);
  viewer.addLine(
      getPointXYZFromVector4f(detected_planes.at(0).getCentroid()),
      getPointXYZFromVector3f(thirdDest),
        0, 0, 255, "f3",v2);



  //
  // ROTATE THE OBJECT to align it with the x-y and the x-z plane
  // TODO: dynamic
  //
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  // Align the front face with the cameras front plane
  {
    for (int i = 0; i < detected_planes.size(); i++) 
    {
      std::cout << "Angle between Normal " << i << " and x-y Normal ";

      Eigen::Vector3f n2( 0,0,1);
      float angle = detected_planes.at(i).angleBetween(n2);
      std::cout << ": " << angle << " RAD, " << ((angle * 180) / M_PI) << " DEG";
      std::cout << std::endl;
    }
    std::cout << "Angle between Normal 0 and x-y Normal ";

    Eigen::Vector3f n2( 0,0,1);
    float angle = detected_planes.at(0).angleBetween(n2);

    pcl::PointXYZRGB dest;
    dest.x = detected_planes.at(0).getCoefficients()->values.at(0);
    dest.y = detected_planes.at(0).getCoefficients()->values.at(1);
    dest.z = detected_planes.at(0).getCoefficients()->values.at(2);

    // Translate the first plane's origin to the camera origin
    translatePointCloud(original_cloud,
        -detected_planes.at(0).getCentroid()[0],
        -detected_planes.at(0).getCentroid()[1],
        -detected_planes.at(0).getCentroid()[2],
        rotated_cloud);


    // M
    Eigen::Vector3f plane_normal(dest.x, dest.y, dest.z);

    // Compute the necessary rotation to align a face of the object with the camera's
    // imaginary image plane
    // N
    Eigen::Vector3f camera_normal;
    camera_normal(0)=0;
    camera_normal(1)=0;
    camera_normal(2)=1;

    camera_normal = CuboidMatcher::reduceNormAngle(plane_normal, camera_normal);

    // float dotproduct3 = camera_normal.dot(plane_normal);
    // if(acos(dotproduct3)> M_PI/2)
    // {
    //   std::cout << "NORM IS ABOVE 90 DEG! TURN IN THE OTHER DIRECTION" << std::endl;
    //   camera_normal = -camera_normal;
    //   // rotated_normal_of_second_plane = - rotated_normal_of_second_plane;
    // }

    
    Eigen::Matrix< float, 4, 4 > rotationBox = 
      rotateAroundCrossProductOfNormals(camera_normal, plane_normal);
    
    pcl::transformPointCloud (*rotated_cloud, *rotated_cloud, rotationBox);   

    // Draw the rotated object in the first window
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_r(rotated_cloud);
    viewer.addPointCloud<pcl::PointXYZRGB> (rotated_cloud, rgb_r, "rotated_cloud", v2);

    // Rotate the normal of the second plane with the first rotation matrix
    Eigen::Vector3f normal_of_second_plane =
      detected_planes.at(1).getCoefficientsAsVector3f();

    Eigen::Vector3f rotated_normal_of_second_plane;
    rotated_normal_of_second_plane = 
      removeTranslationVectorFromMatrix(rotationBox) * normal_of_second_plane;

    // Now align the second normal (which has been rotated) with the x-z plane
    Eigen::Vector3f xz_plane;
    xz_plane(0)=0;
    xz_plane(1)=1;
    xz_plane(2)=0;

    std::cout << "Angle between rotated normal and xz-plane ";
    float dotproduct2 = xz_plane.dot(rotated_normal_of_second_plane);
    std::cout << ": " << acos(dotproduct2) << " RAD, " << ((acos(dotproduct2) * 180) / M_PI) << " DEG";
    std::cout << std::endl;

    if(acos(dotproduct2)> M_PI/2)
    {
      std::cout << "NORM IS ABOVE 90 DEG! TURN IN THE OTHER DIRECTION" << std::endl;
      rotated_normal_of_second_plane = - rotated_normal_of_second_plane;
    }

    
    float angle_between_xz_and_second_normal = ((acos(dotproduct2) * 180) / M_PI);
    Eigen::Matrix< float, 4, 4 > secondRotation;
    if(angle_between_xz_and_second_normal > MIN_ANGLE &&
        angle_between_xz_and_second_normal < 180-MIN_ANGLE)
    {
    
      // Rotate the original centroid
      Eigen::Vector4f offset_between_centroids =
          detected_planes.at(1).getCentroid() - detected_planes.at(0).getCentroid();
      Eigen::Vector3f rotated_offset =
        removeTranslationVectorFromMatrix(rotationBox) * getVector3fFromVector4f(offset_between_centroids);
   
      
      // translatePointCloud(rotated_cloud, 
      //     - rotated_offset[0],
      //     - rotated_offset[1],
      //     - rotated_offset[2],
      //     rotated_cloud);
          

      secondRotation = 
        rotateAroundCrossProductOfNormals(xz_plane, rotated_normal_of_second_plane);

      pcl::transformPointCloud (*rotated_cloud, *rotated_cloud, secondRotation);   
      second_rotation_performed = true;
    }
    else
    {
      secondRotation = Eigen::Matrix< float, 4, 4 >::Identity();
      second_rotation_performed = false;
      std::cout << "No second rotation, since the angle is too small: "<< angle_between_xz_and_second_normal << "DEG" << std::endl;
    }




    pcl::PointXYZ origin(0,0,0);
    viewer.addLine(origin, getPointXYZFromVector3f(rotated_normal_of_second_plane) , 255, 0, 0, "normal1",v2);

    // Draw the rotated object
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_r2(rotated_cloud);
    viewer.addPointCloud<pcl::PointXYZRGB> (rotated_cloud, rgb_r2, "rotated_cloud_second", v3);

    // Compute the bounding box for the rotated object
    pcl::PointXYZRGB min_pt, max_pt;
    pcl::getMinMax3D(*rotated_cloud, min_pt, max_pt);
    
    // Compute the bounding box edge points
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr manual_bounding_box(new pcl::PointCloud<pcl::PointXYZRGB>);
    computeCuboidCornersWithMinMax3D(rotated_cloud,manual_bounding_box);

    // Draw the bounding box edge points
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red_pts (manual_bounding_box, 255,0,0);
    viewer.addPointCloud<pcl::PointXYZRGB> (manual_bounding_box, red_pts, "bb", v3);
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "bb");

    // Now reverse all the transformations to get the bounding box around the actual object
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr bounding_box_on_object(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> white_pts (bounding_box_on_object, 255,255,255);

    // Inverse the last rotation
    pcl::transformPointCloud (*manual_bounding_box, *bounding_box_on_object, secondRotation.transpose());   

    // Translate back to the first centroid, if this was done
    if(second_rotation_performed)
    {
      Eigen::Vector4f offset_between_centroids =
        detected_planes.at(1).getCentroid() - detected_planes.at(0).getCentroid();
      Eigen::Vector3f rotated_offset =
        removeTranslationVectorFromMatrix(rotationBox) * getVector3fFromVector4f(offset_between_centroids);
      // translatePointCloud(bounding_box_on_object, 
      //     rotated_offset[0], // Translation is now NOT negative
      //     rotated_offset[1],
      //     rotated_offset[2],
      //     bounding_box_on_object);
    }
    // Translated to first centroid
    
    
    // Inverse the second rotation
    pcl::transformPointCloud (*bounding_box_on_object, *bounding_box_on_object, rotationBox.transpose());   

    // And translate back to the original position
    translatePointCloud(bounding_box_on_object, 
        detected_planes.at(0).getCentroid()[0],  
        detected_planes.at(0).getCentroid()[1], 
        detected_planes.at(0).getCentroid()[2], 
        bounding_box_on_object);

    viewer.addPointCloud<pcl::PointXYZRGB> (bounding_box_on_object, white_pts, "bb_real", v3);
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "bb_real");
    drawBoundingBoxLines(viewer, bounding_box_on_object, v3);
    // And finally, render the original cloud
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_v3(original_cloud);
    viewer.addPointCloud<pcl::PointXYZRGB> (original_cloud, rgb_v3, "original_cloud", v3);

    std::cout << "Cuboid statistics" << std::endl;
    Cuboid c = computeCuboidFromBorderPoints(bounding_box_on_object);
    std::cout << "Width: " << c.length1 << " Height: " << c.length2 << " Depth: " << c.length3 << " Volume: " << c.volume << " m^3" << std::endl;
  }

  boost::posix_time::ptime t_done = boost::posix_time::microsec_clock::local_time();

  printDuration(t_s, t_file_loaded, "Loaded file");
  printDuration(t_file_loaded, t_extraction_done, "Plane Extraction, Normal Estimation");
  printDuration(t_extraction_done, t_done, "Rotation and Visualization");
  printDuration(t_s, t_done, "Overall");

  // 
  // Create a plane x-y plane that originates in the kinect camera frame
  // 
  pcl::ModelCoefficients::Ptr kinect_plane_coefficients (new pcl::ModelCoefficients);
  // Let the Normal of the plane point along the z-Axis
  kinect_plane_coefficients->values.push_back(0); // x
  kinect_plane_coefficients->values.push_back(0); // y
  kinect_plane_coefficients->values.push_back(1); // z 
  kinect_plane_coefficients->values.push_back(0); // d 

  viewer.addPlane (*kinect_plane_coefficients, "kinect_plane", v2);
  viewer.spin();
*/
  return (0);
}