From dc0f358c996f493a5e26c1f0dd1c1c1849deeb3c Mon Sep 17 00:00:00 2001 From: xiangxw Date: Tue, 27 May 2014 13:14:10 +0800 Subject: [PATCH] PCLVisualizer: save and restore camera information Press ctrl + s to save camera parameters and ctrl + r to restore the saved camera parameters. If a camera file is specified with "-cam" option or pcd file(s) is(are) available in the command line, the camera parameters will be written to the corresponding file and can be restored later from that file. In this case, PCLVisualizer will restore camera parameters automatically from the corresponding file when the pragram restarts with the same command line input. --- .../pcl/visualization/interactor_style.h | 81 ++++- .../pcl/visualization/pcl_visualizer.h | 29 +- visualization/src/interactor_style.cpp | 334 +++++++++++++++++- visualization/src/pcl_visualizer.cpp | 290 ++++++--------- visualization/tools/pcd_viewer.cpp | 4 +- 5 files changed, 533 insertions(+), 205 deletions(-) diff --git a/visualization/include/pcl/visualization/interactor_style.h b/visualization/include/pcl/visualization/interactor_style.h index fe5174d57aa..fa9bebbb3d7 100644 --- a/visualization/include/pcl/visualization/interactor_style.h +++ b/visualization/include/pcl/visualization/interactor_style.h @@ -90,6 +90,8 @@ namespace pcl * - g, G : display scale grid (on/off) * - u, U : display lookup table (on/off) * - r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}] + * - CTRL + s, S : save camera parameters + * - CTRL + r, R : restore camera parameters * - ALT + s, S : turn stereo mode on/off * - ALT + f, F : switch between maximized window mode and original size * - l, L : list all available geometric and color handlers for the current actor map @@ -115,7 +117,7 @@ namespace pcl max_win_height_ (), max_win_width_ (), grid_enabled_ (), grid_actor_ (), lut_enabled_ (), lut_actor_ (), snapshot_writer_ (), wif_ (), mouse_signal_ (), keyboard_signal_ (), point_picking_signal_ (), area_picking_signal_ (), stereo_anaglyph_mask_default_ (), - mouse_callback_ (), modifier_ () + mouse_callback_ (), modifier_ (), camera_file_ (), camera_ (), camera_saved_ (), win_ () {} /** \brief Empty destructor */ @@ -184,6 +186,56 @@ namespace pcl void saveScreenshot (const std::string &file); + /** \brief Save the camera parameters to disk, as a .cam file. + * \param[in] file the name of the .cam file + */ + bool + saveCameraParameters (const std::string &file); + + /** \brief Get camera parameters and save them to a \ref pcl::visualization::Camera. + * \param[out] camera the name of the \ref pcl::visualization::Camera + */ + void + getCameraParameters (Camera &camera); + + /** \brief Load camera parameters from a camera parameter file. + * \param[in] file the name of the camera parameter file + */ + bool + loadCameraParameters (const std::string &file); + + /** \brief Set the camera parameters via an intrinsics and and extrinsics matrix + * \note This assumes that the pixels are square and that the center of the image is at the center of the sensor. + * \param[in] intrinsics the intrinsics that will be used to compute the VTK camera parameters + * \param[in] extrinsics the extrinsics that will be used to compute the VTK camera parameters + * \param[in] viewport the viewport to modify camera of (0 modifies all cameras) + */ + void + setCameraParameters (const Eigen::Matrix3f &intrinsics, const Eigen::Matrix4f &extrinsics, int viewport = 0); + + /** \brief Set the camera parameters by given a full camera data structure. + * \param[in] camera camera structure containing all the camera parameters. + * \param[in] viewport the viewport to modify camera of (0 modifies all cameras) + */ + void + setCameraParameters (const Camera &camera, int viewport = 0); + + /** \brief Set camera file for camera parameter saving/restoring. + * \param[in] file the name of the camera parameter file + */ + void + setCameraFile (const std::string file) + { + camera_file_ = file; + } + + /** \brief Get camera file for camera parameter saving/restoring. */ + std::string + getCameraFile () const + { + return (camera_file_); + } + /** \brief Change the default keyboard modified from ALT to a different special key. * Allowed values are: * - INTERACTOR_KB_MOD_ALT @@ -284,6 +336,21 @@ namespace pcl void zoomOut (); + /** \brief Get camera parameters from a string vector. + * \param[in] camera A string vector: + * Clipping Range, Focal Point, Position, ViewUp, Distance, Field of View Y, Window Size, Window Pos. + * Values in each string are seperated by a ',' + */ + bool + getCameraParameters (const std::vector &camera); + + /** \brief Set render window. */ + void + setRenderWindow (const vtkSmartPointer &win) + { + win_ = win; + } + /** \brief True if we're using red-blue colors for anaglyphic stereo, false if magenta-green. */ bool stereo_anaglyph_mask_default_; @@ -293,7 +360,19 @@ namespace pcl /** \brief The keyboard modifier to use. Default: Alt. */ InteractorKeyboardModifier modifier_; + /** \brief Camera file for camera parameter saving/restoring. */ + std::string camera_file_; + /** \brief A \ref pcl::visualization::Camera for camera parameter saving/restoring. */ + Camera camera_; + /** \brief A \ref pcl::visualization::Camera is saved or not. */ + bool camera_saved_; + /** \brief The render window. + * Only used when interactor maybe not available + */ + vtkSmartPointer win_; + friend class PointPickingCallback; + friend class PCLVisualizer; }; /** \brief PCL histogram visualizer interactory style class. diff --git a/visualization/include/pcl/visualization/pcl_visualizer.h b/visualization/include/pcl/visualization/pcl_visualizer.h index 04bad14d7a4..24cbf7e8c44 100644 --- a/visualization/include/pcl/visualization/pcl_visualizer.h +++ b/visualization/include/pcl/visualization/pcl_visualizer.h @@ -1595,7 +1595,15 @@ namespace pcl bool getCameraParameters (int argc, char **argv); - /** \brief Checks whether the camera parameters were manually loaded from file.*/ + /** \brief Load camera parameters from a camera parameters file. + * \param[in] file the name of the camera parameters file + */ + bool + loadCameraParameters (const std::string &file); + + /** \brief Checks whether the camera parameters were manually loaded. + * \return True if valid "-cam" option is available in command line or a corresponding camera file is automatically loaded. + */ bool cameraParamsSet () const; @@ -1689,6 +1697,18 @@ namespace pcl void saveScreenshot (const std::string &file); + /** \brief Save the camera parameters to disk, as a .cam file. + * \param[in] file the name of the .cam file + */ + void + saveCameraParameters (const std::string &file); + + /** \brief Get camera parameters and save them to a pcl::visualization::Camera. + * \param[out] camera the name of the pcl::visualization::Camera + */ + void + getCameraParameters (Camera &camera); + /** \brief Return a pointer to the underlying VTK Render Window used. */ vtkSmartPointer getRenderWindow () @@ -2063,6 +2083,13 @@ namespace pcl int textureFromTexMaterial (const pcl::TexMaterial& tex_mat, vtkTexture* vtk_tex) const; + + /** \brief Get camera file for camera parameter saving/restoring from command line. + * Camera filename is calculated using sha1 value of all pathes of input .pcd files + * \return empty string if failed. + */ + std::string + getUniqueCameraFile (int argc, char **argv); //There's no reason these conversion functions shouldn't be public and static so others can use them. public: diff --git a/visualization/src/interactor_style.cpp b/visualization/src/interactor_style.cpp index 24855a7e16e..6afb5476b3a 100644 --- a/visualization/src/interactor_style.cpp +++ b/visualization/src/interactor_style.cpp @@ -126,6 +126,161 @@ pcl::visualization::PCLVisualizerInteractorStyle::saveScreenshot (const std::str snapshot_writer_->Write (); } +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::visualization::PCLVisualizerInteractorStyle::saveCameraParameters (const std::string &file) +{ + FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]); + + ofstream ofs_cam (file.c_str ()); + if (!ofs_cam.is_open ()) + { + return (false); + } + + vtkSmartPointer cam = 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 (); + ofs_cam << 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; + ofs_cam.close (); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::visualization::PCLVisualizerInteractorStyle::getCameraParameters (pcl::visualization::Camera &camera) +{ + FindPokedRenderer (Interactor->GetEventPosition ()[0], Interactor->GetEventPosition ()[1]); + + vtkSmartPointer cam = Interactor->GetRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->GetActiveCamera (); + cam->GetClippingRange (camera.clip); + cam->GetFocalPoint (camera.focal); + cam->GetPosition (camera.pos); + cam->GetViewUp (camera.view); + camera.fovy = cam->GetViewAngle () / 180.0 * M_PI; + int *win_pos = Interactor->GetRenderWindow ()->GetPosition (); + int *win_size = Interactor->GetRenderWindow ()->GetSize (); + camera.window_pos[0] = win_pos[0]; + camera.window_pos[1] = win_pos[1]; + camera.window_size[0] = win_size[0]; + camera.window_size[1] = win_size[1]; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::visualization::PCLVisualizerInteractorStyle::loadCameraParameters (const std::string &file) +{ + std::ifstream fs; + std::string line; + std::vector camera; + bool ret; + + fs.open (file.c_str ()); + while (!fs.eof ()) + { + getline (fs, line); + if (line == "") + continue; + + boost::split (camera, line, boost::is_any_of ("/"), boost::token_compress_on); + break; + } + fs.close (); + + ret = getCameraParameters (camera); + if (ret) + { + camera_file_ = file; + } + + return (ret); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::visualization::PCLVisualizerInteractorStyle::setCameraParameters (const Eigen::Matrix3f &intrinsics, + const Eigen::Matrix4f &extrinsics, + int viewport) +{ + // Position = extrinsic translation + Eigen::Vector3f pos_vec = extrinsics.block<3, 1> (0, 3); + + // Rotate the view vector + Eigen::Matrix3f rotation = extrinsics.block<3, 3> (0, 0); + Eigen::Vector3f y_axis (0.f, 1.f, 0.f); + Eigen::Vector3f up_vec (rotation * y_axis); + + // Compute the new focal point + Eigen::Vector3f z_axis (0.f, 0.f, 1.f); + Eigen::Vector3f focal_vec = pos_vec + rotation * z_axis; + + // Get the width and height of the image - assume the calibrated centers are at the center of the image + Eigen::Vector2i window_size; + window_size[0] = static_cast (intrinsics (0, 2)); + window_size[1] = static_cast (intrinsics (1, 2)); + + // Compute the vertical field of view based on the focal length and image heigh + double fovy = 2 * atan (window_size[1] / (2. * intrinsics (1, 1))) * 180.0 / M_PI; + + + rens_->InitTraversal (); + vtkRenderer* renderer = NULL; + int i = 0; + while ((renderer = rens_->GetNextItem ()) != NULL) + { + // Modify all renderer's cameras + if (viewport == 0 || viewport == i) + { + vtkSmartPointer cam = renderer->GetActiveCamera (); + cam->SetPosition (pos_vec[0], pos_vec[1], pos_vec[2]); + cam->SetFocalPoint (focal_vec[0], focal_vec[1], focal_vec[2]); + cam->SetViewUp (up_vec[0], up_vec[1], up_vec[2]); + cam->SetUseHorizontalViewAngle (0); + cam->SetViewAngle (fovy); + cam->SetClippingRange (0.01, 1000.01); + win_->SetSize (window_size[0], window_size[1]); + } + ++i; + } + win_->Render (); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::visualization::PCLVisualizerInteractorStyle::setCameraParameters (const pcl::visualization::Camera &camera, int viewport) +{ + rens_->InitTraversal (); + vtkRenderer* renderer = NULL; + int i = 0; + while ((renderer = rens_->GetNextItem ()) != NULL) + { + // Modify all renderer's cameras + if (viewport == 0 || viewport == i) + { + vtkSmartPointer cam = renderer->GetActiveCamera (); + cam->SetPosition (camera.pos[0], camera.pos[1], camera.pos[2]); + cam->SetFocalPoint (camera.focal[0], camera.focal[1], camera.focal[2]); + cam->SetViewUp (camera.view[0], camera.view[1], camera.view[2]); + cam->SetClippingRange (camera.clip); + cam->SetUseHorizontalViewAngle (0); + cam->SetViewAngle (camera.fovy * 180.0 / M_PI); + + win_->SetSize (static_cast (camera.window_size[0]), + static_cast (camera.window_size[1])); + } + ++i; + } +} + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::visualization::PCLVisualizerInteractorStyle::zoomIn () @@ -150,6 +305,105 @@ pcl::visualization::PCLVisualizerInteractorStyle::zoomOut () EndDolly (); } +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::visualization::PCLVisualizerInteractorStyle::getCameraParameters (const std::vector &camera) +{ + pcl::visualization::Camera camera_temp; + + // look for '/' as a separator + if (camera.size () != 7) + { + pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Camera parameters given, but with an invalid number of options (%lu vs 7)!\n", static_cast (camera.size ())); + return (false); + } + + std::string clip_str = camera.at (0); + std::string focal_str = camera.at (1); + std::string pos_str = camera.at (2); + std::string view_str = camera.at (3); + std::string fovy_str = camera.at (4); + std::string win_size_str = camera.at (5); + std::string win_pos_str = camera.at (6); + + // Get each camera setting separately and parse for ',' + std::vector clip_st; + boost::split (clip_st, clip_str, boost::is_any_of (","), boost::token_compress_on); + if (clip_st.size () != 2) + { + pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera clipping angle!\n"); + return (false); + } + camera_temp.clip[0] = atof (clip_st.at (0).c_str ()); + camera_temp.clip[1] = atof (clip_st.at (1).c_str ()); + + std::vector focal_st; + boost::split (focal_st, focal_str, boost::is_any_of (","), boost::token_compress_on); + if (focal_st.size () != 3) + { + pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera focal point!\n"); + return (false); + } + camera_temp.focal[0] = atof (focal_st.at (0).c_str ()); + camera_temp.focal[1] = atof (focal_st.at (1).c_str ()); + camera_temp.focal[2] = atof (focal_st.at (2).c_str ()); + + std::vector pos_st; + boost::split (pos_st, pos_str, boost::is_any_of (","), boost::token_compress_on); + if (pos_st.size () != 3) + { + pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera position!\n"); + return (false); + } + camera_temp.pos[0] = atof (pos_st.at (0).c_str ()); + camera_temp.pos[1] = atof (pos_st.at (1).c_str ()); + camera_temp.pos[2] = atof (pos_st.at (2).c_str ()); + + std::vector view_st; + boost::split (view_st, view_str, boost::is_any_of (","), boost::token_compress_on); + if (view_st.size () != 3) + { + pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera viewup!\n"); + return (false); + } + camera_temp.view[0] = atof (view_st.at (0).c_str ()); + camera_temp.view[1] = atof (view_st.at (1).c_str ()); + camera_temp.view[2] = atof (view_st.at (2).c_str ()); + + std::vector fovy_size_st; + boost::split (fovy_size_st, fovy_str, boost::is_any_of (","), boost::token_compress_on); + if (fovy_size_st.size () != 1) + { + pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for field of view angle!\n"); + return (false); + } + camera_temp.fovy = atof (fovy_size_st.at (0).c_str ()); + + std::vector win_size_st; + boost::split (win_size_st, win_size_str, boost::is_any_of (","), boost::token_compress_on); + if (win_size_st.size () != 2) + { + pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for window size!\n"); + return (false); + } + camera_temp.window_size[0] = atof (win_size_st.at (0).c_str ()); + camera_temp.window_size[1] = atof (win_size_st.at (1).c_str ()); + + std::vector win_pos_st; + boost::split (win_pos_st, win_pos_str, boost::is_any_of (","), boost::token_compress_on); + if (win_pos_st.size () != 2) + { + pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for window position!\n"); + return (false); + } + camera_temp.window_pos[0] = atof (win_pos_st.at (0).c_str ()); + camera_temp.window_pos[1] = atof (win_pos_st.at (1).c_str ()); + + setCameraParameters (camera_temp); + + return (true); +} + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::visualization::PCLVisualizerInteractorStyle::OnChar () @@ -200,11 +454,12 @@ pcl::visualization::PCLVisualizerInteractorStyle::OnChar () case 'u': case 'U': case 'q': case 'Q': case 'x': case 'X': - case 'r': case 'R': { break; } - // S have a special !ALT case + // R have a special !CTRL case + // S have special !ALT and !CTRL case + case 'r': case 'R': case 's': case 'S': { if (!keymod) @@ -307,6 +562,63 @@ pcl::visualization::PCLVisualizerInteractorStyle::OnKeyDown () // ---[ Check the rest of the key codes + // Save camera parameters + if ((Interactor->GetKeySym ()[0] == 'S' || Interactor->GetKeySym ()[0] == 's') && ctrl && !alt && !shift) + { + if (camera_file_.empty ()) + { + getCameraParameters (camera_); + camera_saved_ = true; + pcl::console::print_info ("Camera parameters saved, you can press CTRL + R to restore.\n"); + } + else + { + if (saveCameraParameters (camera_file_)) + { + pcl::console::print_info ("Save camera parameters to %s, you can press CTRL + R to restore.\n", camera_file_.c_str ()); + } + else + { + pcl::console::print_error ("[PCLVisualizerInteractorStyle] Can't save camera parameters to file: %s.\n", camera_file_.c_str ()); + } + } + } + + // Restore camera parameters + if ((Interactor->GetKeySym ()[0] == 'R' || Interactor->GetKeySym ()[0] == 'r') && ctrl && !alt && !shift) + { + if (camera_file_.empty ()) + { + if (camera_saved_) + { + setCameraParameters (camera_); + pcl::console::print_info ("Camera parameters restored.\n"); + } + else + { + pcl::console::print_info ("No camera parameters saved for restoring.\n"); + } + } + else + { + if (boost::filesystem::exists (camera_file_)) + { + if (loadCameraParameters (camera_file_)) + { + pcl::console::print_info ("Restore camera parameters from %s.\n", camera_file_.c_str ()); + } + else + { + pcl::console::print_error ("Can't restore camera parameters from file: %s.\n", camera_file_.c_str ()); + } + } + else + { + pcl::console::print_info ("No camera parameters saved in %s for restoring.\n", camera_file_.c_str ()); + } + } + } + // Switch between point color/geometry handlers if (Interactor->GetKeySym () && Interactor->GetKeySym ()[0] >= '0' && Interactor->GetKeySym ()[0] <= '9') { @@ -444,6 +756,8 @@ pcl::visualization::PCLVisualizerInteractorStyle::OnKeyDown () " u, U : display lookup table (on/off)\n" "\n" " r, R [+ ALT] : reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}]\n" + " CTRL + s, S : save camera parameters\n" + " CTRL + r, R : restore camera parameters\n" "\n" " ALT + s, S : turn stereo mode on/off\n" " ALT + f, F : switch between maximized window mode and original size\n" @@ -517,21 +831,7 @@ pcl::visualization::PCLVisualizerInteractorStyle::OnKeyDown () saveScreenshot (snapshot_fn); sprintf (cam_fn, "screenshot-%d.cam", t); - ofstream ofs_cam; - ofs_cam.open (cam_fn); - vtkSmartPointer cam = 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 (); - ofs_cam << 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; - ofs_cam.close (); + saveCameraParameters (cam_fn); pcl::console::print_info ("Screenshot (%s) and camera information (%s) successfully captured.\n", snapshot_fn, cam_fn); break; diff --git a/visualization/src/pcl_visualizer.cpp b/visualization/src/pcl_visualizer.cpp index 742649ca1e2..f298067b281 100644 --- a/visualization/src/pcl_visualizer.cpp +++ b/visualization/src/pcl_visualizer.cpp @@ -95,6 +95,9 @@ #include #include #include +#include +#include +#include #if defined(_WIN32) // Remove macros defined in Windows.h @@ -151,6 +154,9 @@ pcl::visualization::PCLVisualizer::PCLVisualizer (const std::string &name, const while ((renderer = rens_->GetNextItem ()) != NULL) win_->AddRenderer (renderer); + // Set renderer window in case no interactor is created + style_->setRenderWindow (win_); + // Create the interactor style style_->Initialize (); style_->setRendererCollection (rens_); @@ -182,8 +188,6 @@ pcl::visualization::PCLVisualizer::PCLVisualizer (int &argc, char **argv, const , coordinate_actor_map_ () , camera_set_ () { - style_ = style; - // Create a Renderer vtkSmartPointer ren = vtkSmartPointer::New (); ren->AddObserver (vtkCommand::EndEvent, update_fps_); @@ -201,18 +205,6 @@ pcl::visualization::PCLVisualizer::PCLVisualizer (int &argc, char **argv, const win_ = vtkSmartPointer::New (); win_->SetWindowName (name.c_str ()); - // Get screen size - int *scr_size = win_->GetScreenSize (); - - // Set default camera parameters - initCameraParameters (); - - // Parse the camera settings and update the internal camera - camera_set_ = getCameraParameters (argc, argv); - // Set the window size as 1/2 of the screen size or the user given parameter - win_->SetSize (scr_size[0]/2, scr_size[1]/2); - win_->SetPosition (0, 0); - // By default, don't use vertex buffer objects use_vbos_ = false; @@ -222,12 +214,47 @@ pcl::visualization::PCLVisualizer::PCLVisualizer (int &argc, char **argv, const while ((renderer = rens_->GetNextItem ()) != NULL) win_->AddRenderer (renderer); + // Set renderer window in case no interactor is created + style_->setRenderWindow (win_); + // Create the interactor style style_->Initialize (); style_->setRendererCollection (rens_); style_->setCloudActorMap (cloud_actor_map_); style_->UseTimersOn (); + // Get screen size + int *scr_size = win_->GetScreenSize (); + + // Set default camera parameters + initCameraParameters (); + + // Parse the camera settings and update the internal camera + camera_set_ = getCameraParameters (argc, argv); + // Calculate unique camera filename for camera parameter saving/restoring + if (!camera_set_) + { + std::string camera_file = getUniqueCameraFile (argc, argv); + if (!camera_file.empty ()) + { + if (boost::filesystem::exists (camera_file) && style_->loadCameraParameters (camera_file)) + { + pcl::console::print_info ("\nRestore camera parameters from %s.\n", camera_file.c_str ()); + camera_set_ = true; + } + else + { + style_->setCameraFile (camera_file); + } + } + } + // Set the window size as 1/2 of the screen size or the user given parameter + if (!camera_set_) + { + win_->SetSize (scr_size[0]/2, scr_size[1]/2); + win_->SetPosition (0, 0); + } + if (create_interactor) createInteractor (); @@ -391,6 +418,20 @@ pcl::visualization::PCLVisualizer::saveScreenshot (const std::string &file) style_->saveScreenshot (file); } +///////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::visualization::PCLVisualizer::saveCameraParameters (const std::string &file) +{ + style_->saveCameraParameters (file); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::visualization::PCLVisualizer::getCameraParameters (pcl::visualization::Camera &camera) +{ + style_->getCameraParameters (camera); +} + ///////////////////////////////////////////////////////////////////////////////////////////// boost::signals2::connection pcl::visualization::PCLVisualizer::registerKeyboardCallback (boost::function callback) @@ -1869,74 +1910,14 @@ pcl::visualization::PCLVisualizer::setCameraParameters (const Eigen::Matrix3f &i const Eigen::Matrix4f &extrinsics, int viewport) { - // Position = extrinsic translation - Eigen::Vector3f pos_vec = extrinsics.block<3, 1> (0, 3); - - // Rotate the view vector - Eigen::Matrix3f rotation = extrinsics.block<3, 3> (0, 0); - Eigen::Vector3f y_axis (0.f, 1.f, 0.f); - Eigen::Vector3f up_vec (rotation * y_axis); - - // Compute the new focal point - Eigen::Vector3f z_axis (0.f, 0.f, 1.f); - Eigen::Vector3f focal_vec = pos_vec + rotation * z_axis; - - // Get the width and height of the image - assume the calibrated centers are at the center of the image - Eigen::Vector2i window_size; - window_size[0] = static_cast (intrinsics (0, 2)); - window_size[1] = static_cast (intrinsics (1, 2)); - - // Compute the vertical field of view based on the focal length and image heigh - double fovy = 2 * atan (window_size[1] / (2. * intrinsics (1, 1))) * 180.0 / M_PI; - - - rens_->InitTraversal (); - vtkRenderer* renderer = NULL; - int i = 0; - while ((renderer = rens_->GetNextItem ()) != NULL) - { - // Modify all renderer's cameras - if (viewport == 0 || viewport == i) - { - vtkSmartPointer cam = renderer->GetActiveCamera (); - cam->SetPosition (pos_vec[0], pos_vec[1], pos_vec[2]); - cam->SetFocalPoint (focal_vec[0], focal_vec[1], focal_vec[2]); - cam->SetViewUp (up_vec[0], up_vec[1], up_vec[2]); - cam->SetUseHorizontalViewAngle (0); - cam->SetViewAngle (fovy); - cam->SetClippingRange (0.01, 1000.01); - win_->SetSize (window_size[0], window_size[1]); - } - ++i; - } - win_->Render (); + style_->setCameraParameters (intrinsics, extrinsics, viewport); } ///////////////////////////////////////////////////////////////////////////////////////////// void pcl::visualization::PCLVisualizer::setCameraParameters (const pcl::visualization::Camera &camera, int viewport) { - rens_->InitTraversal (); - vtkRenderer* renderer = NULL; - int i = 0; - while ((renderer = rens_->GetNextItem ()) != NULL) - { - // Modify all renderer's cameras - if (viewport == 0 || viewport == i) - { - vtkSmartPointer cam = renderer->GetActiveCamera (); - cam->SetPosition (camera.pos[0], camera.pos[1], camera.pos[2]); - cam->SetFocalPoint (camera.focal[0], camera.focal[1], camera.focal[2]); - cam->SetViewUp (camera.view[0], camera.view[1], camera.view[2]); - cam->SetClippingRange (camera.clip); - cam->SetUseHorizontalViewAngle (0); - cam->SetViewAngle (camera.fovy * 180.0 / M_PI); - - win_->SetSize (static_cast (camera.window_size[0]), - static_cast (camera.window_size[1])); - } - ++i; - } + style_->setCameraParameters (camera, viewport); } ///////////////////////////////////////////////////////////////////////////////////////////// @@ -2021,133 +2002,36 @@ pcl::visualization::PCLVisualizer::resetCameraViewpoint (const std::string &id) bool pcl::visualization::PCLVisualizer::getCameraParameters (int argc, char **argv) { - Camera camera_temp; for (int i = 1; i < argc; i++) { if ((strcmp (argv[i], "-cam") == 0) && (++i < argc)) { - std::ifstream fs; std::string camfile = std::string (argv[i]); - std::string line; - std::vector camera; if (camfile.find (".cam") == std::string::npos) { // Assume we have clip/focal/pos/view + std::vector camera; boost::split (camera, argv[i], boost::is_any_of ("/"), boost::token_compress_on); + return (style_->getCameraParameters (camera)); } else { // Assume that if we don't have clip/focal/pos/view, a filename.cam was given as a parameter - fs.open (camfile.c_str ()); - while (!fs.eof ()) - { - getline (fs, line); - if (line == "") - continue; - - boost::split (camera, line, boost::is_any_of ("/"), boost::token_compress_on); - break; - } - fs.close (); - } - - // look for '/' as a separator - if (camera.size () != 7) - { - pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Camera parameters given, but with an invalid number of options (%lu vs 7)!\n", static_cast (camera.size ())); - return (false); - } - - std::string clip_str = camera.at (0); - std::string focal_str = camera.at (1); - std::string pos_str = camera.at (2); - std::string view_str = camera.at (3); - std::string fovy_str = camera.at (4); - std::string win_size_str = camera.at (5); - std::string win_pos_str = camera.at (6); - - // Get each camera setting separately and parse for ',' - std::vector clip_st; - boost::split (clip_st, clip_str, boost::is_any_of (","), boost::token_compress_on); - if (clip_st.size () != 2) - { - pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera clipping angle!\n"); - return (false); - } - camera_temp.clip[0] = atof (clip_st.at (0).c_str ()); - camera_temp.clip[1] = atof (clip_st.at (1).c_str ()); - - std::vector focal_st; - boost::split (focal_st, focal_str, boost::is_any_of (","), boost::token_compress_on); - if (focal_st.size () != 3) - { - pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera focal point!\n"); - return (false); - } - camera_temp.focal[0] = atof (focal_st.at (0).c_str ()); - camera_temp.focal[1] = atof (focal_st.at (1).c_str ()); - camera_temp.focal[2] = atof (focal_st.at (2).c_str ()); - - std::vector pos_st; - boost::split (pos_st, pos_str, boost::is_any_of (","), boost::token_compress_on); - if (pos_st.size () != 3) - { - pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera position!\n"); - return (false); - } - camera_temp.pos[0] = atof (pos_st.at (0).c_str ()); - camera_temp.pos[1] = atof (pos_st.at (1).c_str ()); - camera_temp.pos[2] = atof (pos_st.at (2).c_str ()); - - std::vector view_st; - boost::split (view_st, view_str, boost::is_any_of (","), boost::token_compress_on); - if (view_st.size () != 3) - { - pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for camera viewup!\n"); - return (false); - } - camera_temp.view[0] = atof (view_st.at (0).c_str ()); - camera_temp.view[1] = atof (view_st.at (1).c_str ()); - camera_temp.view[2] = atof (view_st.at (2).c_str ()); - - std::vector fovy_size_st; - boost::split (fovy_size_st, fovy_str, boost::is_any_of (","), boost::token_compress_on); - if (fovy_size_st.size () != 1) - { - pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for field of view angle!\n"); - return (false); - } - camera_temp.fovy = atof (fovy_size_st.at (0).c_str ()); - - std::vector win_size_st; - boost::split (win_size_st, win_size_str, boost::is_any_of (","), boost::token_compress_on); - if (win_size_st.size () != 2) - { - pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for window size!\n"); - return (false); - } - camera_temp.window_size[0] = atof (win_size_st.at (0).c_str ()); - camera_temp.window_size[1] = atof (win_size_st.at (1).c_str ()); - - std::vector win_pos_st; - boost::split (win_pos_st, win_pos_str, boost::is_any_of (","), boost::token_compress_on); - if (win_pos_st.size () != 2) - { - pcl::console::print_error ("[PCLVisualizer::getCameraParameters] Invalid parameters given for window position!\n"); - return (false); + return (style_->loadCameraParameters (camfile)); } - camera_temp.window_pos[0] = atof (win_pos_st.at (0).c_str ()); - camera_temp.window_pos[1] = atof (win_pos_st.at (1).c_str ()); - - setCameraParameters (camera_temp); - - return (true); } } return (false); } +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::visualization::PCLVisualizer::loadCameraParameters (const std::string &file) +{ + return (style_->loadCameraParameters (file)); +} + //////////////////////////////////////////////////////////////////////////////////////////// bool pcl::visualization::PCLVisualizer::addCylinder (const pcl::ModelCoefficients &coefficients, @@ -4481,3 +4365,43 @@ pcl::visualization::PCLVisualizer::textureFromTexMaterial (const pcl::TexMateria return (0); } + +////////////////////////////////////////////////////////////////////////////////////////////// +std::string +pcl::visualization::PCLVisualizer::getUniqueCameraFile (int argc, char **argv) +{ + std::vector p_file_indices; + boost::uuids::detail::sha1 sha1; + unsigned int digest[5]; + const char *str; + std::ostringstream sstream; + bool valid = false; + + p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); + if (p_file_indices.size () != 0) + { + // Calculate sha1 using canonical paths + for (size_t i = 0; i < p_file_indices.size (); ++i) + { + boost::filesystem::path path (argv[p_file_indices[i]]); + if (boost::filesystem::exists (path)) + { + path = boost::filesystem::canonical (path); + str = path.string ().c_str (); + sha1.process_bytes (str, std::strlen (str)); + valid = true; + } + } + + // Build camera filename + if (valid) + { + sha1.get_digest (digest); + sstream << "."; + sstream << std::hex << digest[0] << digest[1] << digest[2] << digest[3] << digest[4]; + sstream << ".cam"; + } + } + + return (sstream.str ()); +} diff --git a/visualization/tools/pcd_viewer.cpp b/visualization/tools/pcd_viewer.cpp index 16bee346ce9..6c5fcada922 100644 --- a/visualization/tools/pcd_viewer.cpp +++ b/visualization/tools/pcd_viewer.cpp @@ -229,8 +229,6 @@ main (int argc, char** argv) if (debug) pcl::console::setVerbosityLevel (pcl::console::L_DEBUG); - bool cam = pcl::console::find_switch (argc, argv, "-cam"); - // Parse the command line arguments for .pcd files std::vector p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); std::vector vtk_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".vtk"); @@ -474,7 +472,7 @@ main (int argc, char** argv) // Set whether or not we should be using the vtkVertexBufferObjectMapper p->setUseVbos (use_vbos); - if (!cam) + if (!p->cameraParamsSet ()) { Eigen::Matrix3f rotation; rotation = orientation;