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 eabb29d..31cf409 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 +} 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 d493f3c..27ae9c7 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)/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 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 bde0295..0dfffd0 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())/1e9; LOG_IF_F(INFO, true, "Time stamp of frame: %4.2f [s]", timestamp); cv::Mat img = frame->getResizedImageRaw();