Get keyframes

[dubing-bit]

I'm completing a python API to get image pose and keypoint via stella_vslam,

using namespace stella_vslam;
class OpenVSLAM
{
public:
    using Mat44_t = Eigen::Matrix4d;
    using Ptr = std::shared_ptr<OpenVSLAM>;
    OpenVSLAM(const std::shared_ptr<stella_vslam::config> config_set,const string vocab_file_path):
        m_base_point_id(0),
        m_max_point_id(0),
        m_nrof_keyframes(0),
        m_previous_state(stella_vslam::tracker_state_t::Initializing),
        //m_max_keyframe_links(10),
        m_last_keyframe(nullptr) 
    {
        // m_config = config_set;
        m_vslam =  std::make_shared<stella_vslam::system>(config_set, vocab_file_path);
        m_frame_publisher = m_vslam->get_frame_publisher();
        m_map_publisher = m_vslam->get_map_publisher();
        m_vslam->startup(true);
    }; 

    void shutdown()
    {
        m_vslam->shutdown();
    };
    void reset(){
     m_vslam->request_reset();
    };
    std::pair<bool, Mat44_t> track(const cv::Mat& image,const double timestamp,const cv::Mat& Mask = cv::Mat{}){
        
        std::shared_ptr<stella_vslam::Mat44_t> T_w2c;
        T_w2c = m_vslam->feed_monocular_frame(image, timestamp);
        // m_mutex_last_drawn_frame.lock();
        // m_last_drawn_frame = m_frame_publisher->draw_frame();
        // m_mutex_last_drawn_frame.unlock();
        stella_vslam::tracker_state_t tracker_state ;
        std::string state_str = m_frame_publisher->get_tracking_state();
        if (state_str ==  "Tracking")
            tracker_state = stella_vslam::tracker_state_t::Tracking;
        else if (state_str =="Lost")
        {
            tracker_state = stella_vslam::tracker_state_t::Lost;
        }
        else if (state_str =="Initializing")
        {
            tracker_state = stella_vslam::tracker_state_t::Initializing;
        }
        else
            throw std::runtime_error("unknown state");


        if (tracker_state == stella_vslam::tracker_state_t::Tracking)
        {
            if (T_w2c == nullptr)
                return ptr2pose(T_w2c);
             //m_last_drawn_frame = m_frame_publisher->draw_frame();
            std::cout<<"current tracking state is Tracking"<<endl;
            std::vector<std::shared_ptr<stella_vslam::data::keyframe>>  keyframes;
            unsigned int current_nrof_keyframes = m_map_publisher->get_keyframes(keyframes);
            m_mutex_last_keyframe.lock();
            if (m_last_keyframe == nullptr)
                m_last_keyframe = keyframes.back().get();
            else
            {
                for (auto kf : keyframes)
                    if (kf->id_ > m_last_keyframe->id_)
                        m_last_keyframe = kf.get();
            }
            m_mutex_last_keyframe.unlock();

            m_sparse_cloud = getTrackedMapPoints();
            
            m_previous_state = tracker_state;
            if (m_nrof_keyframes == 0 && current_nrof_keyframes > 0){
                m_nrof_keyframes = current_nrof_keyframes;
                return ptr2pose(T_w2c);
            }
            else if (current_nrof_keyframes != m_nrof_keyframes)
            {
                m_last_keyframe->set_not_to_be_erased();
                m_nrof_keyframes = current_nrof_keyframes;
                return ptr2pose(T_w2c);
            }
            else{
                return ptr2pose(T_w2c);
            }
            }
        else if    ((m_previous_state == stella_vslam::tracker_state_t::Tracking || m_previous_state == stella_vslam::tracker_state_t::Lost) &&
            (tracker_state == stella_vslam::tracker_state_t::Initializing)){
                internalReset();
                //m_reset_callback();
                return ptr2pose(T_w2c);
            }     
        m_previous_state = tracker_state;
        return ptr2pose(T_w2c);

    };
    std::pair<std::vector<uint32_t>, cv::Mat> get_sparse_cloud(){
    return m_sparse_cloud;
    };
    std::pair<std::vector<uint32_t>, cv::Mat> getTrackedMapPoints(){
        m_mutex_last_keyframe.lock();
        vector<std::shared_ptr<stella_vslam::data::landmark>> landmarks = m_last_keyframe->get_landmarks();
        m_mutex_last_keyframe.unlock();
        std::vector<uint32_t> point_ids;
        cv::Mat points;
        points.reserve(landmarks.size());
        for (const auto &lm : landmarks)
        {
            if (!lm || lm->will_be_erased())
            {
            continue;
            }
            stella_vslam::Vec3_t pos = lm->get_pos_in_world();
            cv::Mat pt = (cv::Mat_<double>(1, 3) << pos[0], pos[1], pos[2]);
            points.push_back(pt);
            uint32_t point_id = extractPointId(lm.get());
            point_ids.push_back(point_id);
            if (point_id > m_max_point_id)
                m_max_point_id = point_id;
        }

        return std::pair<std::vector<uint32_t>,cv::Mat>(point_ids, points);
 
        
    };
    uint32_t extractPointId(stella_vslam::data::landmark* lm){
        return m_base_point_id + lm->id_;
    };
    std::pair<bool, Mat44_t> ptr2pose(std::shared_ptr<Mat44_t> pose_ptr)
    {
        if(pose_ptr == nullptr)
            // No pose
            return std::pair<bool, Mat44_t>(false, Mat44_t());
        else
            // Pose ok
            return std::pair<bool, Mat44_t>(true, (Mat44_t) *pose_ptr);
      };
    void internalReset()
    {
        std::lock_guard<std::mutex> lock(m_mutex_last_keyframe);
        m_last_keyframe = nullptr;
        m_nrof_keyframes = 0;
        // The new base point id is the maximum point id ever recognised +1. This way even though the SLAM starts counting
        // at 0 again, we still have a unique id for all the points.
        m_base_point_id = m_max_point_id+1;
        //m_keyframe_links.clear();
    };
private:
    uint32_t m_base_point_id;
    uint32_t m_max_point_id;
    unsigned int m_nrof_keyframes;
    cv::Mat m_last_drawn_frame;
    mutable std::mutex m_mutex_last_drawn_frame;
    mutable std::mutex m_mutex_last_keyframe;
    stella_vslam::data::keyframe* m_last_keyframe;
    stella_vslam::tracker_state_t m_previous_state;
    std::shared_ptr<stella_vslam::system> m_vslam;
    std::shared_ptr<stella_vslam::publish::frame_publisher> m_frame_publisher;
    std::shared_ptr<stella_vslam::publish::map_publisher> m_map_publisher;
    std::future<void> m_future_update_keyframes;
    std::pair<std::vector<uint32_t>, cv::Mat> m_sparse_cloud;
};

now I can output the tracked image pose, point cloud and keypoint correctly, but I don't know how to determine if the frame is a keyframe or not. I see in #

stella_vslam/src/stella_vslam/tracking_module.h

Line 30 in 0a1f13e

enum class tracker_state_t {

that there are only three tracking states, how do I determine if the frame is a keyframe?

[Skwangles]

I have a similar use case, I have been digging and it looks like there is no present way to do this other than monitoring the # of keyframes. The tracking process only adds the keyframe when needed, i.e.

  succeeded = track(relocalization_is_needed, num_tracked_lms, num_reliable_lms, min_num_obs_thr);

        // check to insert the new keyframe derived from the current frame
        if (succeeded && !is_stopped_keyframe_insertion_ && new_keyframe_is_needed(num_tracked_lms, num_reliable_lms, min_num_obs_thr)) {
            keyfrm_inserter_.insert_new_keyframe(map_db_, curr_frm_);
        }

If we were to provide some form of identifying if a keyframe was added, potentially a call back to a publisher, or editing the return value.