From 2dd7e72b51176b3a0bb8ea9b3b15ec3a41b39ebd Mon Sep 17 00:00:00 2001 From: Marc Haubenstock Date: Wed, 27 Dec 2023 14:45:55 +0100 Subject: [PATCH 1/5] Update orb_slam.cpp time conversion + cam parameters fix --- modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp b/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp index d493f3c9..f85f02bf 100644 --- a/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp +++ b/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp @@ -29,8 +29,8 @@ OrbSlam::OrbSlam(const VisualSlamSettings::Ptr &vslam_set, const CameraSettings: cam.K = K_32f; cam.distCoeffs = dist_coeffs_32f; cam.fps = (*cam_set)["fps"].toFloat(); - cam.width = (*cam_set)["width"].toInt(); - cam.height = (*cam_set)["height"].toInt(); + cam.width = static_cast((*cam_set)["width"].toInt() * m_resizing); + cam.height = static_cast((*cam_set)["height"].toInt() * m_resizing); cam.isRGB = false; // BGR ORB_SLAM::OrbParameters orb{}; @@ -86,7 +86,7 @@ VisualSlamIF::State OrbSlam::track(Frame::Ptr &frame, const cv::Mat &T_c2w_initi // Set image resizing accoring to settings frame->setImageResizeFactor(m_resizing); - double timestamp = static_cast(frame->getTimestamp() - m_timestamp_reference)/10e3; + double timestamp = static_cast(frame->getTimestamp() - m_timestamp_reference)/10e9; LOG_IF_F(INFO, true, "Time elapsed since first frame: %4.2f [s]", timestamp); // ORB SLAM returns a transformation from the world to the camera frame (T_w2c). In case we provide an initial guess From 4e723bc6294dd6b3a8603a54d227cff90aeda71f Mon Sep 17 00:00:00 2001 From: Marc Haubenstock Date: Wed, 27 Dec 2023 14:47:30 +0100 Subject: [PATCH 2/5] Update ov2_slam.cpp fix timestamp conversion and camera parameters --- .../realm_vslam/realm_vslam_base/src/ov2_slam.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp b/modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp index bde02958..2f5959d8 100644 --- a/modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp +++ b/modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp @@ -68,12 +68,12 @@ Ov2Slam::Ov2Slam(const VisualSlamSettings::Ptr &vslam_set, const CameraSettings: m_slam_params->do_full_ba_ = (*vslam_set)["do_full_ba"].toInt() > 0; m_slam_params->cam_left_model_ = "pinhole"; - m_slam_params->img_left_w_ = (*cam_set)["width"].toDouble(); - m_slam_params->img_left_h_ = (*cam_set)["height"].toDouble(); - m_slam_params->fxl_ = (*cam_set)["fx"].toDouble(); - m_slam_params->fyl_ = (*cam_set)["fy"].toDouble(); - m_slam_params->cxl_ = (*cam_set)["cx"].toDouble(); - m_slam_params->cyl_ = (*cam_set)["cy"].toDouble(); + m_slam_params->img_left_w_ = (*cam_set)["width"].toDouble()*static_cast(m_resizing);; + m_slam_params->img_left_h_ = (*cam_set)["height"].toDouble()*static_cast(m_resizing);; + m_slam_params->fxl_ = (*cam_set)["fx"].toDouble()*static_cast(m_resizing); + m_slam_params->fyl_ = (*cam_set)["fy"].toDouble()*static_cast(m_resizing);; + m_slam_params->cxl_ = (*cam_set)["cx"].toDouble()*static_cast(m_resizing);; + m_slam_params->cyl_ = (*cam_set)["cy"].toDouble()*static_cast(m_resizing);; m_slam_params->k1l_ = (*cam_set)["k1"].toDouble(); m_slam_params->k2l_ = (*cam_set)["k2"].toDouble(); m_slam_params->p1l_ = (*cam_set)["p1"].toDouble(); @@ -106,7 +106,7 @@ VisualSlamIF::State Ov2Slam::track(Frame::Ptr &frame, const cv::Mat &T_c2w_initi // Set image resizing accoring to settings frame->setImageResizeFactor(m_resizing); - const double timestamp = static_cast(frame->getTimestamp())/10e3; + const double timestamp = static_cast(frame->getTimestamp())/10e9; LOG_IF_F(INFO, true, "Time stamp of frame: %4.2f [s]", timestamp); cv::Mat img = frame->getResizedImageRaw(); From e67f38a88d847a86951296b2fcbb525c4c5fcfab Mon Sep 17 00:00:00 2001 From: Marc Haubenstock Date: Wed, 27 Dec 2023 14:51:05 +0100 Subject: [PATCH 3/5] Update ov2_slam.cpp fixed timestamp --- modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp b/modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp index 2f5959d8..0dfffd0f 100644 --- a/modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp +++ b/modules/realm_vslam/realm_vslam_base/src/ov2_slam.cpp @@ -106,7 +106,7 @@ VisualSlamIF::State Ov2Slam::track(Frame::Ptr &frame, const cv::Mat &T_c2w_initi // Set image resizing accoring to settings frame->setImageResizeFactor(m_resizing); - const double timestamp = static_cast(frame->getTimestamp())/10e9; + const double timestamp = static_cast(frame->getTimestamp())/1e9; LOG_IF_F(INFO, true, "Time stamp of frame: %4.2f [s]", timestamp); cv::Mat img = frame->getResizedImageRaw(); From 422d18bd54a8c5bd91f62643d88e2ccf114ac638 Mon Sep 17 00:00:00 2001 From: Marc Haubenstock Date: Wed, 27 Dec 2023 14:51:38 +0100 Subject: [PATCH 4/5] Update orb_slam.cpp fixed timestamp --- modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp b/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp index f85f02bf..27ae9c7f 100644 --- a/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp +++ b/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp @@ -86,7 +86,7 @@ VisualSlamIF::State OrbSlam::track(Frame::Ptr &frame, const cv::Mat &T_c2w_initi // Set image resizing accoring to settings frame->setImageResizeFactor(m_resizing); - double timestamp = static_cast(frame->getTimestamp() - m_timestamp_reference)/10e9; + double timestamp = static_cast(frame->getTimestamp() - m_timestamp_reference)/1e9; LOG_IF_F(INFO, true, "Time elapsed since first frame: %4.2f [s]", timestamp); // ORB SLAM returns a transformation from the world to the camera frame (T_w2c). In case we provide an initial guess From d9dabea43ed5911677fec88aab6692ac8c3f35d9 Mon Sep 17 00:00:00 2001 From: Marc Haubenstock Date: Wed, 27 Dec 2023 14:53:04 +0100 Subject: [PATCH 5/5] Update open_vslam.cpp timestamp fix --- modules/realm_vslam/realm_vslam_base/src/open_vslam.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/realm_vslam/realm_vslam_base/src/open_vslam.cpp b/modules/realm_vslam/realm_vslam_base/src/open_vslam.cpp index eabb29da..31cf409c 100644 --- a/modules/realm_vslam/realm_vslam_base/src/open_vslam.cpp +++ b/modules/realm_vslam/realm_vslam_base/src/open_vslam.cpp @@ -76,7 +76,7 @@ VisualSlamIF::State OpenVslam::track(Frame::Ptr &frame, const cv::Mat &T_c2w_ini std::shared_ptr T_w2c_eigen; if (T_c2w_initial.empty()) { - T_w2c_eigen = m_vslam->feed_monocular_frame(frame->getResizedImageRaw(), frame->getTimestamp() * 10e-9); + T_w2c_eigen = m_vslam->feed_monocular_frame(frame->getResizedImageRaw(), frame->getTimestamp() * 1e-9); if (T_w2c_eigen != nullptr) T_w2c = convertToCv(*T_w2c_eigen); @@ -353,4 +353,4 @@ void OpenVslam::updateKeyframes() } } LOG_F(INFO, "Timing [Update KFs]: %lu ms", Timer::getCurrentTimeMilliseconds()-t); -} \ No newline at end of file +}