Skip to content

Commit

Permalink
TODO
Browse files Browse the repository at this point in the history
  • Loading branch information
@s-trinh
s-trinh committed Jan 28, 2024
1 parent 9a6bbcf commit 38f28d5
Show file tree
Hide file tree
Showing 4 changed files with 88 additions and 31 deletions.
29 changes: 21 additions & 8 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
83 changes: 63 additions & 20 deletions tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-full.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,18 @@
#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpVideoWriter.h>

namespace
{
std::vector<double> poseToVec(const vpHomogeneousMatrix &cMo)
{
vpThetaUVector tu = cMo.getThetaUVector();
vpTranslationVector t = cMo.getTranslationVector();
std::vector<double> 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)
Expand All @@ -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<unsigned char> I;
vpDisplay *display = nullptr;
vpPlot *plot = nullptr;
vpVideoWriter *writer = nullptr;
std::shared_ptr<vpDisplay> display;
std::shared_ptr<vpPlot> plot;
std::shared_ptr<vpVideoWriter> writer;

try {
for (int i = 0; i < argc; i++) {
Expand All @@ -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;
}
Expand Down Expand Up @@ -80,7 +96,8 @@ int main(int argc, char **argv)
<< " [--tracker <0=egde|1=keypoint|2=hybrid>]"
<< " [--downscale-img <scale factor>]"
<< " [--dof <0/1 0/1 0/1 0/1 0/1 0/1>]"
<< " [--save <video>]"
<< " [--save <e.g. results-%04d.png>]"
<< " [--save-results <e.g. tracking_poses.npz>]"
<< " [--display-scale-auto]"
<< " [--plot]"
<< " [--verbose,-v]"
Expand Down Expand Up @@ -121,12 +138,18 @@ int main(int argc, char **argv)
<< " is not estimated. It's value is the one from the initialisation." << std::endl
<< " Default: 1 1 1 1 1 1 (to estimate all 6 dof)" << std::endl
<< std::endl
<< " --save <video>" << std::endl
<< " --save <e.g. results-%04d.png>" << std::endl
<< " Name of the saved image sequence that contains tracking results in overlay." << std::endl
<< " When the name contains a folder like in the next example, the folder" << std::endl
<< " is created if it doesn't exist."
<< " Example: \"result/image-%04d.png\"." << std::endl
<< std::endl
<< " --save-results <e.g. tracking_results.npz>" << std::endl
<< " Name of the npz file containing cMo data estimated from MBT." << std::endl
<< " When the name contains a folder like in the next example, the folder" << std::endl
<< " is created if it doesn't exist."
<< " Example: \"result/tracking_results.npz\"." << std::endl
<< std::endl
<< " --display-scale-auto" << std::endl
<< " Enable display window auto scaling to ensure that the image is fully" << std::endl
<< " visible on the screen. Useful for large images." << std::endl
Expand Down Expand Up @@ -168,6 +191,13 @@ int main(int argc, char **argv)
vpIoTools::makeDirectory(parent);
}
}
if (!opt_save_results.empty()) {
std::string parent = vpIoTools::getParent(opt_save_results);
if (!parent.empty()) {
std::cout << "Create output directory for the npz file: " << parent << std::endl;
vpIoTools::makeDirectory(parent);
}
}

//! [Image]
vpImage<unsigned char> Ivideo;
Expand All @@ -191,26 +221,26 @@ int main(int argc, char **argv)

vpImage<vpRGBa> O;
if (!opt_save.empty()) {
writer = new vpVideoWriter();
writer = std::make_shared<vpVideoWriter>();
writer->setFileName(opt_save);
writer->open(O);
}

#if defined(VISP_HAVE_X11)
display = new vpDisplayX;
display = std::make_shared<vpDisplayX>();
#elif defined(VISP_HAVE_GDI)
display = new vpDisplayGDI;
display = std::make_shared<vpDisplayGDI>();
#elif defined(HAVE_OPENCV_HIGHGUI)
display = new vpDisplayOpenCV;
display = std::make_shared<vpDisplayOpenCV>();
#endif
if (opt_display_scale_auto) {
display->setDownScalingFactor(vpDisplay::SCALE_AUTO);
}
display->init(I, 100, 100, "Model-based tracker");

if (opt_plot) {
plot = new vpPlot(2, 700, 700, display->getWindowXPosition() + I.getWidth() / display->getDownScalingFactor() + 30,
display->getWindowYPosition(), "Estimated pose");
plot = std::make_shared<vpPlot>(2, 700, 700, display->getWindowXPosition() + I.getWidth() / display->getDownScalingFactor() + 30,
display->getWindowYPosition(), "Estimated pose");
plot->initGraph(0, 3); // Translation
plot->setTitle(0, "Translation [m]");
plot->setColor(0, 0, vpColor::red);
Expand Down Expand Up @@ -343,6 +373,19 @@ int main(int argc, char **argv)

std::cout << "Initialize tracker on image size: " << I.getWidth() << " x " << I.getHeight() << std::endl;

std::vector<double> vec_poses;
if (!opt_save_results.empty()) {
const unsigned int height = I.getHeight(), width = I.getWidth();
visp::cnpy::npz_save(opt_save_results, "height", &height, { 1 }, "w");
visp::cnpy::npz_save(opt_save_results, "width", &width, { 1 }, "a");

const double cam_px = cam.get_px(), cam_py = cam.get_py(), cam_u0 = cam.get_u0(), cam_v0 = cam.get_v0();
visp::cnpy::npz_save(opt_save_results, "cam_px", &cam_px, { 1 }, "a");
visp::cnpy::npz_save(opt_save_results, "cam_py", &cam_py, { 1 }, "a");
visp::cnpy::npz_save(opt_save_results, "cam_u0", &cam_u0, { 1 }, "a");
visp::cnpy::npz_save(opt_save_results, "cam_v0", &cam_v0, { 1 }, "a");
}

//! [Init]
tracker.initClick(I, objectname + ".init", true);
//! [Init]
Expand Down Expand Up @@ -414,10 +457,19 @@ int main(int argc, char **argv)
writer->saveFrame(O);
}

if (!opt_save_results.empty()) {
std::vector<double> vec_pose = poseToVec(cMo);
vec_poses.insert(vec_poses.end(), vec_pose.begin(), vec_pose.end());
}

if (vpDisplay::getClick(I, false))
break;
}
vpDisplay::getClick(I);

if (!opt_save_results.empty()) {
visp::cnpy::npz_save(opt_save_results, "vec_poses", vec_poses.data(), { static_cast<size_t>(vec_poses.size()/6), 6 }, "a");
}
}
catch (const vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
Expand All @@ -429,15 +481,6 @@ int main(int argc, char **argv)
vpDisplay::getClick(I);
}
#endif
//! [Cleanup]
delete display;
if (opt_plot) {
delete plot;
}
if (writer) {
delete writer;
}
//! [Cleanup]
#else
(void)argc;
(void)argv;
Expand Down
4 changes: 2 additions & 2 deletions tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,8 +169,8 @@ int main(int argc, char *argv[])
}
}

vpHomogeneousMatrix cMo(vpTranslationVector(vec_poses_ptr[pose_size*iter + 3], vec_poses_ptr[pose_size*iter + 4], vec_poses_ptr[pose_size*iter + 5]),
vpThetaUVector(vec_poses_ptr[pose_size*iter], vec_poses_ptr[pose_size*iter + 1], vec_poses_ptr[pose_size*iter + 2])
vpHomogeneousMatrix cMo(vpTranslationVector(vec_poses_ptr[pose_size*iter], vec_poses_ptr[pose_size*iter + 1], vec_poses_ptr[pose_size*iter + 2]),
vpThetaUVector(vec_poses_ptr[pose_size*iter + 3], vec_poses_ptr[pose_size*iter + 4], vec_poses_ptr[pose_size*iter + 5])
);

if (print_cMo) {
Expand Down
3 changes: 2 additions & 1 deletion tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-save.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ std::vector<double> poseToVec(const vpHomogeneousMatrix &cMo)
{
vpThetaUVector tu = cMo.getThetaUVector();
vpTranslationVector t = cMo.getTranslationVector();
std::vector<double> vec { tu[0], tu[1], tu[2], t[0], t[1], t[2] };
std::vector<double> vec { t[0], t[1], t[2], tu[0], tu[1], tu[2] };

return vec;
}
Expand Down Expand Up @@ -196,6 +196,7 @@ int main(int argc, char **argv)
visp::cnpy::npz_save(npz_filename, "vec_img_data_size", vec_img_data_size.data(), { vec_img_data_size.size() }, "a");
visp::cnpy::npz_save(npz_filename, "vec_img", vec_img_data.data(), { vec_img_data.size() }, "a");
// Show how to save a "multidimensional" array
assert(iter == vec_poses.size()/6);
visp::cnpy::npz_save(npz_filename, "vec_poses", vec_poses.data(), { static_cast<size_t>(iter), 6 }, "a");

visp::cnpy::npz_save(npz_filename, "nb_data", &iter, { 1 }, "a");
Expand Down

0 comments on commit 38f28d5

Please sign in to comment.