#include "syscontroller.h"

#define NEAR_TRAJ_DIS 20
namespace juefx{

//----------------------------------------------------------------------------
SysController::SysController()
    :imuQueue(1000), wheelQueue(1000),
      gnssQueue(1000), poseQueue(1000)
{
    workTrd_.reset(new boost::thread(
                       boost::bind(&SysController::WorkTrdFunction, this)));
#ifndef TEST_ESKF
    State::Instance()->AddState(LOADING_MAP);
    backendTrd_.reset(new boost::thread(
                       boost::bind(&SysController::BackendTrdFunction, this)));

    std::ofstream ofs(Parameter::Instance()->GetBaseDir() + "/CloudPredictPose.txt", ios::out);
    ofs.close();
    ofs.open(Parameter::Instance()->GetBaseDir() + "/UndisortPose.txt", ios::out);
    ofs.close();
#else
    ReadMeasureLidarPose();
#endif
    std::ofstream ofs1(Parameter::Instance()->GetBaseDir() + "/MapPose.txt", ios::out);
    ofs1.close();
    CloudLocalize::Instance();
    positionToLoadQueue.push(Parameter::Instance()->GetInitPose().translation());
    backendCond_.notify_one();
}

//----------------------------------------------------------------------------
SysController::~SysController()
{
    requestToEnd_ = true;
    workCond_.notify_one();
    backendCond_.notify_one();
    workTrd_->join();
    workTrd_.reset();
    backendTrd_->join();
    backendTrd_.reset();
    Filter::Instance()->Reset();
    CloudLocalize::Instance()->Reset();
    ImuCaliber::Instance()->Reset();
    LOG(INFO) << "~SysController";
}

//----------------------------------------------------------------------------
void SysController::OnNewDataReceived(
        const SensorDataPtr &data)
{
    DCHECK(data);

    boost::unique_lock<boost::mutex> guard(netMtx_);

    if(CurrTimestamp < 0){
        CurrTimestamp = data->timestamp;
    }
    EnqueueData(data);

    if(!State::Instance()->HasState(FILTER_INIT)){
        workCond_.notify_one();
        return;
    }
    if(CLOUD_DATA == data->sensorDataType){
        CloudPacket *cloud = dynamic_cast<CloudPacket*>(data.get());
        if(cloud == nullptr){
            return;
        }
        double backPTime = cloud->timestamp;
        if(backPTime > CurrTimestamp){
            cloudQueue.push(*cloud);
            return;
        }
        Timer::Evaluate(
            [&, this]() {
            HandleCloud(*cloud);
        },
        "3 process and publish cloud");
        return;
    }
    if(data->timestamp < CurrTimestamp){
//        LOG(INFO) << setprecision(15) << "pre data: " << data->timestamp
//                  << " data->sensorDataType: " << data->sensorDataType;
        auto dataTime = data->timestamp - 1e-4;
        if(Filter::Instance()->SwitchBack(dataTime)){
            CurrTimestamp = dataTime;
            poseQueue.SwitchBack(CurrTimestamp);
            poseQueue.Shrink(CurrTimestamp);
        }
    }
    bool success = false;
    do{
        Timer::Evaluate(
            [&, this]() {
            Timer::Evaluate(
                [&, this]() {
                success = Measure();
            },
            "2.1 filter");
            if(success){
                GetPoseAndPublish();
            }
        },
        "2 filter and publish");
    }while(success);
    for(uint16_t i = 0; i < cloudQueue.size(); i++){
        CloudPacket cloud = cloudQueue.front();
        double backPTime = cloud.timestamp;
        if(backPTime < CurrTimestamp){
            Timer::Evaluate(
                [&, this]() {
                HandleCloud(cloud);
            },
            "3 process and publish cloud");
            cloudQueue.pop();
        }else{
            break;
        }
    }
}

//----------------------------------------------------------------------------
void SysController::WorkTrdFunction()
{
    const State::Ptr &state = State::Instance();
    const ImuCaliber::Ptr &imuCaliber = ImuCaliber::Instance();
    while(WaitForData(workMtx_, workCond_)){
        static auto t1 = std::chrono::high_resolution_clock::now();
        if(state->HasState(IMU_CALIBING)){
            IMUPacket imu;
            boost::unique_lock<boost::mutex> guard(netMtx_);
            while(imuQueue.getNext(imu)){
                bool success = false;
                Timer::Evaluate(
                    [&, this]() {
                    success = imuCaliber->CalculateImuPose(imu);
                    },
                    "5 calib imu");
                if(!success){
                    continue;
                }
                state->RemoveState(IMU_CALIBING);
                state->AddState(IMU_CALIBED);
                if(Filter::Instance()->Init()){
                    state->AddState(FILTER_INIT);
                    state->AddState(INIT_MATCHING);
                }
                CurrTimestamp = imu.timestamp;
                break;
            }
            continue;
        }
        if(state->HasState(MATCHING) /*&& state->HasState(MAP_LOADED)*/){
            double cloudTime = 0;
            CloudInfoForMatch cloudInfo;
            MatchResult matchResult;
            Isometry3f filterPoseToMeasure;
            bool shouldContinue = false;
            Timer::Evaluate(
                [&, this]() {
#ifndef TEST_ESKF
                shouldContinue = true;
                while(cloudInfoQueue.size() > 2){
                    static uint32_t skipCloudCnt = 0;
                    LOG(WARNING) << setprecision(15) << "skipCloudCnt! timestamp: "
                                 << cloudInfoQueue.front().timestamp
                                 << " skip Cnt: " << ++skipCloudCnt
                                 << " cloudInfoQueue.size(): " << cloudInfoQueue.size();
                    cloudInfoQueue.pop();
                }
                cloudInfo = cloudInfoQueue.front();
                cloudInfoQueue.pop();
                shouldContinue = false;
                if(shouldContinue){
                    return;
                }
                SetNewFilterFramePose(cloudInfo);
                cloudTime = cloudInfo.timestamp;
                Isometry3d mapPose;
                if(!CloudLocalize::Instance()->FindMapPose(cloudInfo.filterPoseInfo, mapPose)){
                    shouldContinue = true;
                    return;
                }
                PointCloudJ guessCloud;
                Matrix4f guessMapPose;
                guessMapPose.block(0, 0, 3, 3) = mapPose.linear().cast<float>();
                guessMapPose.block(0, 3, 3, 1) = mapPose.translation().cast<float>();
                pcl::transformPointCloud(*cloudInfo.frame, guessCloud, guessMapPose);
                pcl::io::savePCDFileBinary(Parameter::Instance()->GetBaseDir() + "/result/guessPcd/" +
                                           to_string(cloudTime) + ".pcd", guessCloud);
                Isometry3d futureFilterPose = GuessFutureFilterPose(cloudInfo.filterPoseInfo, 2);
                FilterPoseInfo futureFilterPoseInfo;
                futureFilterPoseInfo.filterPose = futureFilterPose;
                Isometry3d futureMapPose;
                if(CloudLocalize::Instance()->FindMapPose(futureFilterPoseInfo, futureMapPose, false)){
                    Vector3d diff = futureMapPose.translation() - mapPose.translation();
                    uint16_t positionDis = FutureDisReselution;
                    vector<Vector3d> positionToLoadVec;
                    while(positionDis < diff.norm()){
                        Vector3d positionToLoad = mapPose.translation() +
                                positionDis / diff.norm() * diff;
                        positionToLoadVec.emplace_back(positionToLoad);
                        positionDis += FutureDisReselution;
                    }
                    positionToLoadVec.emplace_back(futureMapPose.translation());
                    boost::unique_lock<boost::mutex> guard(backendMtx_);
                    for(const auto & pos : positionToLoadVec){
                        positionToLoadQueue.push(pos);
                    }
                    backendCond_.notify_one();
                }

                if(!CloudLocalize::Instance()->IsMapLoaded(mapPose.translation())){
                    shouldContinue = true;
                    static uint32_t skipCloudCntNoMap = 0;
                    LOG(WARNING) << setprecision(15) << "skipCloudCntNoMap! timestamp: "
                                 << cloudTime
                                 << " mapPose: " << mapPose.translation().transpose()
                                 << " skip Cnt: " << ++skipCloudCntNoMap;
                    return;
                }
                string process_name = "4.1 MatchMap at " + string((state->HasState(INIT_MATCHED) ? "INIT_MATCHED" : "INIT_MATCHING"));
                Timer::Evaluate(
                    [&, this]() {
                    GuessPose guessPose;
                    guessPose.map_point = mapPose;
                    auto increaseRpy = RotationQuaternionToEulerVector(
                                Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));
                    guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
                    guessPose.rotation = fabs(increaseRpy.z());
                    guessPose.use_gnss = cloudInfo.filterPoseInfo.hasGnss;
                    cloudInfo.frame->header.frame_id = to_string(cloudTime);

                    shouldContinue = !CloudLocalize::Instance()->MatchMap(      /////////////////// MatchMap
                                cloudInfo.frame, guessPose, matchResult);
                    },
                    process_name);

                if(shouldContinue){
                    return;
                }
                if(!state->HasState(FILTER_INIT) ||
                        !state->HasState(INIT_MATCHED)){
                    shouldContinue = true;
                    return;
                }
                static auto t2 = std::chrono::high_resolution_clock::now();
                static bool CurrIsFistMatch = true;
                if(CurrIsFistMatch){
                    auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;
                    LOG(INFO) << setprecision(15) << "INIT_MATCHING time_used: " << time_used;
                    CurrIsFistMatch = false;
                }

                filterPoseToMeasure =
                        Parameter::Instance()->GetFilter2Map().inverse().cast<float>() * matchResult.finalPose;
                Quaternionf resultQ(filterPoseToMeasure.linear());
                Vector3f rpy= RotationQuaternionToEulerVector(resultQ);
                std::ofstream ofs(Parameter::Instance()->GetBaseDir() + "/CloudPredictPose.txt", ios::app);
                ofs << setprecision(16) << cloudInfo.timestamp << ", " << filterPoseToMeasure.translation().x()
                    << ", " << filterPoseToMeasure.translation().y() << ", " << filterPoseToMeasure.translation().z()
                    << ", " << resultQ.w() << ", " << resultQ.x() << ", "
                    << resultQ.y() << ", " << resultQ.z() << ", " << rpy.x() << ", "
                    << rpy.y() << ", " << rpy.z() << ", " << matchResult.state << endl;
                ofs.close();

#else
                if(measureLidarQueue.size()){
                    matchResult = measureLidarQueue.front();
                    measureLidarQueue.pop();
                    filterPoseToMeasure = matchResult.finalPose;
                    cloudTime = matchResult.timestamp;
                }else{
                    shouldContinue = true;
                    return;
                }
#endif

            },
            "4 worktrd process cloud");
            if(shouldContinue){
                continue;
            }
            Timer::Evaluate(
                [&, this]() {
                boost::unique_lock<boost::mutex> guard(netMtx_);
                Filter::Instance()->MeasureLidar(filterPoseToMeasure, cloudTime, matchResult.state);
                LOG(INFO) << setprecision(15) << "MeasureLidar: " << cloudTime
                          << " translation: " << filterPoseToMeasure.translation().transpose()
                          << " rotation: " << RotationQuaternionToEulerVector(
                                 Quaternionf(filterPoseToMeasure.linear())).transpose();
                CurrTimestamp = cloudTime;
                poseQueue.SwitchBack(cloudTime);
                poseQueue.Shrink(cloudTime);
                while(Measure()){
                    GetPoseAndPublish();
                }
                },
                "4.3 switch back and remeasure");
        }
    }
}

//----------------------------------------------------------------------------
void SysController::BackendTrdFunction()
{
    bool isFst = true;
    while(WaitForData(backendMtx_, backendCond_)){
        while(positionToLoadQueue.size()){
            Vector3d position;
            {
                boost::unique_lock<boost::mutex> guard(backendMtx_);
                position = positionToLoadQueue.front();
                positionToLoadQueue.pop();
            }
            if(CloudLocalize::Instance()->IsMapLoaded(position)){
                continue;
            }
            LOG(INFO) << "Position to load: " << position.transpose();
            Timer::Evaluate(
                [&, this]() {
                CloudLocalize::Instance()->LoadArea(position);
                },
                "1 LoadBlocks");
        }
        if(isFst){
            if(Publisher::Instance()){
                Publisher::Instance()->load();
            }
            isFst = false;
        }
    }
}

//----------------------------------------------------------------------------
bool SysController::WaitForData(
        boost::mutex &mtx, boost::condition_variable &cond)
{
    if(requestToEnd_){
        return false;
    }
    boost::unique_lock<boost::mutex> lock(mtx);
    cond.wait(lock);
    return !requestToEnd_;
}

//----------------------------------------------------------------------------
void SysController::EnqueueData(const SensorDataPtr &data)
{
    if(IMU_DATA == data->sensorDataType){
        IMUPacket *imu = dynamic_cast<IMUPacket*>(data.get());
        if(imu == nullptr){
            return;
        }
        imuQueue.enque(*imu);
    }else if(WHEEL_DATA == data->sensorDataType){
        WheelPacket *wheel = dynamic_cast<WheelPacket*>(data.get());
        if(wheel == nullptr){
            return;
        }
        wheelQueue.enque(*wheel);
    }else if(GNSS_DATA == data->sensorDataType){
        GNSSPacket *gnss = dynamic_cast<GNSSPacket*>(data.get());
        if(gnss == nullptr){
            return;
        }
        gnssQueue.enque(*gnss);
        if(!State::Instance()->HasState(USING_GNSS)){
            State::Instance()->AddState(USING_GNSS);
        }
    }else if(IMU_POSE_DATA == data->sensorDataType){
        if(State::Instance()->HasState(FILTER_INIT)){
            return;
        }
        ImuPosePacket *gnss = dynamic_cast<ImuPosePacket*>(data.get());
        auto rpy = RotationQuaternionToEulerVector(gnss->rotation);
        Isometry3f eskf_init_pose = Isometry3f::Identity();
        eskf_init_pose.linear() = Matrix3f(AngleAxisf(rpy.y(), Vector3f::UnitY())
                                          * AngleAxisf(rpy.x(), Vector3f::UnitX()));
        Parameter::Instance()->SetEskfInitPose(eskf_init_pose);
        State::Instance()->RemoveState(IMU_CALIBING);
        State::Instance()->AddState(IMU_CALIBED);
        if(Filter::Instance()->Init()){
            State::Instance()->AddState(FILTER_INIT);
            State::Instance()->AddState(INIT_MATCHING);
            LOG(INFO) << "Filter Inited";
        }
    }
}

//----------------------------------------------------------------------------
void SysController::HandleImu(IMUPacket imu)
{
    Vector3f baseAngleVelo =
            Parameter::Instance()->GetImu2Base().linear().cast<float>() *
            imu.angular_velocity;
    Vector3f baseAngleAcc =
            Parameter::Instance()->GetImu2Base().linear().cast<float>() *
            imu.linear_acceleration;
    Filter::Instance()->UpdateImu(baseAngleAcc, baseAngleVelo, lTime(imu.timestamp));
}

//----------------------------------------------------------------------------
void SysController::HandleWheel(WheelPacket wheel)
{
    Vector3f speed(wheel.linear.x(), 0, 0);
    Filter::Instance()->MeasureWheel(speed, 0, lTime(wheel.timestamp));
}

//----------------------------------------------------------------------------
void SysController::HandleGnss(GNSSPacket gnss)
{
    static float lastGnssTime = -1;
    if(gnss.timestamp - lastGnssTime < 1){
        return;
    }
    vector<string> closeMapList = MapDetector::Instance()->GetCloseMap(gnss.blh, NEAR_TRAJ_DIS);
    if(closeMapCb_ && closeMapList.size()){
        closeMapCb_(closeMapList);
    }
    lastGnssTime = gnss.timestamp;
}

//----------------------------------------------------------------------------
void SysController::HandleCloud(CloudPacket cloud)
{
    if(!State::Instance()->HasState(MATCHING) /*|| !State::Instance()->HasState(MAP_LOADED)*/){
        return;
    }
#ifndef TEST_ESKF
    if(cloud.cloud.size() == 0){
        return;
    }
    static uint32_t frameCnt = 0;
    frameCnt++;
    double backPTime = cloud.timestamp;
    double frontPTime = cloud.timestamp + cloud.cloud.front().time;

    lidarInterval_ = (lidarInterval_ * (frameCnt - 1) + (backPTime - frontPTime)) / frameCnt;
//    LOG(INFO) << setprecision(15) << "frontPTime: " << frontPTime  << " backPTime: " << backPTime;
    vector<IsometryData> filterPeriodPose =
            GetPeriod<IsometryData>(poseQueue, frontPTime, backPTime);
    if(filterPeriodPose.size() < 3){
         LOG(INFO) << "filterPeriodPose.size(): " << filterPeriodPose.size();
        return;
    }
    for(size_t i = 0; i < filterPeriodPose.size(); i++){
        const IsometryData& curr = filterPeriodPose.at(i);
        Quaterniond resultQ(curr.pose.linear());
        Vector3d rpy= RotationQuaternionToEulerVector(resultQ);
        std::ofstream ofs(Parameter::Instance()->GetBaseDir() + "/UndisortPose.txt", ios::app);
        ofs << setprecision(15) << curr.timestamp << ", " << curr.pose.translation().x()
            << ", " << curr.pose.translation().y() << ", " << curr.pose.translation().z()
            << ", " << rpy.x() << ", " << rpy.y() << ", " << rpy.z() << endl;
        ofs.close();
    }
    PointCloudInter::Ptr frame =
            CloudPreprocess::Instance()->GetBaseFrame(cloud);
//    pcl::io::savePCDFileBinary(Parameter::Instance()->GetBaseDir() + "/result/raw_frame/" +
//                               to_string(cloud.timestamp) + ".pcd", *frame);
    Isometry3d framePose, increasePose;
    PointCloudJ::Ptr undisortedFrame(new PointCloudJ);
    PointCloudJ mappedFrame;
    bool success = false;
    Timer::Evaluate(
        [&, this]() {
        success = CloudPreprocess::Instance()->Undisort(
                        frame, undisortedFrame, &mappedFrame, backPTime, &filterPeriodPose, framePose, increasePose);
    },
    "3.1 undisort");
    if(!success){
        LOG(INFO) << "undisort fail ";
        return;
    }
    LOG(INFO) << setprecision(15) << "frontPTime: " << frontPTime
             << " backPTime: " << backPTime
             << " filterPeriodPose.front: " << filterPeriodPose.front().timestamp
             << " filterPeriodPose.back(): " << filterPeriodPose.back().timestamp
             << " framePose: " << framePose.translation().transpose()
             << " rotation: " << RotationQuaternionToEulerVector(
                     Quaterniond(framePose.linear())).transpose();

    CloudInfoForMatch cloudInfo;
    cloudInfo.frame = undisortedFrame;
    cloudInfo.filterPoseInfo.increasePose = increasePose;
    cloudInfo.timestamp = cloud.timestamp;
    cloudInfo.filterPoseInfo.filterPose = framePose;
    if(State::Instance()->HasState(TRAJ_HAS_GNSS)){
        cloudInfo.filterPoseInfo.hasGnss = PoseGuesser::InterpBlh(
                    &gnssQueue, cloud.timestamp, cloudInfo.filterPoseInfo.blh);
    }

    cloudInfoQueue.push(move(cloudInfo));
    workCond_.notify_one();
    pcl::io::savePCDFileBinary(Parameter::Instance()->GetBaseDir() + "/result/basePcd/" +
                               to_string(cloud.timestamp) + ".pcd", *undisortedFrame);
    if(State::Instance()->HasState(INIT_MATCHED)){
        PointCloudJ outputCloud;
        pcl::transformPointCloud(mappedFrame, outputCloud,
                                 Parameter::Instance()->GetFilter2Map().cast<float>());
//        pcl::io::savePCDFileBinary(Parameter::Instance()->GetBaseDir() + "/result/guessPcd/" +
//                                   to_string(cloud.timestamp) + ".pcd", outputCloud);
#if (defined ENABLE_ROS) || (defined ENABLE_BAG)
        if(Publisher::Instance()){
            Publisher::Instance()->PublishCloud(outputCloud);
        }
#endif
    }
#else
    workCond_.notify_one();
#endif
}

//----------------------------------------------------------------------------
bool SysController::Measure()
{
    IMUPacket imu;
    WheelPacket wheel;
    GNSSPacket gnss;
    bool imuPeek = imuQueue.peek(CurrTimestamp, imu);
    bool wheelPeek = wheelQueue.peek(CurrTimestamp, wheel);
    bool gnssPeek = gnssQueue.peek(CurrTimestamp, gnss);

#ifdef ENABLE_ROS
    bool peekSuccess = imuPeek && wheelPeek;
    if(false == peekSuccess){
        return false;
    }
#else
    if(!(imuPeek || wheelPeek)){
        return false;
    }
    if(!imuPeek){
        imu.timestamp = std::numeric_limits<double>::max();
    }
    if(!wheelPeek){
        wheel.timestamp = std::numeric_limits<double>::max();
    }
#endif
    CurrTimestamp = min(imu.timestamp, wheel.timestamp);
//    if(State::Instance()->HasState(USING_GNSS)){
//        CurrTimestamp = min(CurrTimestamp, gnss.timestamp);
//    }
//    LOG(INFO) << setprecision(15) << "CurrTimestamp: " << imu.timestamp;
    if(imu.timestamp == CurrTimestamp){
//        LOG(INFO) << setprecision(15) << "imu.timestamp: " << imu.timestamp;
        HandleImu(imu);
    }else if(wheel.timestamp == CurrTimestamp){
//        LOG(INFO) << setprecision(15) << "wheel.timestamp: " << wheel.timestamp;
        HandleWheel(wheel);
    }
    if(gnssPeek && gnss.timestamp > CurrTimestamp){
        HandleGnss(gnss);
    }
    return true;
}

//----------------------------------------------------------------------------
void SysController::GetPoseAndPublish()
{
    IsometryData pose;
    if(!Filter::Instance()->GetStateTransform(pose)){
        LOG(INFO) << "GetStateTransform fail: " << pose.timestamp;
        return;
    }
//    LOG(INFO) << setprecision(15) << "poseQueue.enque: " << pose.timestamp;
    poseQueue.enque(pose);
    if(State::Instance()->HasState(INIT_MATCHED)){
        auto F2M = Parameter::Instance()->GetFilter2Map();
        pose.pose.translation() = F2M.linear() * pose.pose.translation() + F2M.translation();
        pose.pose.linear() = F2M.linear() * pose.pose.linear() * F2M.inverse().linear();
#if (defined ENABLE_ROS) || (defined ENABLE_BAG)
        if(Publisher::Instance()){
            Publisher::Instance()->PublishPose(pose);
        }
#endif
    }
}

//----------------------------------------------------------------------------
Isometry3d SysController::GuessFutureFilterPose(
        const FilterPoseInfo &filterPoseInfo, float sec)
{
    float futureFrameCnt = sec / lidarInterval_;
    Isometry3d increasePose = filterPoseInfo.increasePose;
    increasePose.translation() *= futureFrameCnt;
    Isometry3d futureFilterPose = filterPoseInfo.filterPose * increasePose;
    return futureFilterPose;
}

//----------------------------------------------------------------------------
void SysController::ReadMeasureLidarPose()
{
    std::ifstream ifs(Parameter::Instance()->GetBaseDir() + "/CloudPredictPose.txt");
    if(ifs) {
        string line;
        while(getline(ifs, line))
        {
            MatchResult pose;
            Quaternionf q;
            int matchState;
            std::istringstream iss(line);
            string subline;
            getline(iss, subline, ',');
            std::istringstream(subline) >> pose.timestamp;
            getline(iss, subline, ',');
            std::istringstream(subline) >> pose.finalPose.translation().x();
            getline(iss, subline, ',');
            std::istringstream(subline) >> pose.finalPose.translation().y();
            getline(iss, subline, ',');
            std::istringstream(subline) >> pose.finalPose.translation().z();
            getline(iss, subline, ',');
            std::istringstream(subline) >> q.w();
            getline(iss, subline, ',');
            std::istringstream(subline) >> q.x();
            getline(iss, subline, ',');
            std::istringstream(subline) >> q.y();
            getline(iss, subline, ',');
            std::istringstream(subline) >> q.z();
            getline(iss, subline, ',');
            getline(iss, subline, ',');
            getline(iss, subline, ',');
            getline(iss, subline, ',');
            std::istringstream(subline) >> matchState;
            pose.state = (MatchPrecisionState)matchState;
            pose.finalPose.linear() = q.toRotationMatrix();
            measureLidarQueue.push(pose);
        }
        ifs.close();
    }
}

void SysController::SetNewFilterFramePose(CloudInfoForMatch &cloudInfo)
{
    vector<IsometryData> filterPeriodPose =
            GetPeriod<IsometryData>(poseQueue, cloudInfo.timestamp - 0.1, cloudInfo.timestamp + 0.05);

    for(size_t poseId = 1; poseId < filterPeriodPose.size(); poseId++){
        const IsometryData& pre = filterPeriodPose.at(poseId - 1);
        const IsometryData& pro = filterPeriodPose.at(poseId);
        if(cloudInfo.timestamp >= pre.timestamp &&
                          cloudInfo.timestamp < pro.timestamp){
            double deltTime = pro.timestamp - pre.timestamp;
            double timeDiff = cloudInfo.timestamp - pre.timestamp;
            double radio = timeDiff / deltTime;
            Isometry3d interpedMapPose;
            interpedMapPose.translation() = pre.pose.translation() + (pro.pose.translation() - pre.pose.translation()) * radio;
            interpedMapPose.linear() =
                    Quaterniond(pre.pose.linear()).slerp(radio, Quaterniond(pro.pose.linear())).toRotationMatrix();
            auto diff = cloudInfo.filterPoseInfo.filterPose.translation() - interpedMapPose.translation();
            LOG(INFO) << "norm: " << diff.norm();
            if(diff.norm() < 0.1){
                cloudInfo.filterPoseInfo.filterPose = interpedMapPose;
                LOG(INFO) << setprecision(15) << "SetNewFilterFramePose: " << cloudInfo.timestamp
                          << " pre.timestamp: " << pre.timestamp
                          << " pro.timestamp: " << pro.timestamp
                         << " interpedMapPose: " << interpedMapPose.translation().transpose()
                         << " rotation: " << RotationQuaternionToEulerVector(
                                 Quaterniond(interpedMapPose.linear())).transpose()
                         << " pre.pose: " << pre.pose.translation().transpose()
                         << " pre.pose rotation: " << RotationQuaternionToEulerVector(
                                 Quaterniond(pre.pose.linear())).transpose()
                         << " pro.pose: " << pro.pose.translation().transpose()
                         << " pro.pose rotation: " << RotationQuaternionToEulerVector(
                                 Quaterniond(pro.pose.linear())).transpose();
            }else{
                LOG(INFO) << setprecision(15) << "norm() >= 0.1: " << cloudInfo.timestamp;
            }
            return;
        }
    }
}

} // end of namespace