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;
}