C++ KinfuTracker类代码示例

  void
  showGeneratedDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose)
  {            
    raycaster_ptr_->run(kinfu.volume(), pose, kinfu.getCyclicalBufferStructure ());
    raycaster_ptr_->generateDepthImage(generated_depth_);    

    int c;
    vector<unsigned short> data;
    generated_depth_.download(data, c);

    viewerDepth_.showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 5000, true);
  }

void SceneCloudView::generateCloud(KinfuTracker& kinfu, bool integrate_colors)
{
  viewer_pose_ = kinfu.getCameraPose();

  ScopeTimeT time ("PointCloud Extraction");
  cout << "\nGetting cloud... " << flush;

  valid_combined_ = false;
  bool valid_extracted_ = false;

  if (extraction_mode_ != GPU_Connected6)     // So use CPU
  {
    kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
  }
  else
  {
    DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);

    if(extracted.size() > 0){
        valid_extracted_ = true;

        extracted.download (cloud_ptr_->points);
        cloud_ptr_->width = (int)cloud_ptr_->points.size ();
        cloud_ptr_->height = 1;

        if (integrate_colors)
        {
          kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
          point_colors_device_.download(point_colors_ptr_->points);
          point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
          point_colors_ptr_->height = 1;
          //pcl::gpu::mergePointRGB(extracted, point_colors_device_, combined_color_device_);
          //combined_color_device_.download (combined_color_ptr_->points);
        }
        else
          point_colors_ptr_->points.clear();
        combined_color_ptr_->clear();
        generateXYZRGB(cloud_ptr_, point_colors_ptr_, combined_color_ptr_);

    }else{
        valid_extracted_ = false;
        cout << "Failed to Extract Cloud " << endl;

    }
  }

  cout << "Done.  Cloud size: " << cloud_ptr_->points.size () / 1000 << "K" << endl;

}

  void
  publishGeneratedDepth (KinfuTracker& kinfu)
  {
	const Eigen::Affine3f& pose= kinfu.getCameraPose();
    raycaster_ptr_->run(kinfu.volume(), pose, kinfu.getCyclicalBufferStructure());
    raycaster_ptr_->generateDepthImage(generated_depth_);

    int c;
    vector<unsigned short> data;
    generated_depth_.download(data, c);

    sensor_msgs::ImagePtr msg(new sensor_msgs::Image);
    sensor_msgs::fillImage(*msg, "bgr8", view_device_.rows(), view_device_.cols(), view_device_.cols() * 3, &view_host_[0]);
    pubgen.publish(msg);
  }

  void
  publishScene (KinfuTracker& kinfu, std_msgs::Header header, bool registration, Eigen::Affine3f* pose_ptr = 0)
  {
//    if (pose_ptr)
//    {
//        raycaster_ptr_->run ( kinfu.volume (), *pose_ptr, kinfu.getCyclicalBufferStructure () ); //says in cmake it does not know it
//        raycaster_ptr_->generateSceneView(view_device_);
//    }
//    else
    {
      kinfu.getImage (view_device_);
    }

 /*   if (paint_image_ && registration && !pose_ptr)
    {
      colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
      paint3DView (colors_device_, view_device_);
    }
*/
    int cols;
    view_device_.download (view_host_, cols);

    //convert image to sensor message
    sensor_msgs::ImagePtr msg(new sensor_msgs::Image);
    sensor_msgs::fillImage((*msg), "rgb8", view_device_.rows(), view_device_.cols(),
    		view_device_.cols() * 3, &view_host_[0]);

    msg->header.frame_id = header.frame_id;
    pubKinfu.publish(msg);
  }

void ImageView::generateDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose, KinfuTracker::DepthMap &generated_depth_)
  {
    raycaster_ptr_->run(kinfu.volume(), pose);
    raycaster_ptr_->generateDepthImage(generated_depth_);

    //if (viz_)
      //  viewerDepth_->showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 1000, true);
  }

  void
  publishPose (KinfuTracker& kinfu, std_msgs::Header header)
  {
	  Eigen::Matrix<float, 3, 3, Eigen::RowMajor> erreMats = kinfu.getCameraPose().linear();
	  Eigen::Vector3f teVecs = kinfu.getCameraPose().translation();


	  //TODO: start position: init_tcam_ = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);

	  tf::Transform transform(
	      tf::Matrix3x3(erreMats(0,0),erreMats(0, 1),erreMats(0, 2),
			  erreMats(1,0),erreMats(1, 1),erreMats(1, 2),
			  erreMats(2,0),erreMats(2, 1),erreMats(2, 2)),
	  	  tf::Vector3(teVecs[0], teVecs[1], teVecs[2])
	  );

	  odom_broad.sendTransform(tf::StampedTransform(transform, header.stamp, "/odom", "/kinfu_frame"));
  }

  void
  displayICPState (KinfuTracker& kinfu, bool was_lost_)
  {
    string name = "last_good_track";
    string name_estimate = "last_good_estimate";
    if (was_lost_ && !kinfu.icpIsLost ()) //execute only when ICP just recovered (i.e. was_lost_ == true && icpIsLost == false)
    {
      removeCamera(name);
      removeCamera(name_estimate);
      clearClouds(false);
      cloud_viewer_.updateText ("ICP State: OK", 450, 55, 20, 0.0, 1.0, 0.0, "icp");
      cloud_viewer_.updateText ("Press 'S' to save the current world", 450, 35, 10, 0.0, 1.0, 0.0, "icp_save");
      cloud_viewer_.updateText ("Press 'R' to reset the system", 450, 15, 10, 0.0, 1.0, 0.0, "icp_reset");
    }
    else if (!was_lost_ && kinfu.icpIsLost()) //execute only when we just lost ourselves (i.e. was_lost_ = false && icpIsLost == true)
    { 
      // draw position of the last good track
      Eigen::Affine3f last_pose = kinfu.getCameraPose();
      drawCamera(last_pose, name, 0.0, 1.0, 0.0);
      
     
      
      cloud_viewer_.updateText ("ICP State: LOST", 450, 55, 20, 1.0, 0.0, 0.0, "icp");
      cloud_viewer_.updateText ("Press 'S' to save the current world", 450, 35, 10, 1.0, 0.0, 0.0, "icp_save");
      cloud_viewer_.updateText ("Press 'R' to reset the system", 450, 15, 10, 1.0, 0.0, 0.0, "icp_reset");
    }
    
    
    if( kinfu.icpIsLost() )
    {
      removeCamera(name_estimate);
       // draw current camera estimate
      Eigen::Affine3f last_pose_estimate = kinfu.getLastEstimatedPose();
      drawCamera(last_pose_estimate, name_estimate, 1.0, 0.0, 0.0);      
    }
      
      
      
//       cout << "current estimated pose: " << kinfu.getLastEstimatedPose().translation() << std::endl << kinfu.getLastEstimatedPose().linear() << std::endl;
//     
  }

void CurrentFrameCloudView::show (const KinfuTracker& kinfu)
{
    kinfu.getLastFrameCloud (cloud_device_);

    int c;
    cloud_device_.download (cloud_ptr_->points, c);
    cloud_ptr_->width = cloud_device_.cols ();
    cloud_ptr_->height = cloud_device_.rows ();
    cloud_ptr_->is_dense = false;

    cloud_viewer_.removeAllPointClouds ();
    cloud_viewer_.addPointCloud<PointXYZ>(cloud_ptr_);
    cloud_viewer_.spinOnce ();
}

void KinectVisualization::showCloudXYZ(const KinfuTracker& kinfu)
{
	//cout << cloud_device_xyz.cols() << endl;
	kinfu.getLastFrameCloud (cloud_device_xyz);

	int c;
	cloud_device_xyz.download (cloud_ptr_->points, c);
	cloud_ptr_->width = cloud_device_xyz.cols ();
	cloud_ptr_->height = cloud_device_xyz.rows ();
	cloud_ptr_->is_dense = false;

	//cout << cloud_ptr_->width << " " << cloud_ptr_->height << endl;
	//writeCloudFile(2, cloud_ptr_);
	cloud_viewer_xyz_.removeAllPointClouds ();
	cloud_viewer_xyz_.addPointCloud<PointXYZ>(cloud_ptr_);
}

  void
  showMesh(KinfuTracker& kinfu, bool /*integrate_colors*/)
  {
    ScopeTimeT time ("Mesh Extraction");
    cout << "\nGetting mesh... " << flush;

    if (!marching_cubes_)
      marching_cubes_ = MarchingCubes::Ptr( new MarchingCubes() );

    DeviceArray<PointXYZ> triangles_device = marching_cubes_->run(kinfu.volume(), triangles_buffer_device_);    
    mesh_ptr_ = convertToMesh(triangles_device);

    cloud_viewer_.removeAllPointClouds ();
    if (mesh_ptr_)
      cloud_viewer_.addPolygonMesh(*mesh_ptr_);	

    cout << "Done.  Triangles number: " << triangles_device.size() / MarchingCubes::POINTS_PER_TRIANGLE / 1000 << "K" << endl;
  }

  void
  show (KinfuTracker& kinfu, bool integrate_colors)
  {
    viewer_pose_ = kinfu.getCameraPose();

    ScopeTimeT time ("PointCloud Extraction");
    cout << "\nGetting cloud... " << flush;

    valid_combined_ = false;

    if (extraction_mode_ != GPU_Connected6)     // So use CPU
    {
      kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
    }
    else
    {
      DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);             

      if (compute_normals_)
      {
        kinfu.volume().fetchNormals (extracted, normals_device_);
        pcl::gpu::mergePointNormal (extracted, normals_device_, combined_device_);
        combined_device_.download (combined_ptr_->points);
        combined_ptr_->width = (int)combined_ptr_->points.size ();
        combined_ptr_->height = 1;

        valid_combined_ = true;
      }
      else
      {
        extracted.download (cloud_ptr_->points);
        cloud_ptr_->width = (int)cloud_ptr_->points.size ();
        cloud_ptr_->height = 1;
      }

      if (integrate_colors)
      {
        kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
        point_colors_device_.download(point_colors_ptr_->points);
        point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
        point_colors_ptr_->height = 1;
      }
      else
        point_colors_ptr_->points.clear();
    }
    size_t points_size = valid_combined_ ? combined_ptr_->points.size () : cloud_ptr_->points.size ();
    cout << "Done.  Cloud size: " << points_size / 1000 << "K" << endl;

    cloud_viewer_.removeAllPointClouds ();    
    if (valid_combined_)
    {
      visualization::PointCloudColorHandlerRGBHack<PointNormal> rgb(combined_ptr_, point_colors_ptr_);
      cloud_viewer_.addPointCloud<PointNormal> (combined_ptr_, rgb, "Cloud");
      cloud_viewer_.addPointCloudNormals<PointNormal>(combined_ptr_, 50);
    }
    else
    {
      visualization::PointCloudColorHandlerRGBHack<PointXYZ> rgb(cloud_ptr_, point_colors_ptr_);
      cloud_viewer_.addPointCloud<PointXYZ> (cloud_ptr_, rgb);
    }    
  }