From 4cfa0037a9db628796ba35fe55f2b4abd976c78f Mon Sep 17 00:00:00 2001 From: Souriya Trinh Date: Thu, 25 Jan 2024 00:06:43 +0100 Subject: [PATCH] Update "tutorial-mb-generic-tracker-full.cpp" to export poses into npz 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 | 58 ++++++++++--- modules/core/src/tools/file/vpIoTools.cpp | 38 +++++++-- script/PlotCameraTrajectory.py | 33 +++++--- .../tutorial-mb-generic-tracker-full.cpp | 83 ++++++++++++++----- .../tutorial-mb-generic-tracker-read.cpp | 4 +- .../tutorial-mb-generic-tracker-save.cpp | 3 +- 6 files changed, 169 insertions(+), 50 deletions(-) diff --git a/modules/core/include/visp3/core/vpIoTools.h b/modules/core/include/visp3/core/vpIoTools.h index d118197616..d473f60c03 100644 --- a/modules/core/include/visp3/core/vpIoTools.h +++ b/modules/core/include/visp3/core/vpIoTools.h @@ -136,7 +136,7 @@ VISP_EXPORT NpyArray npy_load(std::string fname); template std::vector &operator+=(std::vector &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); @@ -144,10 +144,6 @@ template std::vector &operator+=(std::vector &lhs, const return lhs; } -//template<> std::vector &operator+=(std::vector &lhs, const std::string rhs); -//template<> std::vector &operator+=(std::vector &lhs, const char *rhs); - - template<> inline std::vector &operator+=(std::vector &lhs, const std::string rhs) { lhs.insert(lhs.end(), rhs.begin(), rhs.end()); @@ -165,6 +161,16 @@ template<> inline std::vector &operator+=(std::vector &lhs, const ch return lhs; } +/*! + Save an array of data (\p data) into the \p fname npy file. This function is similar to the + numpy.save function. + \param[in] fname : Path to the npy file. + \param[in] data : Pointer to an array of basic datatype (int, float, double, std::complex, ...). + \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: cnpy with MIT license. + */ template void npy_save(std::string fname, const T *data, const std::vector shape, std::string mode = "w") { FILE *fp = NULL; @@ -173,7 +179,7 @@ template 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 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 + numpy.savez 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, ...). + \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: cnpy with MIT license. + */ template void npz_save(std::string zipname, std::string fname, const T *data, const std::vector &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 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,6 +318,15 @@ template 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 + numpy.save 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, ...). + \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: cnpy with MIT license. + */ template void npy_save(std::string fname, const std::vector data, std::string mode = "w") { std::vector shape; @@ -308,6 +334,16 @@ template void npy_save(std::string fname, const std::vector data, 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 + numpy.savez 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, ...). + \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: cnpy with MIT license. + */ template void npz_save(std::string zipname, std::string fname, const std::vector data, std::string mode = "w") { std::vector shape; diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index 9c4e05e56b..e228872ce8 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/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 &shape, bool &fortran_order) { -//std::string magic_string(buffer,6); -// uint8_t major_version = *reinterpret_cast(buffer+6); -// uint8_t minor_version = *reinterpret_cast(buffer+7); + //std::string magic_string(buffer,6); + // uint8_t major_version = *reinterpret_cast(buffer+6); + // uint8_t minor_version = *reinterpret_cast(buffer+7); uint16_t header_len = *reinterpret_cast(buffer+8); std::string header(reinterpret_cast(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 buffer_compr(compr_bytes); std::vector 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 + numpy.load 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: cnpy 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 + numpy.load 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: cnpy 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 + numpy.load 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: cnpy with MIT license. + */ visp::cnpy::NpyArray visp::cnpy::npy_load(std::string fname) { diff --git a/script/PlotCameraTrajectory.py b/script/PlotCameraTrajectory.py index 8b8f5a7126..09ff0d2894 100644 --- a/script/PlotCameraTrajectory.py +++ b/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) diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp index 1640de31e9..a8f8e06bd4 100644 --- a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp +++ b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp @@ -10,6 +10,18 @@ #include #include +namespace +{ +std::vector poseToVec(const vpHomogeneousMatrix &cMo) +{ + vpThetaUVector tu = cMo.getThetaUVector(); + vpTranslationVector t = cMo.getTranslationVector(); + std::vector vec { t[0], t[1], t[2], tu[0], tu[1], tu[2] }; + + return vec; +} +} + int main(int argc, char **argv) { #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && defined(HAVE_OPENCV_HIGHGUI) @@ -23,12 +35,13 @@ int main(int argc, char **argv) bool opt_display_scale_auto = false; vpColVector opt_dof_to_estimate(6, 1.); // Here we consider 6 dof estimation std::string opt_save; + std::string opt_save_results; unsigned int thickness = 2; vpImage I; - vpDisplay *display = nullptr; - vpPlot *plot = nullptr; - vpVideoWriter *writer = nullptr; + std::shared_ptr display; + std::shared_ptr plot; + std::shared_ptr writer; try { for (int i = 0; i < argc; i++) { @@ -50,6 +63,9 @@ int main(int argc, char **argv) else if (std::string(argv[i]) == "--save") { opt_save = std::string(argv[++i]); } + else if (std::string(argv[i]) == "--save-results") { + opt_save_results = std::string(argv[++i]); + } else if (std::string(argv[i]) == "--plot") { opt_plot = true; } @@ -80,7 +96,8 @@ int main(int argc, char **argv) << " [--tracker <0=egde|1=keypoint|2=hybrid>]" << " [--downscale-img ]" << " [--dof <0/1 0/1 0/1 0/1 0/1 0/1>]" - << " [--save