Fix camera parameters

ChangeLog.txt

@@ -69,6 +69,8 @@ ViSP 3.5.1 (under development)
     . New tutorial: Exporting a 3D model to MegaPose
       https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-megapose-model.html
   - Bug fixed
+    . [#1041] [example/device/framegrabber/saveRealSenseData] Wrong camera parameters
+              and depth_M_color homogeneous matrix when aligned depth is requested
     . [#1042] testColorConversion.cpp segfault randomly
     . [#1045] CXX standard not propagated when using pkg-config or visp-config
     . [#1046] vpIoTools::getParent() returns an empty string when folder separator is not found
@@ -81,6 +83,7 @@ ViSP 3.5.1 (under development)
     . [#1155] Wrong representation of the ellipse in model-based tracker
     . [#1160] MBT does not estimate dof set by the user using vpMbTracker::setEstimatedDoF()
     . [#1176] ViSP cannot be built with a subset of OpenCV modules
+    . [#1198] Unable to save multiple cameras in a same xml file
 ----------------------------------------------
 ViSP 3.5.0 (released February 15, 2022)
   - Contributors:

doc/tutorial/unix/tutorial-install-jetson-quasar.dox

@@ -170,7 +170,7 @@ $ cd librealsense
 
 4. Run Intel Realsense permissions script located in `librealsense` root directory:
 \verbatim
-$ ./scripts/setup_udev_rules.sh
+$ sudo ./scripts/setup_udev_rules.sh
 \endverbatim
 
 5. Build and install librealsense

doc/tutorial/unix/tutorial-install-ubuntu.dox

@@ -141,7 +141,7 @@ $ cd librealsense
 
 4. Run Intel Realsense permissions script located in `librealsense` root directory:
 \verbatim
-$ ./scripts/setup_udev_rules.sh
+$ sudo ./scripts/setup_udev_rules.sh
 \endverbatim
 
 5. Build and install librealsense

doc/tutorial/visual-servo/tutorial-franka-pbvs.dox

@@ -185,7 +185,7 @@ $ cd librealsense
 
 4. Run Intel Realsense permissions script located in `librealsense` root directory:
 \verbatim
-$ ./scripts/setup_udev_rules.sh
+$ sudo ./scripts/setup_udev_rules.sh
 \endverbatim
 
 5. Build and install librealsense

example/device/framegrabber/readRealSenseData.cpp

@@ -30,6 +30,12 @@
  *
 *****************************************************************************/
 
+/*!
+  \example readRealSenseData.cpp
+
+  \brief Example that show how to replay realsense data saved with saveRealSenseData.cpp
+*/
+
 #include <iostream>
 
 #include <visp3/core/vpConfig.h>
@@ -149,7 +155,7 @@ bool cancelled = false, update_pointcloud = false;
 class ViewerWorker
 {
 public:
-  explicit ViewerWorker(std::mutex &mutex) : m_mutex(mutex) {}
+  explicit ViewerWorker(std::mutex &mutex) : m_mutex(mutex) { }
 
   void run()
   {
@@ -183,7 +189,8 @@ class ViewerWorker
           viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
           viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
           first_init = false;
-        } else {
+        }
+        else {
           viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
         }
       }
@@ -281,7 +288,8 @@ bool readData(int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_co
         point_cloud->points[(size_t)(i * width + j)].z = z;
       }
     }
-  } else {
+  }
+  else {
     if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename_pointcloud, *point_cloud) == -1) {
       std::cerr << "Cannot read PCD: " << filename_pointcloud << std::endl;
     }

example/device/framegrabber/saveRealSenseData.cpp

@@ -28,12 +28,18 @@
  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
  *
-*****************************************************************************/
+ *****************************************************************************/
+
+ /*!
+   \example saveRealSenseData.cpp
+
+   \brief Example that show how to save realsense data that can be replayed with readRealSenseData.cpp
+ */
 
 #include <iostream>
 
 #include <visp3/core/vpConfig.h>
-#if (defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2)) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) &&  \
+#if (defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2)) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
     (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
 
 #include <condition_variable>
@@ -57,7 +63,7 @@
 #include <visp3/sensor/vpRealSense.h>
 #include <visp3/sensor/vpRealSense2.h>
 
-// Priority to libRealSense2
+ // Priority to libRealSense2
 #if defined(VISP_HAVE_REALSENSE2)
 #define USE_REALSENSE2
 #endif
@@ -168,7 +174,8 @@ bool getOptions(int argc, char **argv, bool &save, std::string &output_directory
     // standalone param or error
     usage(argv[0], NULL);
     std::cerr << "ERROR: " << std::endl;
-    std::cerr << "  Bad argument " << optarg << std::endl << std::endl;
+    std::cerr << "  Bad argument " << optarg << std::endl
+      << std::endl;
     return false;
   }
 
@@ -178,14 +185,13 @@ bool getOptions(int argc, char **argv, bool &save, std::string &output_directory
 class FrameQueue
 {
 public:
-  struct cancelled {
-  };
+  struct cancelled
+  { };
 
   FrameQueue()
     : m_cancelled(false), m_cond(), m_queueColor(), m_queueDepth(), m_queuePointCloud(), m_queueInfrared(),
-      m_maxQueueSize(1024 * 8), m_mutex()
-  {
-  }
+    m_maxQueueSize(1024 * 8), m_mutex()
+  { }
 
   void cancel()
   {
@@ -272,14 +278,14 @@ class FrameQueue
 private:
   bool m_cancelled;
   std::condition_variable m_cond;
-  std::queue<vpImage<vpRGBa> > m_queueColor;
-  std::queue<vpImage<uint16_t> > m_queueDepth;
+  std::queue<vpImage<vpRGBa>> m_queueColor;
+  std::queue<vpImage<uint16_t>> m_queueDepth;
 #ifdef VISP_HAVE_PCL
   std::queue<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_queuePointCloud;
 #else
-  std::queue<std::vector<vpColVector> > m_queuePointCloud;
+  std::queue<std::vector<vpColVector>> m_queuePointCloud;
 #endif
-  std::queue<vpImage<unsigned char> > m_queueInfrared;
+  std::queue<vpImage<unsigned char>> m_queueInfrared;
   size_t m_maxQueueSize;
   std::mutex m_mutex;
 };
@@ -298,16 +304,15 @@ class StorageWorker
 #ifndef VISP_HAVE_PCL
                     height
 #endif
-                )
+  )
     : m_queue(queue), m_directory(directory), m_cpt(0), m_save_color(save_color), m_save_depth(save_depth),
-      m_save_pointcloud(save_pointcloud), m_save_infrared(save_infrared),
-      m_save_pointcloud_binary_format(save_pointcloud_binary_format)
+    m_save_pointcloud(save_pointcloud), m_save_infrared(save_infrared),
+    m_save_pointcloud_binary_format(save_pointcloud_binary_format)
 #ifndef VISP_HAVE_PCL
-      ,
-      m_size_height(height), m_size_width(width)
+    ,
+    m_size_height(height), m_size_width(width)
 #endif
-  {
-  }
+  { }
 
   // Thread main loop
   void run()
@@ -411,7 +416,8 @@ class StorageWorker
                 }
 #endif
               }
-            } else {
+            }
+            else {
 #ifdef VISP_HAVE_PCL
               pcl::io::savePCDFileBinary(filename_point_cloud, *pointCloud);
 #endif
@@ -430,7 +436,8 @@ class StorageWorker
           m_cpt++;
         }
       }
-    } catch (const FrameQueue::cancelled &) {
+    }
+    catch (const FrameQueue::cancelled &) {
       std::cout << "Receive cancel FrameQueue." << std::endl;
     }
   }
@@ -553,12 +560,20 @@ int main(int argc, char *argv[])
     vpXmlParserCamera xml_camera;
     xml_camera.save(cam_color, output_directory + "/camera.xml", "color_camera", width, height);
 
-    vpCameraParameters cam_depth = realsense.getCameraParameters(RS2_STREAM_DEPTH);
-    xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
+    if (use_aligned_stream) {
+      xml_camera.save(cam_color, output_directory + "/camera.xml", "depth_camera", width, height);
+    }
+    else {
+      vpCameraParameters cam_depth = realsense.getCameraParameters(RS2_STREAM_DEPTH);
+      xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
+    }
 
     vpCameraParameters cam_infrared = realsense.getCameraParameters(RS2_STREAM_INFRARED);
     xml_camera.save(cam_infrared, output_directory + "/camera.xml", "infrared_camera", width, height);
-    vpHomogeneousMatrix depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
+    vpHomogeneousMatrix depth_M_color;
+    if (!use_aligned_stream) {
+      depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
+    }
 #else
     vpCameraParameters cam_color = realsense.getCameraParameters(rs::stream::color);
     vpXmlParserCamera xml_camera;
@@ -567,17 +582,25 @@ int main(int argc, char *argv[])
     vpCameraParameters cam_color_rectified = realsense.getCameraParameters(rs::stream::rectified_color);
     xml_camera.save(cam_color_rectified, output_directory + "/camera.xml", "color_camera_rectified", width, height);
 
-    vpCameraParameters cam_depth = realsense.getCameraParameters(rs::stream::depth);
-    xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
+    if (use_aligned_stream) {
+      vpCameraParameters cam_depth = realsense.getCameraParameters(rs::stream::depth);
+      xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
+    }
+    else {
+      xml_camera.save(cam_color, output_directory + "/camera.xml", "depth_camera", width, height);
+    }
 
     vpCameraParameters cam_depth_aligned_to_rectified_color =
-        realsense.getCameraParameters(rs::stream::depth_aligned_to_rectified_color);
+      realsense.getCameraParameters(rs::stream::depth_aligned_to_rectified_color);
     xml_camera.save(cam_depth_aligned_to_rectified_color, output_directory + "/camera.xml",
                     "depth_camera_aligned_to_rectified_color", width, height);
 
     vpCameraParameters cam_infrared = realsense.getCameraParameters(rs::stream::infrared);
     xml_camera.save(cam_infrared, output_directory + "/camera.xml", "infrared_camera", width, height);
-    vpHomogeneousMatrix depth_M_color = realsense.getTransformation(rs::stream::color, rs::stream::depth);
+    vpHomogeneousMatrix depth_M_color;
+    if (!use_aligned_stream) {
+      depth_M_color = realsense.getTransformation(rs::stream::color, rs::stream::depth);
+    }
 #endif
     std::ofstream file(std::string(output_directory + "/depth_M_color.txt"));
     depth_M_color.save(file);
@@ -593,8 +616,8 @@ int main(int argc, char *argv[])
   rs2::align align_to(RS2_STREAM_COLOR);
   if (use_aligned_stream && save_infrared) {
     std::cerr << "Cannot use aligned streams with infrared acquisition currently."
-                 "\nInfrared stream acquisition is disabled!"
-              << std::endl;
+      "\nInfrared stream acquisition is disabled!"
+      << std::endl;
   }
 #endif
 
@@ -626,7 +649,8 @@ int main(int argc, char *argv[])
                         rs::stream::depth_aligned_to_rectified_color);
 #endif
 #endif
-    } else {
+    }
+    else {
 #ifdef VISP_HAVE_PCL
       realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, NULL, pointCloud,
                         (unsigned char *)I_infrared.bitmap, NULL);
@@ -645,7 +669,8 @@ int main(int argc, char *argv[])
 
     if (!click_to_save) {
       vpDisplay::displayText(I_gray, 20, 20, "Click to quit.", vpColor::red);
-    } else {
+    }
+    else {
       std::stringstream ss;
       ss << "Images saved:" << nb_saves;
       vpDisplay::displayText(I_gray, 20, 20, ss.str(), vpColor::red);
@@ -669,7 +694,8 @@ int main(int argc, char *argv[])
       if (!click_to_save) {
         save_queue.cancel();
         quit = true;
-      } else {
+      }
+      else {
         switch (button) {
         case vpMouseButton::button1:
           if (save) {