Fixes for loading calib

iandouglas96 · Jan 15, 2022 · 76b711d · 76b711d
1 parent 093173b
commit 76b711d
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 4 deletions.
diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp
@@ -531,6 +531,6 @@ namespace ORB_SLAM3 {
 
         cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
         cv::Matx41f x3D_h = vt.row(3).t();
-        x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3);
+        x3D = x3D_h.get_minor<3,1>(0,0) * (1/x3D_h(3));
     }
 }
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
@@ -625,7 +625,7 @@ void LocalMapping::CreateNewMapPoints()
                     continue;
 
                 // Euclidean coordinates
-                x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3);
+                x3D = x3D_h.get_minor<3,1>(0,0) * (1./x3D_h(3));
                 bEstimated = true;
 
             }

diff --git a/src/Tracking.cc b/src/Tracking.cc
@@ -887,7 +887,11 @@ bool Tracking::ParseCamParamFile(cv::FileStorage &fSettings)
             node = fSettings["Tlr"];
             if(!node.empty())
             {
-                mTlr = node.mat();
+                int mTlr_rows = static_cast<int>(node["rows"]);
+                int mTlr_cols = static_cast<int>(node["cols"]);
+                mTlr = cv::Mat(mTlr_rows, mTlr_cols, CV_32FC1);
+                node["data"].readRaw("f", mTlr.data, mTlr_rows*mTlr_cols);
+
                 if(mTlr.rows != 3 || mTlr.cols != 4)
                 {
                     std::cerr << "*Tlr matrix have to be a 3x4 transformation matrix*" << std::endl;
@@ -1118,7 +1122,10 @@ bool Tracking::ParseIMUParamFile(cv::FileStorage &fSettings)
     cv::FileNode node = fSettings["Tbc"];
     if(!node.empty())
     {
-        Tbc = node.mat();
+        int Tbc_rows = static_cast<int>(node["rows"]);
+        int Tbc_cols = static_cast<int>(node["cols"]);
+        Tbc = cv::Mat(Tbc_rows, Tbc_cols, CV_32FC1);
+        node["data"].readRaw("f", Tbc.data, Tbc_rows*Tbc_cols);
         if(Tbc.rows != 4 || Tbc.cols != 4)
         {
             std::cerr << "*Tbc matrix have to be a 4x4 transformation matrix*" << std::endl;