fix issue#6 (#7)

* fix issue#6

* skip release in github actions

* fix as_se3 bug

---------

Co-authored-by: bingwen.ai <[email protected]>

Author: DuinoDu

.github/workflows/build_wheels.yml

@@ -49,29 +49,29 @@ jobs:
         bash scripts/build_ffmpeg.sh
         python -m build --wheel
 
-  release:
-    needs: build_wheels
-    runs-on: ubuntu-latest
-    steps:
-    - name: Create release
-      id: create_release
-      uses: actions/create-release@v1
-      env:
-        GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} # This token is provided by Actions, you do not need to create your own token
-      with:
-        tag_name: ${{ github.ref }}
-        release_name: Release ${{ github.ref }}
-        body: |
-          Changes in this Release
-          - First release
-        draft: true
-        prerelease: false
-    - name: Upload release asset
-      uses: actions/upload-release-asset@v1
-      env:
-        GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-      with:
-        upload_url: ${{ steps.create_release.outputs.upload_url }}
-        asset_path: ./dist/spatialmp4-*.whl
-        asset_name: spatialmp4-*.whl
-        asset_content_type: application/zip
+  # release:
+  #   needs: build_wheels
+  #   runs-on: ubuntu-latest
+  #   steps:
+  #   - name: Create release
+  #     id: create_release
+  #     uses: actions/create-release@v1
+  #     env:
+  #       GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} # This token is provided by Actions, you do not need to create your own token
+  #     with:
+  #       tag_name: ${{ github.ref }}
+  #       release_name: Release ${{ github.ref }}
+  #       body: |
+  #         Changes in this Release
+  #         - First release
+  #       draft: true
+  #       prerelease: false
+  #   - name: Upload release asset
+  #     uses: actions/upload-release-asset@v1
+  #     env:
+  #       GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+  #     with:
+  #       upload_url: ${{ steps.create_release.outputs.upload_url }}
+  #       asset_path: ./dist/spatialmp4-*.whl
+  #       asset_name: spatialmp4-*.whl
+  #       asset_content_type: application/zip

bindings/spatialmp4.cpp

@@ -19,6 +19,7 @@
 #include <pybind11/numpy.h>
 #include <pybind11/eigen.h>
 #include <opencv2/core/core.hpp>
+#include <Eigen/Core>
 #include "spatialmp4/reader.h"
 #include "spatialmp4/data_types.h"
 #include "spatialmp4/utils.h"
@@ -27,6 +28,32 @@
 
 namespace py = pybind11;
 
+namespace {
+
+template <int Rows, int Cols>
+Eigen::Matrix<double, Rows, Cols> MatxToEigen(const cv::Matx<double, Rows, Cols> &matx) {
+  Eigen::Matrix<double, Rows, Cols> result;
+  for (int r = 0; r < Rows; ++r) {
+    for (int c = 0; c < Cols; ++c) {
+      result(r, c) = matx(r, c);
+    }
+  }
+  return result;
+}
+
+template <int Rows, int Cols>
+cv::Matx<double, Rows, Cols> EigenToMatx(const Eigen::Matrix<double, Rows, Cols> &matrix) {
+  cv::Matx<double, Rows, Cols> result;
+  for (int r = 0; r < Rows; ++r) {
+    for (int c = 0; c < Cols; ++c) {
+      result(r, c) = matrix(r, c);
+    }
+  }
+  return result;
+}
+
+}  // namespace
+
 // Helper function to convert cv::Mat to numpy array
 py::array_t<uint8_t> mat_to_numpy(const cv::Mat &mat) {
   if (mat.empty()) {
@@ -81,7 +108,9 @@ PYBIND11_MODULE(spatialmp4, m) {
       .def_readwrite("qx", &SpatialML::pose_frame::qx)
       .def_readwrite("qy", &SpatialML::pose_frame::qy)
       .def_readwrite("qz", &SpatialML::pose_frame::qz)
-      .def("as_se3", &SpatialML::pose_frame::as_se3)
+      .def("as_se3", [](const SpatialML::pose_frame &pose) {
+        return pose.as_se3().matrix();
+      })
       .def("__repr__", [](const SpatialML::pose_frame &p) {
         std::ostringstream ss;
         ss << p;
@@ -132,7 +161,9 @@ PYBIND11_MODULE(spatialmp4, m) {
       .def_readwrite("fy", &SpatialML::camera_intrinsics::fy)
       .def_readwrite("cx", &SpatialML::camera_intrinsics::cx)
       .def_readwrite("cy", &SpatialML::camera_intrinsics::cy)
-      .def("as_cvmat", &SpatialML::camera_intrinsics::as_cvmat)
+      .def("as_cvmat", [](const SpatialML::camera_intrinsics &intrinsics) {
+        return MatxToEigen(intrinsics.as_cvmat());
+      })
       .def("__repr__", [](const SpatialML::camera_intrinsics &i) {
         std::ostringstream ss;
         ss << i;
@@ -142,9 +173,18 @@ PYBIND11_MODULE(spatialmp4, m) {
   // Bind camera_extrinsics struct
   py::class_<SpatialML::camera_extrinsics>(m, "CameraExtrinsics")
       .def(py::init<>())
-      .def_readwrite("extrinsics", &SpatialML::camera_extrinsics::extrinsics)
-      .def("as_cvmat", &SpatialML::camera_extrinsics::as_cvmat)
-      .def("as_se3", &SpatialML::camera_extrinsics::as_se3)
+      .def_property(
+          "extrinsics",
+          [](const SpatialML::camera_extrinsics &extrinsics) { return MatxToEigen(extrinsics.extrinsics); },
+          [](SpatialML::camera_extrinsics &extrinsics, const Eigen::Matrix4d &matrix) {
+            extrinsics.extrinsics = EigenToMatx(matrix);
+          })
+      .def("as_cvmat", [](const SpatialML::camera_extrinsics &extrinsics) {
+        return MatxToEigen(extrinsics.as_cvmat());
+      })
+      .def("as_se3", [](const SpatialML::camera_extrinsics &extrinsics) {
+        return extrinsics.as_se3().matrix();
+      })
       .def("__repr__", [](const SpatialML::camera_extrinsics &e) {
         std::ostringstream ss;
         ss << e;

python/tests/test_reader.py

@@ -34,6 +34,7 @@ def test_reader():
 
         # Get pose data
         pose = rgb_frame.pose
+        pose_se3 = pose.as_se3()
         print(f"Frame timestamp: {rgb_frame.timestamp}")
         print(f"Pose: x={pose.x}, y={pose.y}, z={pose.z}, qw={pose.qw}, qx={pose.qx}, qy={pose.qy}, qz={pose.qz}")
 
@@ -48,3 +49,7 @@ def test_reader():
         # Access depth data (returns numpy array)
         depth = depth_frame.depth  # Shape: (height, width)
         print(f"Depth: {depth.shape}")
+
+
+def test_bugfix_extrinsics():
+    print(spatialmp4.Reader('video/test.mp4').get_rgb_extrinsics_left().extrinsics)

src/spatialmp4/reader.cc

@@ -791,7 +791,7 @@ void Reader::ParseDepthFrame(const AVPacket& pkt, depth_frame& frame_depth, bool
     Eigen::Vector3d t = T_W_Stof.translation();
     Eigen::Quaterniond q = T_W_Stof.unit_quaternion();
     frame_depth.pose.x = t.x();
-    frame_depth.pose.y = t.z();
+    frame_depth.pose.y = t.y();
     frame_depth.pose.z = t.z();
     frame_depth.pose.qx = q.x();
     frame_depth.pose.qy = q.y();