Function simulate_callback

simulation/tools/sim_viewer.cpp:328–403

}

void
simulate_callback(const pcl::visualization::KeyboardEvent& event, void* viewer_void)
{
  pcl::visualization::PCLVisualizer::Ptr viewer =
      *static_cast<pcl::visualization::PCLVisualizer::Ptr*>(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;
    }
    // std::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

    std::cout << pose(0, 0) << " " << pose(0, 1) << " " << pose(0, 2) << " "
              << pose(0, 3) << " x0\n";
    std::cout << pose(1, 0) << " " << pose(1, 1) << " " << pose(1, 2) << " "
              << pose(1, 3) << " x1\n";
    std::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);

    std::cout << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << " "
              << " x0\n";
    std::cout << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << " "
              << " x1\n";
    std::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];