Exemple #1
0
void simulate_callback (const pcl::visualization::KeyboardEvent &event,
                        void* viewer_void)
{
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);
  // I choose v for virtual as s for simulate is takwen
  if (event.getKeySym () == "v" && event.keyDown ())
  {
    std::cout << "v was pressed => simulate cloud" << std::endl;

    std::vector<pcl::visualization::Camera> cams;
    viewer->getCameras(cams);
    
    if (cams.size() !=1){
      std::cout << "n cams not 1 exiting\n"; // for now in case ...
     return; 
    }
   // cout << "n cams: " << cams.size() << "\n";
    pcl::visualization::Camera cam = cams[0];
    


	      
	      Eigen::Affine3f pose;
	      pose = viewer->getViewerPose() ;
	      
	      
     std::cout << cam.pos[0] << " " 
               << cam.pos[1] << " " 
               << cam.pos[2] << " p\n";	      
	      
	     Eigen::Matrix3f m;
	     m =pose.rotation();
	     //All axies use right hand rule. x=red axis, y=green axis, z=blue axis z direction is point into the screen. z \ \ \ -----------> x | | | | | | y 
	      
 cout << pose(0,0)  << " " << pose(0,1) << " " << pose(0,2)  << " " << pose(0,3) << " x0\n";
 cout << pose(1,0) << " " << pose(1,1) << " " << pose(1,2) << " " << pose(1,3) << " x1\n";
  cout << pose(2,0) << " " << pose(2,1)  << " " << pose(2,2) << " " << pose(2,3)<< "x2\n";

  double yaw,pitch, roll;
  wRo_to_euler(m,yaw,pitch,roll);
  
 printf("RPY: %f %f %f\n", roll*180/M_PI,pitch*180/M_PI,yaw*180/M_PI);    

// matrix->GetElement(1,0);
  
 cout << m(0,0)  << " " << m(0,1) << " " << m(0,2)  << " "  << " x0\n";
 cout << m(1,0) << " " << m(1,1) << " " << m(1,2) << " "  << " x1\n";
  cout << m(2,0) << " " << m(2,1)  << " " << m(2,2) << " "<< "x2\n\n";
  
  Eigen::Quaternionf rf;
  rf = Eigen::Quaternionf(m);
  
  
   Eigen::Quaterniond r(rf.w(),rf.x(),rf.y(),rf.z());
  
   Eigen::Isometry3d init_pose;
   init_pose.setIdentity();
   init_pose.translation() << cam.pos[0], cam.pos[1], cam.pos[2];  
   //Eigen::Quaterniond m = euler_to_quat(-1.54, 0, 0);
   init_pose.rotate(r);  
//   
  
    std::stringstream ss;
  print_Isometry3d(init_pose,ss);
  std::cout << "init_pose: " << ss.str() << "\n";
  
  
  
  
  viewer->addCoordinateSystem (1.0,pose);
  
  
  
    double tic = getTime();
    std::stringstream ss2;
    ss2.precision(20);
    ss2 << "simulated_pcl_" << tic << ".pcd";
    capture(init_pose,ss2.str());
    cout << (getTime() -tic) << " sec\n";  
  
  
  
  // these three variables determine the position and orientation of
    // the camera.
	      
	      
//     double lookat[3]; - focal location
//     double eye[3]; - my location
//     double up[3]; - updirection
     
	      
	      
//    std::cout << view[0]  << "," << view[1]  << "," << view[2] 
    
//    cameras.back ().view[2] = renderer->GetActiveCamera ()->GetOrientationWXYZ()[2];    
    
//ViewTransform->GetOrientationWXYZ();    
    
  //  Superclass::OnKeyUp ();
    
//       vtkSmartPointer<vtkCamera> cam = event.Interactor->GetRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->GetActiveCamera ();
//       double clip[2], focal[3], pos[3], view[3];
//       cam->GetClippingRange (clip);
/*      cam->GetFocalPoint (focal);
      cam->GetPosition (pos);
      cam->GetViewUp (view);
      int *win_pos = Interactor->GetRenderWindow ()->GetPosition ();
      int *win_size = Interactor->GetRenderWindow ()->GetSize ();
      std::cerr << clip[0]  << "," << clip[1]  << "/" << focal[0] << "," << focal[1] << "," << focal[2] << "/" <<
                   pos[0]   << "," << pos[1]   << "," << pos[2]   << "/" << view[0]  << "," << view[1]  << "," << view[2] << "/" <<
                   cam->GetViewAngle () / 180.0 * M_PI  << "/" << win_size[0] << "," << win_size[1] << "/" << win_pos[0] << "," << win_pos[1]
                << endl;    
  */  
  }
}
void Viewer::keyboardEventOccurred( const pcl::visualization::KeyboardEvent &event, void* data ) {

	if( (event.getKeySym() == "d" || event.getKeySym() == "D") && event.keyDown() ) {

		if( event.getKeySym() == "d" ) {
			selectedDepth++;
		}
		else {
			selectedDepth--;
			if( selectedDepth < 0 )
				selectedDepth = 15;
		}

		selectedDepth = selectedDepth % 16;
		std::cout << "Selected Depth " << selectedDepth << "\n";
	}
	if( (event.getKeySym() == "v" || event.getKeySym() == "V") && event.keyDown() ) {

		if( event.getKeySym() == "v" ) {
			selectedViewDir++;
			if( selectedViewDir == 7 )
				selectedViewDir = -1;
		}
		else {
			selectedViewDir--;
			if( selectedViewDir < -1 )
				selectedViewDir = 6;
		}

		std::cout << "Selected View Dir " << selectedViewDir << "\n";

	}
	if( (event.getKeySym() == "p") && event.keyDown() ) {
		processFrame = true;
	}
	if( (event.getKeySym() == "h") && event.keyDown() ) {
		processFrame = !processFrame;
	}
	if( (event.getKeySym() == "s") && event.keyDown() ) {
		displayScene = !displayScene;
	}
	if( (event.getKeySym() == "m") && event.keyDown() ) {
		displayMap = !displayMap;
	}
	if( (event.getKeySym() == "a") && event.keyDown() ) {
		displayAll = !displayAll;
	}
	if( (event.getKeySym() == "c") && event.keyDown() ) {
		displayCorr = !displayCorr;
	}
	if( (event.getKeySym() == "S") && event.keyDown() ) {
		displayFeatureSimilarity = !displayFeatureSimilarity;
		std::cout << "feature similarity " << (displayFeatureSimilarity ? "on" : "off") << "\n";
	}
	if( (event.getKeySym() == "f" || event.getKeySym() == "r" || event.getKeySym() == "N") && event.keyDown() ) {
		forceRedraw = true;
	}
	if( (event.getKeySym() == "i") && event.keyDown() ) {
		forceRedraw = true;
		index++;
	}
}
void
keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}