Update "tutorial-mb-generic-tracker-full.cpp" to export poses into np…

…z file.

Update "script/PlotCameraTrajectory.py" to read poses from npz file.
Add documentation to "visp::cnpy::npz_load()", "npz_save()", [...] functions.

modules/core/include/visp3/core/vpIoTools.h

@@ -136,18 +136,14 @@ VISP_EXPORT NpyArray npy_load(std::string fname);
 
 template<typename T> std::vector<char> &operator+=(std::vector<char> &lhs, const T rhs)
 {
-//write in little endian
+  //write in little endian
   for (size_t byte = 0; byte < sizeof(T); byte++) {
     char val = *((char *)&rhs+byte);
     lhs.push_back(val);
   }
   return lhs;
 }
 
-//template<> std::vector<char> &operator+=(std::vector<char> &lhs, const std::string rhs);
-//template<> std::vector<char> &operator+=(std::vector<char> &lhs, const char *rhs);
-
-
 template<> inline std::vector<char> &operator+=(std::vector<char> &lhs, const std::string rhs)
 {
   lhs.insert(lhs.end(), rhs.begin(), rhs.end());
@@ -165,6 +161,16 @@ template<> inline std::vector<char> &operator+=(std::vector<char> &lhs, const ch
   return lhs;
 }
 
+/*!
+  Save an array of data (\p data) into the \p fname npy file. This function is similar to the
+  <a href="https://numpy.org/doc/stable/reference/generated/numpy.save.html">numpy.save</a> function.
+  \param[in] fname : Path to the npy file.
+  \param[in] data : Pointer to an array of basic datatype (int, float, double, std::complex<double>, ...).
+  \param[in] shape : Shape of the array, e.g. Nz x Ny x Nx.
+  \param[in] mode : Writing mode, i.e. overwrite (w) or append (a) to the file.
+  \warning This function has only been tested on little endian platform.
+  \note Original library: <a href="https://github.com/rogersce/cnpy">cnpy</a> with MIT license.
+ */
 template<typename T> void npy_save(std::string fname, const T *data, const std::vector<size_t> shape, std::string mode = "w")
 {
   FILE *fp = NULL;
@@ -173,7 +179,7 @@ template<typename T> void npy_save(std::string fname, const T *data, const std::
   if (mode == "a") fp = fopen(fname.c_str(), "r+b");
 
   if (fp) {
-      //file exists. we need to append to it. read the header, modify the array size
+    //file exists. we need to append to it. read the header, modify the array size
     size_t word_size;
     bool fortran_order;
     parse_npy_header(fp, word_size, true_data_shape, fortran_order);
@@ -211,9 +217,20 @@ template<typename T> void npy_save(std::string fname, const T *data, const std::
   fclose(fp);
 }
 
+/*!
+  Save the specified \p fname array of data (\p data) into the \p zipname npz file. This function is similar to the
+  <a href="https://numpy.org/doc/stable/reference/generated/numpy.savez.html">numpy.savez</a> function.
+  \param[in] zipname : Path to the npz file.
+  \param[in] fname : Identifier for the corresponding array of data.
+  \param[in] data : Pointer to an array of basic datatype (int, float, double, std::complex<double>, ...).
+  \param[in] shape : Shape of the array, e.g. Nz x Ny x Nx.
+  \param[in] mode : Writing mode, i.e. overwrite (w) or append (a) to the file.
+  \warning This function has only been tested on little endian platform.
+  \note Original library: <a href="https://github.com/rogersce/cnpy">cnpy</a> with MIT license.
+ */
 template<typename T> void npz_save(std::string zipname, std::string fname, const T *data, const std::vector<size_t> &shape, std::string mode = "w")
 {
-    //first, append a .npy to the fname
+  //first, append a .npy to the fname
   fname += ".npy";
 
   //now, on with the show
@@ -225,10 +242,10 @@ template<typename T> void npz_save(std::string zipname, std::string fname, const
   if (mode == "a") fp = fopen(zipname.c_str(), "r+b");
 
   if (fp) {
-      //zip file exists. we need to add a new npy file to it.
-      //first read the footer. this gives us the offset and size of the global header
-      //then read and store the global header.
-      //below, we will write the the new data at the start of the global header then append the global header and footer below it
+    //zip file exists. we need to add a new npy file to it.
+    //first read the footer. this gives us the offset and size of the global header
+    //then read and store the global header.
+    //below, we will write the the new data at the start of the global header then append the global header and footer below it
     size_t global_header_size;
     parse_zip_footer(fp, nrecs, global_header_size, global_header_offset);
     fseek(fp, global_header_offset, SEEK_SET);
@@ -301,13 +318,32 @@ template<typename T> void npz_save(std::string zipname, std::string fname, const
   fclose(fp);
 }
 
+/*!
+  Save the specified 1-D array of data (\p data) into the \p fname npz file. This function is similar to the
+  <a href="https://numpy.org/doc/stable/reference/generated/numpy.save.html">numpy.save</a> function.
+  \param[in] fname : Path to the npy file.
+  \param[in] data : Pointer to a 1-D array of basic datatype (int, float, double, std::complex<double>, ...).
+  \param[in] mode : Writing mode, i.e. overwrite (w) or append (a) to the file.
+  \warning This function has only been tested on little endian platform.
+  \note Original library: <a href="https://github.com/rogersce/cnpy">cnpy</a> with MIT license.
+ */
 template<typename T> void npy_save(std::string fname, const std::vector<T> data, std::string mode = "w")
 {
   std::vector<size_t> shape;
   shape.push_back(data.size());
   npy_save(fname, &data[0], shape, mode);
 }
 
+/*!
+  Save the specified \p fname 1-D array of data (\p data) into the \p zipname npz file. This function is similar to the
+  <a href="https://numpy.org/doc/stable/reference/generated/numpy.savez.html">numpy.savez</a> function.
+  \param[in] zipname : Path to the npz file.
+  \param[in] fname : Identifier for the corresponding array of data.
+  \param[in] data : Pointer to a 1-D array of basic datatype (int, float, double, std::complex<double>, ...).
+  \param[in] mode : Writing mode, i.e. overwrite (w) or append (a) to the file.
+  \warning This function has only been tested on little endian platform.
+  \note Original library: <a href="https://github.com/rogersce/cnpy">cnpy</a> with MIT license.
+ */
 template<typename T> void npz_save(std::string zipname, std::string fname, const std::vector<T> data, std::string mode = "w")
 {
   std::vector<size_t> shape;

modules/core/src/tools/file/vpIoTools.cpp

@@ -107,6 +107,8 @@
 #define USE_ZLIB_API 0
 
 #if !USE_ZLIB_API
+// See: https://github.com/BinomialLLC/basis_universal/blob/master/encoder/basisu_miniz.h
+// Apache License, Version 2.0
 #include "basisu_miniz.h"
 
 using namespace buminiz;
@@ -158,9 +160,9 @@ char visp::cnpy::map_type(const std::type_info &t)
 
 void visp::cnpy::parse_npy_header(unsigned char *buffer, size_t &word_size, std::vector<size_t> &shape, bool &fortran_order)
 {
-//std::string magic_string(buffer,6);
-// uint8_t major_version = *reinterpret_cast<uint8_t*>(buffer+6);
-// uint8_t minor_version = *reinterpret_cast<uint8_t*>(buffer+7);
+  //std::string magic_string(buffer,6);
+  // uint8_t major_version = *reinterpret_cast<uint8_t*>(buffer+6);
+  // uint8_t minor_version = *reinterpret_cast<uint8_t*>(buffer+7);
   uint16_t header_len = *reinterpret_cast<uint16_t *>(buffer+8);
   std::string header(reinterpret_cast<char *>(buffer+9), header_len);
 
@@ -288,7 +290,6 @@ visp::cnpy::NpyArray load_the_npy_file(FILE *fp)
 
 visp::cnpy::NpyArray load_the_npz_array(FILE *fp, uint32_t compr_bytes, uint32_t uncompr_bytes)
 {
-
   std::vector<unsigned char> buffer_compr(compr_bytes);
   std::vector<unsigned char> buffer_uncompr(uncompr_bytes);
   size_t nread = fread(&buffer_compr[0], 1, compr_bytes, fp);
@@ -328,6 +329,14 @@ visp::cnpy::NpyArray load_the_npz_array(FILE *fp, uint32_t compr_bytes, uint32_t
   return array;
 }
 
+/*!
+  Load the specified \p fname filepath as arrays of data. This function is similar to the
+  <a href="https://numpy.org/doc/stable/reference/generated/numpy.load.html">numpy.load</a> function.
+  \param[in] fname : Path to the npz file.
+  \return A map of arrays data. The key represents the variable name, the value is an array of basic data type.
+  \warning This function has only been tested on little endian platform.
+  \note Original library: <a href="https://github.com/rogersce/cnpy">cnpy</a> with MIT license.
+ */
 visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname)
 {
   FILE *fp = fopen(fname.c_str(), "rb");
@@ -344,7 +353,7 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname)
     if (headerres != 30)
       throw std::runtime_error("npz_load: failed fread");
 
-  //if we've reached the global header, stop reading
+    //if we've reached the global header, stop reading
     if (local_header[2] != 0x03 || local_header[3] != 0x04) break;
 
     //read in the variable name
@@ -354,7 +363,7 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname)
     if (vname_res != name_len)
       throw std::runtime_error("npz_load: failed fread");
 
-  //erase the lagging .npy
+    //erase the lagging .npy
     varname.erase(varname.end()-4, varname.end());
 
     //read in the extra field
@@ -378,6 +387,15 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname)
   return arrays;
 }
 
+/*!
+  Load the specified \p varname array of data from the \p fname npz file. This function is similar to the
+  <a href="https://numpy.org/doc/stable/reference/generated/numpy.load.html">numpy.load</a> function.
+  \param[in] fname : Path to the npz file.
+  \param[in] varname : Identifier for the requested array of data.
+  \return An array of basic data type.
+  \warning This function has only been tested on little endian platform.
+  \note Original library: <a href="https://github.com/rogersce/cnpy">cnpy</a> with MIT license.
+ */
 visp::cnpy::NpyArray visp::cnpy::npz_load(std::string fname, std::string varname)
 {
   FILE *fp = fopen(fname.c_str(), "rb");
@@ -427,6 +445,14 @@ visp::cnpy::NpyArray visp::cnpy::npz_load(std::string fname, std::string varname
   throw std::runtime_error("npz_load: Variable name "+varname+" not found in "+fname);
 }
 
+/*!
+  Load the specified npy \p fname filepath as one array of data. This function is similar to the
+  <a href="https://numpy.org/doc/stable/reference/generated/numpy.load.html">numpy.load</a> function.
+  \param[in] fname : Path to the npy file.
+  \return An array of basic data type.
+  \warning This function has only been tested on little endian platform.
+  \note Original library: <a href="https://github.com/rogersce/cnpy">cnpy</a> with MIT license.
+ */
 visp::cnpy::NpyArray visp::cnpy::npy_load(std::string fname)
 {
 

script/PlotCameraTrajectory.py

@@ -101,17 +101,27 @@ def visp_rotation_to_thetau(R):
             thetau[0,2] = -thetau[0,2]
     return thetau
 
-def load_camera_poses(filename, use_thetau=False):
-    if use_thetau:
-        camera_poses_raw = np.loadtxt(filename)
+def load_camera_poses(filename, use_thetau=False, use_npz_file_format=False):
+    if use_npz_file_format:
+        datafile = np.load(filename)
+        camera_poses_raw = datafile['vec_poses']
         camera_poses = np.zeros((4*camera_poses_raw.shape[0], 4))
         for i in range(camera_poses_raw.shape[0]):
-            camera_poses[i*4:i*4+3, 0:3] = visp_thetau_to_rotation(camera_poses_raw[i, 3:])
-            camera_poses[i*4:i*4+3, 3] = camera_poses_raw[i,0:3].T
+            camera_poses[i*4:i*4+3, 0:3] = visp_thetau_to_rotation(camera_poses_raw[i,3:])
+            camera_poses[i*4:i*4+3, 3] = camera_poses_raw[i,:3].T
             camera_poses[i*4+3, 3] = 1
         return camera_poses
     else:
-        return np.loadtxt(filename)
+      if use_thetau:
+          camera_poses_raw = np.loadtxt(filename)
+          camera_poses = np.zeros((4*camera_poses_raw.shape[0], 4))
+          for i in range(camera_poses_raw.shape[0]):
+              camera_poses[i*4:i*4+3, 0:3] = visp_thetau_to_rotation(camera_poses_raw[i,3:])
+              camera_poses[i*4:i*4+3, 3] = camera_poses_raw[i,:3].T
+              camera_poses[i*4+3, 3] = 1
+          return camera_poses
+      else:
+          return np.loadtxt(filename)
 
 def inverse_homogeneoux_matrix(M):
     R = M[0:3, 0:3]
@@ -713,6 +723,8 @@ def main():
     parser.add_argument('-p', type=str, nargs=1, required=True, help='Path to poses file.')
     parser.add_argument('--theta-u', action='store_true', default=False,
                         help='If true, camera poses are expressed using [tx ty tz tux tuy tuz] formalism, otherwise in homogeneous form.')
+    parser.add_argument('--npz', action='store_true', default=False,
+                        help='If true, poses are loaded from a npz file with \"vec_poses\" field and [tx ty tz tux tuy tuz] data.')
     parser.add_argument('-m', type=str, nargs=1, required=True, help='Path to CAO model file.')
     parser.add_argument('--colormap', default='gist_rainbow', type=str, help='Colormap to use for the camera path.')
     parser.add_argument('--save', action='store_true', help='If true, save the figures on disk.')
@@ -774,8 +786,9 @@ def main():
     # Load camera poses
     camera_pose_filename = args.p[0]
     use_thetau = args.theta_u
-    print(f"Load camera poses from: {camera_pose_filename} ; Use theta-u? {use_thetau}")
-    camera_poses = load_camera_poses(camera_pose_filename, use_thetau)
+    use_npz_file_format = args.npz
+    print(f"Load camera poses from: {camera_pose_filename} ; Use theta-u? {use_thetau} ; Load from npz file? {use_npz_file_format}")
+    camera_poses = load_camera_poses(camera_pose_filename, use_thetau, use_npz_file_format)
     print("poses: ", camera_poses.shape)
 
     colormap = args.colormap
@@ -854,12 +867,12 @@ def main():
 
         for i in range(0, inverse_camera_poses.shape[0], 4*pose_step):
             camera_pose = inverse_camera_poses[i:i+4,:]
-            draw_camera(ax, camera_pose, cam_width, cam_height, cam_focal, cam_scale)
+            draw_camera(ax, camera_pose, cam_width, cam_height, cam_focal, cam_scale, camera_colors[i//4])
 
         # Draw the last camera pose
         for i in range(inverse_camera_poses.shape[0]-4, inverse_camera_poses.shape[0], 4):
             camera_pose = inverse_camera_poses[i:i+4,:]
-            draw_camera(ax, camera_pose, cam_width, cam_height, cam_focal, cam_scale)
+            draw_camera(ax, camera_pose, cam_width, cam_height, cam_focal, cam_scale, camera_colors[-1])
 
         draw_camera_path(ax, inverse_camera_poses, camera_colors)
         draw_model(ax, model)