Use shape.

lagadic · Nov 30, 2023 · 13beef0 · 13beef0
1 parent 3b1282c
commit 13beef0
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp
@@ -56,6 +56,9 @@ int main(int argc, char **argv)
   // Load all the poses
   visp::cnpy::NpyArray arr_vec_poses = npz_data["vec_poses"];
   double* vec_poses_ptr = arr_vec_poses.data<double>();
+  assert(arr_vec_poses.shape.size() == 2);
+  assert(arr_vec_poses.shape[1] == 6);
+  size_t pose_size = arr_vec_poses.shape[1];
 
   for (int iter = 0; iter < nb_data; iter++) {
     std::string img_data_str = toString("png_image_%06d", iter);
@@ -90,8 +93,8 @@ int main(int argc, char **argv)
     // vpHomogeneousMatrix cMo(vpTranslationVector(arr_vec_pose.data<double>()[3], arr_vec_pose.data<double>()[4], arr_vec_pose.data<double>()[5]),
     //   vpThetaUVector(arr_vec_pose.data<double>()[0], arr_vec_pose.data<double>()[1], arr_vec_pose.data<double>()[2])
     // );
-    vpHomogeneousMatrix cMo(vpTranslationVector(vec_poses_ptr[6*iter + 3], vec_poses_ptr[6*iter + 4], vec_poses_ptr[6*iter + 5]),
-      vpThetaUVector(vec_poses_ptr[6*iter], vec_poses_ptr[6*iter + 1], vec_poses_ptr[6*iter + 2])
+    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])
     );
     std::cout << "\ncMo:\n" << cMo << std::endl;