laxnpander / OpenREALM

OpenREALM is a pipeline for real-time aerial mapping utilizing visual SLAM and 3D reconstruction frameworks.
GNU Lesser General Public License v2.1
466 stars 119 forks source link

I can't find which function called the third-party slam library in the pose estimation node #78

Closed GY56723-debug closed 2 years ago

GY56723-debug commented 2 years ago

Hello, it's me. When I look at the code, I want to find the calling method of the third-party visual slam library Cpp in addFrame function, but I didn't succeed in finding this function or method. I only found m_vslam in the constructor of poseEstimation, but m_vslam does not process input frames in addFrame function . I am confused about how to use a third-party library to process input frames in this.

the code:

PoseEstimation::PoseEstimation(const StageSettings::Ptr &stage_set, const VisualSlamSettings::Ptr &vslam_set, const CameraSettings::Ptr &cam_set, const ImuSettings::Ptr &imu_set, double rate) : StageBase("pose_estimation", (stage_set)["path_output"].toString(), rate, (stage_set)["queue_size"].toInt(), bool((stage_set)["log_to_file"].toInt())), m_is_georef_initialized(false), m_use_vslam((stage_set)["use_vslam"].toInt() > 0), m_use_imu((stage_set)["use_imu"].toInt() > 0), m_set_all_frames_keyframes((stage_set)["set_all_frames_keyframes"].toInt() > 0), m_strategy_fallback(PoseEstimation::FallbackStrategy((stage_set)["fallback_strategy"].toInt())), m_use_fallback(false), m_init_lost_frames_reset_count((stage_set)["init_lost_frames_reset_count"].toInt()), m_init_lost_frames(0), m_use_initial_guess((stage_set)["use_initial_guess"].toInt() > 0), m_do_update_georef((stage_set)["update_georef"].toInt() > 0), m_do_delay_keyframes((stage_set)["do_delay_keyframes"].toInt() > 0), m_do_suppress_outdated_pose_pub((stage_set)["suppress_outdated_pose_pub"].toInt() > 0), m_th_error_georef((stage_set)["th_error_georef"].toDouble()), m_min_nrof_frames_georef((stage_set)["min_nrof_frames_georef"].toInt()), m_do_auto_reset(false), m_th_scale_change(20.0), m_overlap_max((stage_set)["overlap_max"].toDouble()), m_overlap_max_fallback((stage_set)["overlap_max_fallback"].toDouble()), m_settings_save({(stage_set)["save_trajectory_gnss"].toInt() > 0, (stage_set)["save_trajectory_visual"].toInt() > 0, (stage_set)["save_frames"].toInt() > 0, (stage_set)["save_keyframes"].toInt() > 0, (*stage_set)["save_keyframes_full"].toInt() > 0}) { LOG_S(INFO) << "Stage [" << m_stage_name << "]: Created Stage with Settings:\n"; stage_set->print();

registerAsyncDataReadyFunctor([=]{ return !m_buffer_no_pose.empty(); });

if (m_use_vslam) { if (m_use_imu) { if (imu_set == nullptr) throw(std::runtime_error("Error creating SLAM with IMU support. No IMU settings provided!")); m_vslam = VisualSlamFactory::create(vslam_set, cam_set, imu_set); } else m_vslam = VisualSlamFactory::create(vslam_set, cam_set);

// Set reset callback from vSLAM to this node
// therefore if SLAM resets itself, node is being informed
m_vslam->registerResetCallback([=](){ reset(); });

// Set pose update callback
namespace ph = std::placeholders;
VisualSlamIF::PoseUpdateFuncCb update_func = std::bind(&PoseEstimation::updateKeyframeCb, this, ph::_1, ph::_2, ph::_3);
m_vslam->registerUpdateTransport(update_func);

// Create geo reference initializer
m_georeferencer = std::make_shared<GeometricReferencer>(m_th_error_georef, m_min_nrof_frames_georef);

}

void PoseEstimation::addFrame(const Frame::Ptr &frame) { // First update statistics about incoming frame rate updateStatisticsIncoming();

// The user can provide a-priori georeferencing. Check if this is the case if (!m_is_georef_initialized && frame->isGeoreferenced()) { LOG_F(INFO, "Detected a-priori georeference for frame %u. Assuming all frames are georeferenced.", frame->getFrameId()); m_georeferencer = std::make_shared(frame->getGeoreference()); }

// Push to buffer for visual tracking if (m_use_vslam) pushToBufferNoPose(frame); else pushToBufferPublish(frame); notify();

// Ringbuffer implementation for buffer with no pose if (m_buffer_no_pose.size() > m_queue_size) { std::unique_lock lock(m_mutex_buffer_no_pose); m_buffer_no_pose.pop_front(); updateStatisticsSkippedFrame(); }

m_transport_pose(frame->getDefaultPose(), frame->getGnssUtm().zone, frame->getGnssUtm().band, "output/pose/gnss"); }