Skip to content

Commit

Permalink
Update "tutorial-mb-generic-tracker-full.cpp" to export poses into np…
Browse files Browse the repository at this point in the history 
…z file.

Update "script/PlotCameraTrajectory.py" to read poses from npz file.
Add documentation to "visp::cnpy::npz_load()", "npz_save()", [...] functions.
  • Loading branch information
@s-trinh
s-trinh committed Jan 29, 2024
1 parent 9a6bbcf commit 4cfa003
Show file tree
Hide file tree
Showing 6 changed files with 169 additions and 50 deletions.
58 changes: 47 additions & 11 deletions modules/core/include/visp3/core/vpIoTools.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand All @@ -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;
Expand All @@ -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);
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand Down Expand Up @@ -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;
Expand Down
38 changes: 32 additions & 6 deletions modules/core/src/tools/file/vpIoTools.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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");
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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");
Expand Down Expand Up @@ -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)
{

Expand Down
33 changes: 23 additions & 10 deletions script/PlotCameraTrajectory.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down Expand Up @@ -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.')
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down
Loading

0 comments on commit 4cfa003

Please sign in to comment.