| 316 | } |
| 317 | |
| 318 | void |
| 319 | print_Isometry3d(Eigen::Isometry3d pose, std::stringstream& ss) |
| 320 | { |
| 321 | Eigen::Vector3d t(pose.translation()); |
| 322 | Eigen::Quaterniond r(pose.rotation()); |
| 323 | ss << t[0] << ", " << t[1] << ", " << t[2] << " | " << r.w() << ", " << r.x() << ", " |
| 324 | << r.y() << ", " << r.z(); |
| 325 | // std::cout << ss.str() << "q\n"; |
| 326 | } |
| 327 | |
| 328 | void |
| 329 | simulate_callback(const pcl::visualization::KeyboardEvent& event, void* viewer_void) |