find-the-nearest-interval.cpp

find-the-nearest-interval.cpp

#include <iostream>
#include <algorithm>
#include <deque>

#include <stdint.h>

struct LocalPoseLite{
   uint64_t measurement_time; 
   
   LocalPoseLite(uint64_t t){
       measurement_time = t;
   }
   void set_measurement_time(uint64_t t){
       measurement_time = t;
   }
   
};

bool GetPose(uint64_t frame_id, uint64_t ref_time, LocalPoseLite& lpose, LocalPoseLite& rpose) {
  int64_t left_diff = INT64_MAX, right_diff = INT64_MIN;
  int queue_len = 0;
  uint64_t latest = 0;

  std::deque<LocalPoseLite> pose_queue_;
    
  LocalPoseLite data[]{ LocalPoseLite(196), LocalPoseLite(198), LocalPoseLite(200), LocalPoseLite(203), LocalPoseLite(204)};
  for (int i =0; i < 5; ++i)
        pose_queue_.emplace_back(std::move(data[i]));
        
  //---------------- find the nearest interval []--------------------
  {
    queue_len = pose_queue_.size();
    if (queue_len == 0) {
      std::cout  << "pose queue is empty! \n";
      return false;
    }

    latest = pose_queue_.back().measurement_time;

    auto it = std::find_if(
      pose_queue_.begin(), pose_queue_.end(), [ref_time](const auto& p) { return p.measurement_time >= ref_time; });
    
    bool all_left = false;
    if (it == pose_queue_.end()){
        lpose = ::std::move(*(it - 1));
        pose_queue_.erase(pose_queue_.begin(), it-1);
        all_left = true;
        
        std::cout << "all left \n";
        
        left_diff = lpose.measurement_time - ref_time;
        rpose.set_measurement_time(0);
    }
    
    else if (it != pose_queue_.begin()) {
      lpose = ::std::move(*(it - 1));
      left_diff = lpose.measurement_time - ref_time;
 
      pose_queue_.erase(pose_queue_.begin(), it);
      
      std::cout << "find left \n";
    } else {
      lpose.set_measurement_time(0);
    }

    if (!pose_queue_.empty() && !all_left) {
      rpose = ::std::move(pose_queue_.front());
      right_diff = rpose.measurement_time - ref_time;
    
      std::cout << "find right \n";

    } else {
      rpose.set_measurement_time(0);
    }
    
  }

  auto last_queue_len = pose_queue_.size();

  std::cout << "frame_id:" << frame_id << ", " << ((left_diff <= 0 && right_diff >= 0) ? "best" : "worse") << ", ["
           << left_diff<< ", " << right_diff  << "], dr_t:[" << lpose.measurement_time << ", "
           << rpose.measurement_time << "], camera_t:" << ref_time << ", latest_dr:" << latest << ", "
           << queue_len <<  ", " << last_queue_len << "\n\n";

  return true;
}

int main()
{
{
uint64_t ref_time = 193;

LocalPoseLite lpose(0), rpose(0);    

GetPose(1, ref_time, lpose, rpose) ;
}

//---------------------
{
uint64_t ref_time = 198;

LocalPoseLite lpose(0), rpose(0);    

GetPose(1, ref_time, lpose, rpose) ;
}

//---------------------
{
uint64_t ref_time = 204;

LocalPoseLite lpose(0), rpose(0);    

GetPose(1, ref_time, lpose, rpose) ;
}

//---------------------
{
uint64_t ref_time = 205;

LocalPoseLite lpose(0), rpose(0);    

GetPose(1, ref_time, lpose, rpose) ;
}

return 0;
}