diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp index 6876ad5e05..cdeb6239fc 100644 --- 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 index effa4e1bfd..ec91838e07 100644 --- 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 index 3ecefeb9ce..9ecfbdb1ee 100644 --- 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(node["rows"]); + int mTlr_cols = static_cast(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(node["rows"]); + int Tbc_cols = static_cast(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;