init

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8)
+
+project(multi_traj)
+
+set(CMAKE_BUILD_TYPE "Release")
+message(STATUS "CMAKE_BUILD_TYPE: " ${CMAKE_BUILD_TYPE})
+
+set(CMAKE_CXX_STANDARD 14)
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+
+include_directories(/opt/ros/melodic/include)
+link_directories(/opt/ros/melodic/lib)
+
+find_package(PCL REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system filesystem regex program_options thread)
+find_package(Glog REQUIRED)
+find_package(GFlags REQUIRED)
+find_package(PCAP REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS}
+    /opt/ros/melodic/lib)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/libs)
+add_subdirectory(apps)
+add_subdirectory(libs)
+
+
+
+

apps/CMakeLists.txt

+find_package(PCL REQUIRED)
+
+add_executable(multi_traj multi_traj.cpp
+    function/multi_traj_functions.cpp)
+
+target_include_directories(multi_traj
+    PUBLIC
+    ${PCL_INCLUDE_DIRS})
+
+
+target_link_libraries(multi_traj
+    trajectory
+    ${Boost_LIBRARIES}
+    ${GLOG_LIBRARIES}
+    ${GFLAGS_LIBRARIES})
+#/opt/ros/melodic/lib/libclass_loader.so

apps/cmake/catkinConfig.cmake

+# generated from catkin/cmake/catkinConfig.cmake.in
+# which overlays the default template catkin/cmake/template/pkgConfig.cmake.in
+#
+# :outvar catkin_INCLUDE_DIRS: contains the include dirs of all searched components.
+#    For use with CMake ``include_directories(${catkin_INCLUDE_DIRS})``.
+# :outvar catkin_LIBRARY_DIRS: contains the library dirs of all searched components.
+#    For use with CMake ``link_directories(${catkin_LIBRARY_DIRS})``.
+# :outvar catkin_LIBRARIES: contains the libraries of all searched components.
+#    For use with CMake ``target_link_libraries(<target> ${catkin_LIBRARIES})``.
+# :outvar <comp>_INCLUDE_DIRS/_LIBRARY_DIRS/_LIBRARIES:
+#    contains the include dirs / library dirs / libraries of the searched component <comp>.
+
+if(CATKIN_TOPLEVEL_FIND_PACKAGE OR NOT CATKIN_TOPLEVEL)
+  set(catkin_EXTRAS_DIR "${catkin_DIR}")
+
+  # prevent multiple inclusion from repeated find_package() calls in non-workspace context
+  # as long as this variable is in the scope the variables from all.cmake are also, so no need to be evaluated again
+  if(NOT DEFINED _CATKIN_CONFIG_ALL_INCLUDED_)
+    set(_CATKIN_CONFIG_ALL_INCLUDED_ TRUE)
+    include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  endif()
+endif()
+
+# skip setting find_package() variables when discovered from toplevel.cmake
+if(CATKIN_TOPLEVEL_FIND_PACKAGE)
+  return()
+endif()
+
+
+# prevent usage with wrong case
+if(CATKIN_FIND_COMPONENTS OR CATKIN_FIND_REQUIRED OR CATKIN_FIND_QUIETLY)
+  message(FATAL_ERROR "find_package() only supports lower-case package name 'catkin'")
+endif()
+
+# mark as found
+if(NOT catkin_FOUND)
+  set(catkin_FOUND)
+  set(CATKIN_PACKAGE_PREFIX "" CACHE STRING "Prefix to apply to package generated via gendebian")
+endif()
+
+# flag project as catkin-based to distinguish if a find_package()-ed project is a catkin project
+set(catkin_FOUND_CATKIN_PROJECT TRUE)
+
+# XXXX don't overwrite catkin_* variables when being called recursively
+if(NOT _CATKIN_FIND_ OR _CATKIN_FIND_ EQUAL 0)
+  set(_CATKIN_FIND_ 0)
+  if(catkin_FIND_COMPONENTS)
+    set(catkin_INCLUDE_DIRS "")
+    set(catkin_LIBRARIES "")
+    set(catkin_LIBRARY_DIRS "")
+    set(catkin_EXPORTED_TARGETS "")
+  endif()
+endif()
+
+# increment recursion counter
+math(EXPR _CATKIN_FIND_ "${_CATKIN_FIND_} + 1")
+
+# find all components
+if(catkin_FIND_COMPONENTS)
+  foreach(component ${catkin_FIND_COMPONENTS})
+    string(TOLOWER "${component}" component_lower)
+    # skip catkin since it does not make sense as a component
+    if(NOT ${component_lower} STREQUAL "catkin")
+
+      # get search paths from CMAKE_PREFIX_PATH (which includes devel space)
+      set(paths "")
+      foreach(path ${CMAKE_PREFIX_PATH})
+        if(IS_DIRECTORY ${path}/share/${component}/cmake)
+          list(APPEND paths ${path}/share/${component}/cmake)
+        endif()
+      endforeach()
+
+      # find package component
+      if(catkin_FIND_REQUIRED)
+        # try without REQUIRED first
+        find_package(${component} NO_MODULE PATHS ${paths}
+          NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+        if(NOT ${component}_FOUND)
+          # show better message to help users with the CMake error message coming up
+          message(STATUS "Could not find the required component '${component}'. "
+            "The following CMake error indicates that you either need to install the package "
+            "with the same name or change your environment so that it can be found.")
+          find_package(${component} REQUIRED NO_MODULE PATHS ${paths}
+            NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+        endif()
+      elseif(catkin_FIND_QUIETLY)
+        find_package(${component} QUIET NO_MODULE PATHS ${paths}
+          NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+      else()
+        find_package(${component} NO_MODULE PATHS ${paths}
+          NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+      endif()
+
+      # append component-specific variables to catkin_* variables
+      list_append_unique(catkin_INCLUDE_DIRS ${${component}_INCLUDE_DIRS})
+
+      # merge build configuration keywords with library names to correctly deduplicate
+      catkin_pack_libraries_with_build_configuration(catkin_LIBRARIES ${catkin_LIBRARIES})
+      catkin_pack_libraries_with_build_configuration(_libraries ${${component}_LIBRARIES})
+      list_append_deduplicate(catkin_LIBRARIES ${_libraries})
+      # undo build configuration keyword merging after deduplication
+      catkin_unpack_libraries_with_build_configuration(catkin_LIBRARIES ${catkin_LIBRARIES})
+
+      list_append_unique(catkin_LIBRARY_DIRS ${${component}_LIBRARY_DIRS})
+      list(APPEND catkin_EXPORTED_TARGETS ${${component}_EXPORTED_TARGETS})
+    endif()
+  endforeach()
+  list_insert_in_workspace_order(catkin_INCLUDE_DIRS ${catkin_INCLUDE_DIRS})
+  list_insert_in_workspace_order(catkin_LIBRARY_DIRS ${catkin_LIBRARY_DIRS})
+endif()
+
+# add dummy target to catkin_EXPORTED_TARGETS if empty
+if(NOT catkin_EXPORTED_TARGETS)
+  if(NOT TARGET _catkin_empty_exported_target)
+    add_custom_target(_catkin_empty_exported_target)
+  endif()
+  list(APPEND catkin_EXPORTED_TARGETS _catkin_empty_exported_target)
+endif()
+
+# decrement recursion counter
+math(EXPR _CATKIN_FIND_ "${_CATKIN_FIND_} - 1")
+
+if(_CATKIN_FIND_ EQUAL 0)
+  # store found components (from the fist level only) for validation in catkin_package() that they are build dependencies
+  list(APPEND catkin_ALL_FOUND_COMPONENTS ${catkin_FIND_COMPONENTS})
+endif()

apps/function/multi_traj_functions.cpp

+#include "multi_traj_functions.h"
+using namespace boost::filesystem;
+
+vector<string> getCloseTasks(
+        const string &parentPath,
+        boost::shared_ptr<Trajectory> currTraj)
+{
+    vector<string> closeTasks;
+    boost::shared_ptr<LocalCartesian> proj = currTraj->getProj();
+    directory_iterator task_iter(parentPath);
+    directory_iterator task_end_iter;
+    for(; task_iter != task_end_iter; task_iter++) {
+        if (!is_directory(task_iter->path())){
+            continue;
+        }
+        string rawTraceDir = task_iter->path().string() + "/raw_trace";
+        if (!is_directory(rawTraceDir)){
+            continue;
+        }
+        string iePath = rawTraceDir + "/ie.txt";
+        if (!exists(iePath)){
+            continue;
+        }
+        TrajPoint trajPoint;
+        getFstTraj(iePath, trajPoint, proj);
+        if(trajPoint.translation.norm() > 30000){
+            continue;
+        }
+        closeTasks.push_back(task_iter->path().string());
+    }
+    return closeTasks;
+}
+
+bool getFstTraj(const string &iePath,
+                TrajPoint& trajPoint,
+                boost::shared_ptr<LocalCartesian> proj){
+    std::ifstream ifs(iePath);
+    if(!ifs){
+        LOG(WARNING) << iePath << " load fail!";
+        return false;
+    }
+    string line;
+    getline(ifs, line);
+    getline(ifs, line);
+    ifs.close();
+    vector<string> line_vec;
+    boost::split(line_vec, line, boost::is_any_of(","), boost::token_compress_on);
+    PPK_Raw_Info ppk_raw_info;
+    ppk_raw_info.gps_week = stoi(line_vec[1]);
+    ppk_raw_info.gps_sec = stod(line_vec[2]);
+    ppk_raw_info.lat = stod(line_vec[3]);
+    ppk_raw_info.lng = stod(line_vec[4]);
+    ppk_raw_info.height = stod(line_vec[5]);
+    ppk_raw_info.roll = stod(line_vec[9]);
+    ppk_raw_info.pitch = stod(line_vec[10]);
+    ppk_raw_info.yaw = -stod(line_vec[11]); //ppk yaw take minus!!!!!!!!!!!!!!!!!!!!
+    Vector3d translation;
+    proj->Forward(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height,
+                   translation.x(),
+                   translation.y(),
+                   translation.z());
+    trajPoint.translation = translation;
+    trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw));
+    return true;
+}

apps/function/multi_traj_functions.h

+#ifndef MULTI_TRAJ_FUNCTIONS_H
+#define MULTI_TRAJ_FUNCTIONS_H
+
+#include "utils/basic_types.h"
+#include "trajectory/trajectory.h"
+
+vector<string> getCloseTasks(
+        const string &parentPath,
+        boost::shared_ptr<Trajectory> currTraj);
+
+bool getFstTraj(const string &iePath,
+                TrajPoint& trajPoint,
+                boost::shared_ptr<LocalCartesian> proj);
+
+#endif // MULTI_TRAJ_FUNCTIONS_H

apps/multi_traj.cpp

+#include <gflags/gflags.h>
+#include "trajectory/trajectory.h"
+#include "function/multi_traj_functions.h"
+
+DEFINE_string(base_dir, "", "base_dir");
+
+boost::shared_ptr<Trajectory> ppk_traj_;
+boost::shared_ptr<Trajectory> node_traj_;
+
+void signaleHandle(const char* data, int size){
+    string str = data;
+    LOG(INFO) << "signaleHandle: " << str;
+}
+
+int main(int argc, char *argv[])
+{
+    gflags::ParseCommandLineFlags(&argc, &argv, true);
+    google::InitGoogleLogging(argv[0]);
+    google::InstallFailureSignalHandler();
+    google::InstallFailureWriter(&signaleHandle);
+    FLAGS_log_dir = "/home/juefx/log";
+    FLAGS_alsologtostderr = true;
+    FLAGS_colorlogtostderr = true;
+    FLAGS_stderrthreshold = 0;
+    FLAGS_logbufsecs = 0;
+
+    string ppk_dir_ = FLAGS_base_dir + "/raw_trace/ie.txt";
+
+    ppk_traj_.reset(new Trajectory(ppk_dir_, PPK));
+    ppk_traj_->init();
+    LOG(INFO) << "Trajectory load done!";
+
+    boost::filesystem::path base_dir(FLAGS_base_dir);
+    string parentPath = base_dir.parent_path().string();
+    LOG(INFO) << "parentPath: " << parentPath;
+
+    vector<string> closeTasks = getCloseTasks(parentPath, ppk_traj_);
+    for(string task : closeTasks){
+        LOG(INFO) << "close task chekc in: " << task;
+    }
+
+    string outputPath = FLAGS_base_dir + "/slam/cartographer.bag.pbstream.interpolated.pose";
+
+    std::ofstream ofs(outputPath);
+    if(!ofs){
+        LOG(WARNING) << outputPath << " load fail!";
+        return 0;
+    }
+
+    ofs.close();
+    return 1;
+}
+

cmake/FindGFlags.cmake

+# - Try to find GFLAGS
+#
+# The following variables are optionally searched for defaults
+#  GFLAGS_ROOT_DIR:            Base directory where all GFLAGS components are found
+#
+# The following are set after configuration is done:
+#  GFLAGS_FOUND
+#  GFLAGS_INCLUDE_DIRS
+#  GFLAGS_LIBRARIES
+#  GFLAGS_LIBRARYRARY_DIRS
+
+include(FindPackageHandleStandardArgs)
+
+set(GFLAGS_ROOT_DIR "" CACHE PATH "Folder contains Gflags")
+
+# We are testing only a couple of files in the include directories
+if(WIN32)
+    find_path(GFLAGS_INCLUDE_DIR gflags/gflags.h
+        PATHS ${GFLAGS_ROOT_DIR}/src/windows)
+else()
+    find_path(GFLAGS_INCLUDE_DIR gflags/gflags.h
+        PATHS ${GFLAGS_ROOT_DIR})
+endif()
+
+if(MSVC)
+    find_library(GFLAGS_LIBRARY_RELEASE
+        NAMES libgflags
+        PATHS ${GFLAGS_ROOT_DIR}
+        PATH_SUFFIXES Release)
+
+    find_library(GFLAGS_LIBRARY_DEBUG
+        NAMES libgflags-debug
+        PATHS ${GFLAGS_ROOT_DIR}
+        PATH_SUFFIXES Debug)
+
+    set(GFLAGS_LIBRARY optimized ${GFLAGS_LIBRARY_RELEASE} debug ${GFLAGS_LIBRARY_DEBUG})
+else()
+    find_library(GFLAGS_LIBRARY gflags)
+endif()
+
+find_package_handle_standard_args(GFLAGS DEFAULT_MSG
+    GFLAGS_INCLUDE_DIR GFLAGS_LIBRARY)
+
+
+if(GFLAGS_FOUND)
+    set(GFLAGS_INCLUDE_DIRS ${GFLAGS_INCLUDE_DIR})
+    set(GFLAGS_LIBRARIES ${GFLAGS_LIBRARY})
+endif()

cmake/FindGeographicLib.cmake

+# Look for GeographicLib
+#
+# Set
+#  GeographicLib_FOUND = GEOGRAPHICLIB_FOUND = TRUE
+#  GeographicLib_INCLUDE_DIRS = /usr/local/include
+#  GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so
+#  GeographicLib_LIBRARY_DIRS = /usr/local/lib
+
+find_library (GeographicLib_LIBRARIES Geographic
+  PATHS "${CMAKE_INSTALL_PREFIX}/../GeographicLib/lib")
+
+if (GeographicLib_LIBRARIES)
+  get_filename_component (GeographicLib_LIBRARY_DIRS
+    "${GeographicLib_LIBRARIES}" PATH)
+  get_filename_component (_ROOT_DIR "${GeographicLib_LIBRARY_DIRS}" PATH)
+  set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include")
+  set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin")
+  if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h")
+    # On Debian systems the library is in e.g.,
+    #   /usr/lib/x86_64-linux-gnu/libGeographic.so
+    # so try stripping another element off _ROOT_DIR
+    get_filename_component (_ROOT_DIR "${_ROOT_DIR}" PATH)
+    set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include")
+    set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin")
+    if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h")
+      unset (GeographicLib_INCLUDE_DIRS)
+      unset (GeographicLib_LIBRARIES)
+      unset (GeographicLib_LIBRARY_DIRS)
+      unset (GeographicLib_BINARY_DIRS)
+    endif ()
+  endif ()
+  unset (_ROOT_DIR)
+endif ()
+
+include (FindPackageHandleStandardArgs)
+find_package_handle_standard_args (GeographicLib DEFAULT_MSG
+  GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES
+  GeographicLib_INCLUDE_DIRS)
+mark_as_advanced (GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES
+  GeographicLib_INCLUDE_DIRS)

cmake/FindGlog.cmake

+# - Try to find Glog
+#
+# The following variables are optionally searched for defaults
+#  GLOG_ROOT_DIR:            Base directory where all GLOG components are found
+#
+# The following are set after configuration is done:
+#  GLOG_FOUND
+#  GLOG_INCLUDE_DIRS
+#  GLOG_LIBRARIES
+#  GLOG_LIBRARYRARY_DIRS
+
+include(FindPackageHandleStandardArgs)
+
+set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog")
+
+if(WIN32)
+    find_path(GLOG_INCLUDE_DIR glog/logging.h
+        PATHS ${GLOG_ROOT_DIR}/src/windows)
+else()
+    find_path(GLOG_INCLUDE_DIR glog/logging.h
+        PATHS ${GLOG_ROOT_DIR})
+endif()
+
+if(MSVC)
+    find_library(GLOG_LIBRARY_RELEASE libglog_static
+        PATHS ${GLOG_ROOT_DIR}
+        PATH_SUFFIXES Release)
+
+    find_library(GLOG_LIBRARY_DEBUG libglog_static
+        PATHS ${GLOG_ROOT_DIR}
+        PATH_SUFFIXES Debug)
+
+    set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG})
+else()
+    find_library(GLOG_LIBRARY glog
+        PATHS ${GLOG_ROOT_DIR}
+        PATH_SUFFIXES
+            lib
+            lib64)
+endif()
+
+find_package_handle_standard_args(GLOG DEFAULT_MSG
+    GLOG_INCLUDE_DIR GLOG_LIBRARY)
+
+if(GLOG_FOUND)
+    set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
+    set(GLOG_LIBRARIES ${GLOG_LIBRARY})
+endif()

cmake/FindPCAP.cmake

+# - Try to find libpcap include dirs and libraries
+#
+# Usage of this module as follows:
+#
+#     find_package(PCAP)
+#
+# Variables used by this module, they can change the default behaviour and need
+# to be set before calling find_package:
+#
+#  PCAP_ROOT_DIR             Set this variable to the root installation of
+#                            libpcap if the module has problems finding the
+#                            proper installation path.
+#
+# Variables defined by this module:
+#
+#  PCAP_FOUND                System has libpcap, include and library dirs found
+#  PCAP_INCLUDE_DIR          The libpcap include directories.
+#  PCAP_LIBRARY              The libpcap library (possibly includes a thread
+#                            library e.g. required by pf_ring's libpcap)
+#  HAVE_PF_RING              If a found version of libpcap supports PF_RING
+
+find_path(PCAP_ROOT_DIR
+    NAMES include/pcap.h
+)
+
+find_path(PCAP_INCLUDE_DIR
+    NAMES pcap.h
+    HINTS ${PCAP_ROOT_DIR}/include
+)
+
+find_library(PCAP_LIBRARY
+    NAMES pcap
+    HINTS ${PCAP_ROOT_DIR}/lib
+)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(PCAP DEFAULT_MSG
+    PCAP_LIBRARY
+    PCAP_INCLUDE_DIR
+)
+
+include(CheckCSourceCompiles)
+set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY})
+check_c_source_compiles("int main() { return 0; }" PCAP_LINKS_SOLO)
+set(CMAKE_REQUIRED_LIBRARIES)
+
+# check if linking against libpcap also needs to link against a thread library
+if (NOT PCAP_LINKS_SOLO)
+    find_package(Threads)
+    if (THREADS_FOUND)
+        set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY} ${CMAKE_THREAD_LIBS_INIT})
+        check_c_source_compiles("int main() { return 0; }" PCAP_NEEDS_THREADS)
+        set(CMAKE_REQUIRED_LIBRARIES)
+    endif ()
+    if (THREADS_FOUND AND PCAP_NEEDS_THREADS)
+        set(_tmp ${PCAP_LIBRARY} ${CMAKE_THREAD_LIBS_INIT})
+        list(REMOVE_DUPLICATES _tmp)
+        set(PCAP_LIBRARY ${_tmp}
+            CACHE STRING "Libraries needed to link against libpcap" FORCE)
+    else ()
+        message(FATAL_ERROR "Couldn't determine how to link against libpcap")
+    endif ()
+endif ()
+
+include(CheckFunctionExists)
+set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARY})
+check_function_exists(pcap_get_pfring_id HAVE_PF_RING)
+set(CMAKE_REQUIRED_LIBRARIES)
+
+mark_as_advanced(
+    PCAP_ROOT_DIR
+    PCAP_INCLUDE_DIR
+    PCAP_LIBRARY
+)

cmake/FindTBB.cmake

+# The MIT License (MIT)
+#
+# Copyright (c) 2015 Justus Calvin
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+#
+# FindTBB
+# -------
+#
+# Find TBB include directories and libraries.
+#
+# Usage:
+#
+#  find_package(TBB [major[.minor]] [EXACT]
+#               [QUIET] [REQUIRED]
+#               [[COMPONENTS] [components...]]
+#               [OPTIONAL_COMPONENTS components...])
+#
+# where the allowed components are tbbmalloc and tbb_preview. Users may modify
+# the behavior of this module with the following variables:
+#
+# * TBB_ROOT_DIR          - The base directory the of TBB installation.
+# * TBB_INCLUDE_DIR       - The directory that contains the TBB headers files.
+# * TBB_LIBRARY           - The directory that contains the TBB library files.
+# * TBB_<library>_LIBRARY - The path of the TBB the corresponding TBB library.
+#                           These libraries, if specified, override the
+#                           corresponding library search results, where <library>
+#                           may be tbb, tbb_debug, tbbmalloc, tbbmalloc_debug,
+#                           tbb_preview, or tbb_preview_debug.
+# * TBB_USE_DEBUG_BUILD   - The debug version of tbb libraries, if present, will
+#                           be used instead of the release version.
+#
+# Users may modify the behavior of this module with the following environment
+# variables:
+#
+# * TBB_INSTALL_DIR
+# * TBBROOT
+# * LIBRARY_PATH
+#
+# This module will set the following variables:
+#
+# * TBB_FOUND             - Set to false, or undefined, if we haven’t found, or
+#                           don’t want to use TBB.
+# * TBB_<component>_FOUND - If False, optional <component> part of TBB sytem is
+#                           not available.
+# * TBB_VERSION           - The full version string
+# * TBB_VERSION_MAJOR     - The major version
+# * TBB_VERSION_MINOR     - The minor version
+# * TBB_INTERFACE_VERSION - The interface version number defined in
+#                           tbb/tbb_stddef.h.
+# * TBB_<library>_LIBRARY_RELEASE - The path of the TBB release version of
+#                           <library>, where <library> may be tbb, tbb_debug,
+#                           tbbmalloc, tbbmalloc_debug, tbb_preview, or
+#                           tbb_preview_debug.
+# * TBB_<library>_LIBRARY_DEGUG - The path of the TBB release version of
+#                           <library>, where <library> may be tbb, tbb_debug,
+#                           tbbmalloc, tbbmalloc_debug, tbb_preview, or
+#                           tbb_preview_debug.
+#
+# The following varibles should be used to build and link with TBB:
+#
+# * TBB_INCLUDE_DIRS        - The include directory for TBB.
+# * TBB_LIBRARIES           - The libraries to link against to use TBB.
+# * TBB_LIBRARIES_RELEASE   - The release libraries to link against to use TBB.
+# * TBB_LIBRARIES_DEBUG     - The debug libraries to link against to use TBB.
+# * TBB_DEFINITIONS         - Definitions to use when compiling code that uses
+#                             TBB.
+# * TBB_DEFINITIONS_RELEASE - Definitions to use when compiling release code that
+#                             uses TBB.
+# * TBB_DEFINITIONS_DEBUG   - Definitions to use when compiling debug code that
+#                             uses TBB.
+#
+# This module will also create the "tbb" target that may be used when building
+# executables and libraries.
+
+include(FindPackageHandleStandardArgs)
+
+if(NOT TBB_FOUND)
+
+  ##################################
+  # Check the build type
+  ##################################
+
+  if(NOT DEFINED TBB_USE_DEBUG_BUILD)
+    if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)")
+      set(TBB_BUILD_TYPE DEBUG)
+    else()
+      set(TBB_BUILD_TYPE RELEASE)
+    endif()
+  elseif(TBB_USE_DEBUG_BUILD)
+    set(TBB_BUILD_TYPE DEBUG)
+  else()
+    set(TBB_BUILD_TYPE RELEASE)
+  endif()
+
+  ##################################
+  # Set the TBB search directories
+  ##################################
+
+  # Define search paths based on user input and environment variables
+  set(TBB_SEARCH_DIR ${TBB_ROOT_DIR} $ENV{TBB_INSTALL_DIR} $ENV{TBBROOT})
+
+  # Define the search directories based on the current platform
+  if(CMAKE_SYSTEM_NAME STREQUAL "Windows")
+    set(TBB_DEFAULT_SEARCH_DIR "C:/Program Files/Intel/TBB"
+                               "C:/Program Files (x86)/Intel/TBB")
+
+    # Set the target architecture
+    if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+      set(TBB_ARCHITECTURE "intel64")
+    else()
+      set(TBB_ARCHITECTURE "ia32")
+    endif()
+
+    # Set the TBB search library path search suffix based on the version of VC
+    if(WINDOWS_STORE)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11_ui")
+    elseif(MSVC14)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc14")
+    elseif(MSVC12)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc12")
+    elseif(MSVC11)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11")
+    elseif(MSVC10)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc10")
+    endif()
+
+    # Add the library path search suffix for the VC independent version of TBB
+    list(APPEND TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc_mt")
+
+  elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
+    # OS X
+    set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
+
+    # TODO: Check to see which C++ library is being used by the compiler.
+    if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0)
+      # The default C++ library on OS X 10.9 and later is libc++
+      set(TBB_LIB_PATH_SUFFIX "lib/libc++" "lib")
+    else()
+      set(TBB_LIB_PATH_SUFFIX "lib")
+    endif()
+  elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux")
+    # Linux
+    set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
+
+    # TODO: Check compiler version to see the suffix should be <arch>/gcc4.1 or
+    #       <arch>/gcc4.1. For now, assume that the compiler is more recent than
+    #       gcc 4.4.x or later.
+    if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")
+      set(TBB_LIB_PATH_SUFFIX "lib/intel64/gcc4.4")
+    elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^i.86$")
+      set(TBB_LIB_PATH_SUFFIX "lib/ia32/gcc4.4")
+    endif()
+  endif()
+
+  ##################################
+  # Find the TBB include dir
+  ##################################
+
+  find_path(TBB_INCLUDE_DIRS tbb/tbb.h
+      HINTS ${TBB_INCLUDE_DIR} ${TBB_SEARCH_DIR}
+      PATHS ${TBB_DEFAULT_SEARCH_DIR}
+      PATH_SUFFIXES include)
+
+  ##################################
+  # Set version strings
+  ##################################
+
+  if(TBB_INCLUDE_DIRS)
+    file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file)
+    string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
+        TBB_VERSION_MAJOR "${_tbb_version_file}")
+    string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
+        TBB_VERSION_MINOR "${_tbb_version_file}")
+    string(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
+        TBB_INTERFACE_VERSION "${_tbb_version_file}")
+    set(TBB_VERSION "${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR}")
+  endif()
+
+  ##################################
+  # Find TBB components
+  ##################################
+
+  if(TBB_VERSION VERSION_LESS 4.3)
+    set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc tbb)
+  else()
+    set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc_proxy tbbmalloc tbb)
+  endif()
+
+  # Find each component
+  foreach(_comp ${TBB_SEARCH_COMPOMPONENTS})
+    if(";${TBB_FIND_COMPONENTS};tbb;" MATCHES ";${_comp};")
+
+      # Search for the libraries
+      find_library(TBB_${_comp}_LIBRARY_RELEASE ${_comp}
+          HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
+          PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
+          PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
+
+      find_library(TBB_${_comp}_LIBRARY_DEBUG ${_comp}_debug
+          HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
+          PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
+          PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
+
+      if(TBB_${_comp}_LIBRARY_DEBUG)
+        list(APPEND TBB_LIBRARIES_DEBUG "${TBB_${_comp}_LIBRARY_DEBUG}")
+      endif()
+      if(TBB_${_comp}_LIBRARY_RELEASE)
+        list(APPEND TBB_LIBRARIES_RELEASE "${TBB_${_comp}_LIBRARY_RELEASE}")
+      endif()
+      if(TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE} AND NOT TBB_${_comp}_LIBRARY)
+        set(TBB_${_comp}_LIBRARY "${TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE}}")
+      endif()
+
+      if(TBB_${_comp}_LIBRARY AND EXISTS "${TBB_${_comp}_LIBRARY}")
+        set(TBB_${_comp}_FOUND TRUE)
+      else()
+        set(TBB_${_comp}_FOUND FALSE)
+      endif()
+
+      # Mark internal variables as advanced
+      mark_as_advanced(TBB_${_comp}_LIBRARY_RELEASE)
+      mark_as_advanced(TBB_${_comp}_LIBRARY_DEBUG)
+      mark_as_advanced(TBB_${_comp}_LIBRARY)
+
+    endif()
+  endforeach()
+
+  ##################################
+  # Set compile flags and libraries
+  ##################################
+
+  set(TBB_DEFINITIONS_RELEASE "")
+  set(TBB_DEFINITIONS_DEBUG "-DTBB_USE_DEBUG=1")
+
+  if(TBB_LIBRARIES_${TBB_BUILD_TYPE})
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_${TBB_BUILD_TYPE}}")
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_${TBB_BUILD_TYPE}}")
+  elseif(TBB_LIBRARIES_RELEASE)
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_RELEASE}")
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_RELEASE}")
+  elseif(TBB_LIBRARIES_DEBUG)
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}")
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_DEBUG}")
+  endif()
+
+  find_package_handle_standard_args(TBB
+      REQUIRED_VARS TBB_INCLUDE_DIRS TBB_LIBRARIES
+      HANDLE_COMPONENTS
+      VERSION_VAR TBB_VERSION)
+
+  ##################################
+  # Create targets
+  ##################################
+
+  if(NOT CMAKE_VERSION VERSION_LESS 3.0 AND TBB_FOUND)
+    # Start fix to support different targets for tbb, tbbmalloc, etc.
+    # (Jose Luis Blanco, Jan 2019)
+    # Iterate over tbb, tbbmalloc, etc.
+    foreach(libname ${TBB_SEARCH_COMPOMPONENTS})
+      if ((NOT TBB_${libname}_LIBRARY_RELEASE) AND (NOT TBB_${libname}_LIBRARY_DEBUG))
+        continue()
+      endif()
+
+      add_library(${libname} SHARED IMPORTED)
+
+      set_target_properties(${libname} PROPERTIES
+            INTERFACE_INCLUDE_DIRECTORIES  ${TBB_INCLUDE_DIRS}
+            IMPORTED_LOCATION              ${TBB_${libname}_LIBRARY_RELEASE})
+      if(TBB_${libname}_LIBRARY_RELEASE AND TBB_${libname}_LIBRARY_DEBUG)
+        set_target_properties(${libname} PROPERTIES
+            INTERFACE_COMPILE_DEFINITIONS "$<$<OR:$<CONFIG:Debug>,$<CONFIG:RelWithDebInfo>>:TBB_USE_DEBUG=1>"
+            IMPORTED_LOCATION_DEBUG          ${TBB_${libname}_LIBRARY_DEBUG}
+            IMPORTED_LOCATION_RELWITHDEBINFO ${TBB_${libname}_LIBRARY_DEBUG}
+            IMPORTED_LOCATION_RELEASE        ${TBB_${libname}_LIBRARY_RELEASE}
+            IMPORTED_LOCATION_MINSIZEREL     ${TBB_${libname}_LIBRARY_RELEASE}
+            )
+      elseif(TBB_${libname}_LIBRARY_RELEASE)
+        set_target_properties(${libname} PROPERTIES IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
+      else()
+        set_target_properties(${libname} PROPERTIES
+            INTERFACE_COMPILE_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}"
+            IMPORTED_LOCATION              ${TBB_${libname}_LIBRARY_DEBUG}
+            )
+      endif()
+    endforeach()
+    # End of fix to support different targets
+  endif()
+
+  mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARIES)
+
+  unset(TBB_ARCHITECTURE)
+  unset(TBB_BUILD_TYPE)
+  unset(TBB_LIB_PATH_SUFFIX)
+  unset(TBB_DEFAULT_SEARCH_DIR)
+
+endif()

libs/CMakeLists.txt

+add_subdirectory(io)
+add_subdirectory(trajectory)
+add_subdirectory(utils)
+add_subdirectory(transform)
+

libs/io/CMakeLists.txt

+add_library(io STATIC textio.cpp)
+
+target_link_libraries(io
+    utils
+    ${Boost_LIBRARIES}
+    ${PCAP_LIBRARY})

libs/io/PcapFileReader.h

+#ifndef __PacketFileReader_h
+#define __PacketFileReader_h
+
+#include <pcap.h>
+#include <string>
+#include <iostream>
+
+// Some versions of libpcap do not have PCAP_NETMASK_UNKNOWN
+#if !defined(PCAP_NETMASK_UNKNOWN)
+#define PCAP_NETMASK_UNKNOWN 0xffffffff
+#endif
+
+class PcapFileReader
+{
+public:
+  PcapFileReader() { this->PCAPFile = 0; }
+
+  ~PcapFileReader() { this->Close(); }
+
+  // This function is called to read a savefile .pcap
+  // 1-Open a savefile in the tcpdump/libcap format to read packet
+  // 2-A packet filter is then compile to convert an high level filtering
+  //  expression in a program that can be interpreted by the kernel-level filtering engine
+  // 3- The compiled filter is then associate to the capture
+  bool Open(const std::string& filename)
+  {
+    char errbuff[PCAP_ERRBUF_SIZE];
+    pcap_t* pcapFile = pcap_open_offline(filename.c_str(), errbuff);
+    if (!pcapFile)
+    {
+      this->LastError = errbuff;
+      return false;
+    }
+
+    struct bpf_program filter;
+
+    if (pcap_compile(pcapFile, &filter, "udp", 0, PCAP_NETMASK_UNKNOWN) == -1)
+    {
+      this->LastError = pcap_geterr(pcapFile);
+      return false;
+    }
+
+    if (pcap_setfilter(pcapFile, &filter) == -1)
+    {
+      this->LastError = pcap_geterr(pcapFile);
+      return false;
+    }
+
+    this->FileName = filename;
+    this->PCAPFile = pcapFile;
+    this->StartTime.tv_sec = this->StartTime.tv_usec = 0;
+    return true;
+  }
+
+  bool IsOpen() { return (this->PCAPFile != 0); }
+
+  void Close()
+  {
+    if (this->PCAPFile)
+    {
+      pcap_close(this->PCAPFile);
+      this->PCAPFile = 0;
+      this->FileName.clear();
+    }
+  }
+
+  const std::string& GetLastError() { return this->LastError; }
+
+  const std::string& GetFileName() { return this->FileName; }
+
+  void GetFilePosition(fpos_t* position)
+  {
+#ifdef _MSC_VER
+    pcap_fgetpos(this->PCAPFile, position);
+#else
+    FILE* f = pcap_file(this->PCAPFile);
+    fgetpos(f, position);
+#endif
+  }
+
+  void SetFilePosition(fpos_t* position)
+  {
+#ifdef _MSC_VER
+    pcap_fsetpos(this->PCAPFile, position);
+#else
+    FILE* f = pcap_file(this->PCAPFile);
+    fsetpos(f, position);
+#endif
+  }
+
+  bool NextPacket(const unsigned char*& data, unsigned int& dataLength, double& timeSinceStart,
+    pcap_pkthdr** headerReference = NULL)
+  {
+    if (!this->PCAPFile)
+    {
+      return false;
+    }
+
+    struct pcap_pkthdr* header;
+    int returnValue = pcap_next_ex(this->PCAPFile, &header, &data);
+    if (returnValue < 0)
+    {
+      this->Close();
+      return false;
+    }
+
+    if (headerReference != NULL)
+    {
+      *headerReference = header;
+      dataLength = header->len;
+      timeSinceStart = GetElapsedTime(header->ts, this->StartTime);
+      return true;
+    }
+
+    // The ethernet header is 42 bytes long; unnecessary
+    const unsigned int bytesToSkip = 42;
+    dataLength = header->len - bytesToSkip;
+    data = data + bytesToSkip;
+    timeSinceStart = GetElapsedTime(header->ts, this->StartTime);
+    return true;
+  }
+
+protected:
+  double GetElapsedTime(const struct timeval& end, const struct timeval& start)
+  {
+    return (end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) / 1000000.00;
+  }
+
+  pcap_t* PCAPFile;
+  std::string FileName;
+  std::string LastError;
+  struct timeval StartTime;
+};
+
+#endif

libs/io/textio.cpp

+#include "textio.h"
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
+
+TextIO::TextIO(const string &path, const string &range)
+    :path_(path),
+      range_(range)
+{
+
+}
+
+bool TextIO::load()
+{
+    std::ifstream ifs(path_);
+    if(!ifs){
+        LOG(WARNING) << path_ << " load fail!";
+        return false;
+    }
+    if(!parse_cb_){
+        LOG(WARNING) << "boost::function parse_ is null!";
+        return false;
+    }
+    string line;
+    getline(ifs, line);
+    vector<string> lines;
+    while(getline(ifs, line)) {
+        lines.push_back(line);
+    }
+    ifs.close();
+    multiThreadParseText(lines);
+    return true;
+}
+
+void TextIO::multiThreadParseText(const vector<string> &lines)
+{
+    vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(lines.size());
+    vector<ThreadPtr> thread_vec;
+    for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
+
+        thread_vec.emplace_back(ThreadPtr(new boost::thread(boost::bind(&TextIO::parseTextForPeriod,
+                                                                        this,
+                                                                        &lines,
+                                                                        indexPeriod[thread_index],
+                                                                        thread_index))));
+    }
+    for(uint8_t thread_index = 0; thread_index < thread_vec.size(); thread_index++){
+        thread_vec[thread_index]->join();
+        thread_vec[thread_index].reset();
+    }
+}
+
+void TextIO::parseTextForPeriod(const vector<string> *lines,
+                                pair<uint32_t, uint32_t> duration,
+                                uint8_t trdIndex)
+{
+    uint32_t start_index = duration.first;
+    uint32_t index_cnt = duration.second;
+    uint32_t final_index = start_index + index_cnt;
+    for(uint32_t index = start_index; index < final_index; index++){
+        vector<string> line_vec;
+        boost::split(line_vec, lines->at(index), boost::is_any_of(range_), boost::token_compress_on);
+        parse_cb_(line_vec, trdIndex);
+    }
+}
+
+void TextIO::setParseCallback(
+        boost::function<void (const vector<string> &, uint8_t)> cb)
+{
+    parse_cb_ = cb;
+}
+
+string TextIO::getPath()
+{
+    return path_;
+}

libs/io/textio.h

+#ifndef TEXTIO_H
+#define TEXTIO_H
+#include "utils/basic_functions.h"
+#include <boost/function.hpp>
+
+class TextIO
+{
+public:
+    TextIO(const string &path, const string &range = string());
+
+    bool load();
+
+    void setParseCallback(
+            boost::function<void(const vector<string>&, uint8_t)> cb);
+
+    string getPath();
+private:
+    void multiThreadParseText(const vector<string> &lines);
+
+    void parseTextForPeriod(
+            const vector<string> *lines,
+            pair<uint32_t, uint32_t> duration,
+            uint8_t trdIndex);
+private:
+    string path_;
+
+    string range_;
+
+    boost::function<void(const vector<string>&, uint8_t)> parse_cb_;
+};
+
+#endif // TEXTIO_H

libs/trajectory/CMakeLists.txt

+find_package(GeographicLib REQUIRED)
+
+add_library(trajectory STATIC trajectory.cpp)
+
+target_include_directories(trajectory
+        PUBLIC
+        ${GeographicLib_INCLUDE_DIRS})
+
+target_link_libraries(trajectory
+    io
+    transform
+    ${GeographicLib_LIBRARIES})

libs/trajectory/trajectory.cpp

+#include "trajectory.h"
+#include <boost/bind.hpp>
+
+uint16_t MAX_REVERSE_CNT = 1;
+uint16_t MIN_PPK_LINE_ITEM_CNT = 12;
+uint16_t MIN_NODE_LINE_ITEM_CNT = 10;
+
+Trajectory::Trajectory(const string &traj_path, const TrajType &trajType)
+    :trajectory_path_(traj_path),
+      trajType_(trajType)
+{
+
+}
+
+Trajectory::~Trajectory()
+{
+    if(io_) delete io_;
+}
+
+bool Trajectory::init()
+{
+    string range;
+    switch(trajType_){
+    case PPK:
+        frequency_ = 1000;
+        range = ",";
+        break;
+    case NODE:
+        frequency_ = 10;
+        range = " ";
+        break;
+    }
+    LOG(INFO) << "Trajectory set frequency: " << frequency_;
+    io_ = new TextIO(trajectory_path_, range);
+    boost::function<void(const vector<string>&, uint8_t)> parse_callback =
+            boost::bind(&Trajectory::parseAndPush, this, _1, _2);
+    io_->setParseCallback(parse_callback);
+
+    if(!io_){
+        LOG(WARNING) << "io_ is nullptr!";
+        return false;
+    }
+    if(!io_->load()){
+        LOG(WARNING) << "io_ load fail!";
+        return false;
+    }
+    for(auto trajectory : multiTrajectory_){
+        trajectory_.insert(trajectory_.end(),
+                           trajectory.begin(),
+                           trajectory.end());
+    }
+    sort(trajectory_.begin(), trajectory_.end(),
+         [](const TrajPoint& query, const TrajPoint& target) {
+             return query.timestamp < target.timestamp;
+         });
+
+    LOG(INFO) << "trajectory_ size: " << trajectory_.size();
+    return true;
+}
+
+string Trajectory::getTrajPath() const
+{
+    return trajectory_path_;
+}
+
+TrajType Trajectory::getTrajType() const
+{
+    return trajType_;
+}
+
+int32_t Trajectory::findFloorTrajPoint(const double &timestamp) const
+{
+    if(trajectory_.size() < 2){
+        return -1;
+    }
+    int32_t current_index = 0;
+    int32_t supposed_index = 0;
+    uint16_t find_cnt = 0;
+    double current_timestamp = trajectory_.front().timestamp;
+    uint16_t reverse_cnt = 0;
+    double last_time_diff = 0;
+    int cnt = 0;
+
+    if(trajectory_[0].timestamp > timestamp){
+//        LOG_EVERY_N(INFO, 9999999) << setprecision(15)
+//                                << "trajectory_[0].timestamp: " << trajectory_[0].timestamp
+//                                << " > timestamp: " << timestamp;
+        return -1;
+    }
+    do{
+//        if(find_cnt >= MAX_FIND_CNT){
+//            LOG_EVERY_N(INFO, 9999) << setprecision(15) << "findFloorTrajPoint find " << timestamp
+//                      << " fail " << find_cnt << " over " << MAX_FIND_CNT << " times!";
+//            return -2;
+//        }
+
+        current_index = supposed_index;
+
+
+        if((uint32_t)current_index >= trajectory_.size()){
+            current_index = trajectory_.size() - 1;
+        }
+        current_timestamp = trajectory_[current_index].timestamp;
+        double time_diff = timestamp - current_timestamp;
+        int32_t index_diff = 0;
+        if(time_diff < 0){
+            index_diff = floor(time_diff * frequency_);
+        }else if(time_diff > 0){
+            index_diff = ceil(time_diff * frequency_);
+        }
+        supposed_index = current_index + index_diff;
+//        LOG(INFO) << setprecision(15)
+//                  << "current_index: " << current_index
+//                  << " index_diff: " << index_diff
+//                  << " supposed_index: " << supposed_index;
+
+//        if(trajType_ == NODE){
+//            LOG(INFO) << setprecision(15)
+//                      << "current_index: " << current_index
+//                      << " index_diff: " << index_diff
+//                      << " supposed_index: " << supposed_index
+//                      << " current_timestamp: " << current_timestamp
+//                      << " timestamp: " << timestamp;
+//        }
+
+        if(last_time_diff * time_diff < 0){
+            reverse_cnt++;
+        }
+        if(reverse_cnt > MAX_REVERSE_CNT){
+
+//        if(find(begin(index_history), end(index_history), current_index) != end(index_history)
+//                || find_cnt >= MAX_REVERSE_CNT){
+//            LOG(INFO) << "reverse_cnt > MAX_REVERSE_CNT";
+            uint32_t start_index, end_index;
+            if(current_index < supposed_index){
+                start_index = current_index;
+                end_index = supposed_index;
+            }else{
+                start_index = supposed_index;
+                end_index = current_index;
+            }
+            int32_t index = traversalFindTrajPoint(start_index, end_index, timestamp);
+
+            return index;
+        }
+        last_time_diff = time_diff;
+        cnt++;
+        if(supposed_index < 0 || cnt > 20){
+//            LOG(INFO) << "findFloorTrajPoint supposed_index < 0";
+            return traversalFindTrajPoint(0, trajectory_.size() - 1, timestamp);;
+        }
+        find_cnt++;
+    }while(!checkSatisfied(supposed_index, timestamp));
+
+    return supposed_index;
+}
+
+int32_t Trajectory::traversalFindTrajPoint(const uint32_t &start_index,
+                                           const uint32_t &end_index,
+                                           const double &timestamp) const
+{
+    for(uint32_t index = start_index; index < end_index; index++){
+        if(checkSatisfied(index, timestamp)){
+            return index;
+        }
+    }
+    return -1;
+}
+
+bool Trajectory::interpolation(const double &timestamp,
+                               const int32_t &floor_index,
+                               TrajPoint &res_traj) const
+{
+    int32_t upper_index = floor_index + 1;
+    if((uint32_t)upper_index >= trajectory_.size()){
+        return false;
+    }
+
+    const TrajPoint &traj_floor = trajectory_[floor_index];
+    const TrajPoint &traj_upper = trajectory_[upper_index];
+
+    double radio = (timestamp - traj_floor.timestamp) / (traj_upper.timestamp - traj_floor.timestamp);
+
+    res_traj = traj_floor;
+    res_traj.translation = traj_floor.translation + radio * (traj_upper.translation - traj_floor.translation);
+    res_traj.rotation = traj_floor.rotation.slerp(radio, traj_upper.rotation);
+    res_traj.timestamp = timestamp;
+//    if(timestamp >= 1620921381.682 &&
+//            timestamp <= 1620921381.683){
+//        Vector3d llh;
+//        proj_->Reverse(res_traj.translation.x(), res_traj.translation.y(), res_traj.translation.z(),
+//                       llh.x(), llh.y(), llh.z());
+//        LOG(INFO) << setprecision(15) << "res_traj: "
+//                  << llh.transpose();
+//    }
+    return true;
+}
+
+Matrix4d Trajectory::getTransformFromTrajPoint(const TrajPoint &trajPoint)
+{
+    Matrix4d T;
+    T.block(0, 0, 3, 3) = trajPoint.rotation.toRotationMatrix();
+    T.block(0, 3, 3, 1) = trajPoint.translation;
+    return T;
+}
+
+vector<vector<TrajPoint>> Trajectory::getDurationsTrajs(const vector<Time_Duration> &durations) const
+{
+    vector<vector<TrajPoint>> ret;
+    for(const auto &duration : durations){
+        vector<TrajPoint> durationsTrajs;
+        int32_t traj_index = findFloorTrajPoint(duration.start_sec);
+        if(traj_index < 0){
+            LOG(INFO) << setprecision(15)
+                      << "getDurationsTrajs findFloorTrajPoint fail, error code = " << traj_index
+                      << " duration.start_sec: " << duration.start_sec;
+            traj_index = 0;
+        }
+        for(uint32_t index = traj_index; index < trajectory_.size(); index++){
+            auto &traj = trajectory_.at(index);
+            if(traj.timestamp >= duration.start_sec
+                    && traj.timestamp <= duration.end_sec){
+                durationsTrajs.push_back(traj);
+            }else{
+                continue;
+            }
+        }
+        if(durationsTrajs.size() > 1){
+            ret.push_back(durationsTrajs);
+        }
+    }
+    return ret;
+}
+
+TrajPoint Trajectory::getTrajPointByIndex(const uint32_t &index) const
+{
+    if(index < trajectory_.size()){
+        return trajectory_.at(index);
+    }else{
+        return TrajPoint();
+    }
+}
+
+TrajPoint Trajectory::getTrajPointByNodeId(const uint32_t &nodeId) const
+{
+    for(const TrajPoint &point : trajectory_){
+        if(point.nodeId == nodeId){
+            return point;
+        }
+    }
+    return TrajPoint();
+}
+
+vector<TrajPoint> Trajectory::getTrajectory() const
+{
+    return trajectory_;
+}
+
+boost::shared_ptr<LocalCartesian> Trajectory::getProj() const
+{
+    return proj_;
+}
+
+double Trajectory::getFrequency()
+{
+    return frequency_;
+}
+
+bool Trajectory::checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const
+{
+    if(supposed_floor_index < 0){
+        return false;
+    }
+    if((uint32_t)supposed_floor_index >= trajectory_.size() - 2){
+        return false;
+    }
+    if(trajectory_[supposed_floor_index].timestamp <= timestamp &&
+            trajectory_[supposed_floor_index + 1].timestamp > timestamp &&
+            (trajectory_[supposed_floor_index + 1].timestamp - trajectory_[supposed_floor_index].timestamp) < 3 / frequency_){
+        return true;
+    }else{
+        return false;
+    }
+}
+
+double gpsToUtc(const uint32_t &gps_week, const double &gps_sec) {
+    return (315936000 + gps_week*7*24*3600 + gps_sec - 18);
+}
+
+void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
+{
+    switch(trajType_){
+    case PPK:
+    {
+        if(vec.size() < MIN_PPK_LINE_ITEM_CNT){
+            LOG_EVERY_N(WARNING, 99999)
+                    << io_->getPath() << " line item size < " << MIN_PPK_LINE_ITEM_CNT;
+            return;
+        }
+        PPK_Raw_Info ppk_raw_info;
+        ppk_raw_info.gps_week = stoi(vec[1]);
+        ppk_raw_info.gps_sec = stod(vec[2]);
+        ppk_raw_info.lat = stod(vec[3]);
+        ppk_raw_info.lng = stod(vec[4]);
+        ppk_raw_info.height = stod(vec[5]);
+        ppk_raw_info.roll = stod(vec[9]);
+        ppk_raw_info.pitch = stod(vec[10]);
+        ppk_raw_info.yaw = -stod(vec[11]); //ppk yaw take minus!!!!!!!!!!!!!!!!!!!!
+
+        {
+            if(!proj_){
+                boost::unique_lock<boost::mutex> lock(Mtx_);
+                if(!proj_){
+                    proj_.reset(new LocalCartesian(ppk_raw_info.lat,
+                                                   ppk_raw_info.lng,
+                                                   ppk_raw_info.height));
+                    LOG(INFO) << setprecision(15) << "LocalCartesian center: "
+                              << Vector3d(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height).transpose();
+                }
+            }
+        }
+
+        TrajPoint trajPoint;
+        Vector3d translation;
+        proj_->Forward(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height,
+                       translation.x(),
+                       translation.y(),
+                       translation.z());
+        trajPoint.translation = translation;
+        trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw));
+        trajPoint.nodeId = trajectory_.size();
+        trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
+        if((index + 1) >= multiTrajectory_.size()){
+            boost::unique_lock<boost::mutex> lock_ppk(Mtx_);
+            multiTrajectory_.resize(index + 1);
+        }
+        if(multiTrajectory_[index].size() == 0){
+            multiTrajectory_[index].push_back(trajPoint);
+        }else{
+            if((trajPoint.translation - multiTrajectory_[index].begin()->translation).norm() > 1){
+                multiTrajectory_[index].push_back(trajPoint);
+            }
+        }
+
+        break;
+    }
+    case NODE:
+    {
+        if(vec.size() < MIN_NODE_LINE_ITEM_CNT){
+            LOG_EVERY_N(WARNING, 99999)
+                    << io_->getPath() << " line item size < " << MIN_NODE_LINE_ITEM_CNT;
+            return;
+        }
+
+        NODE_Raw_Info node_raw_info;
+        node_raw_info.node_id = stoi(vec[1]);
+        node_raw_info.time = stod(vec[2]);
+        node_raw_info.x = stod(vec[3]);
+        node_raw_info.y = stod(vec[4]);
+        node_raw_info.z = stod(vec[5]);
+        node_raw_info.w = stod(vec[6]);
+        node_raw_info.qx = stod(vec[7]);
+        node_raw_info.qy = stod(vec[8]);
+        node_raw_info.qz = stod(vec[9]);
+
+//        if(node_raw_info.node_id > 73168 && node_raw_info.node_id < 73504){
+//            return;
+//        }
+
+//        bool skip = (node_raw_info.node_id > 10907 && node_raw_info.node_id < 13417) ||
+//                (node_raw_info.node_id > 86905 && node_raw_info.node_id < 89253) ||
+//                (node_raw_info.node_id > 3076 && node_raw_info.node_id < 5752) ;
+
+//        skip = !skip;
+
+//        if(skip){
+//            return;
+//        }
+
+        TrajPoint trajPoint;
+        trajPoint.nodeId = node_raw_info.node_id;
+        trajPoint.timestamp = node_raw_info.time;
+        trajPoint.translation = {node_raw_info.x, node_raw_info.y, node_raw_info.z};
+        trajPoint.rotation = {node_raw_info.w, node_raw_info.qx, node_raw_info.qy, node_raw_info.qz};
+        //pose yaw take -M_PI/2!!!!!!!!!!!!!!!!!!
+        trajPoint.rotation = trajPoint.rotation * Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ());
+        boost::unique_lock<boost::mutex> lock_node(Mtx_);
+        trajectory_.push_back(trajPoint);
+
+        break;
+    }
+    }
+}
+

libs/trajectory/trajectory.h

+#ifndef TRAJECTORY_H
+#define TRAJECTORY_H
+#include "transform/transform.h"
+#include "io/textio.h"
+#include <GeographicLib/LocalCartesian.hpp>
+
+using namespace GeographicLib;
+
+class Trajectory
+{
+public:
+    Trajectory(const string &traj_path, const TrajType &trajType);
+
+    ~Trajectory();
+
+    bool init();
+
+    string getTrajPath() const;
+
+    TrajType getTrajType() const;
+
+    int32_t findFloorTrajPoint(const double &timestamp) const;
+
+    int32_t traversalFindTrajPoint(const uint32_t& start_index,
+                                   const uint32_t& end_index,
+                                   const double &timestamp) const;
+
+    bool interpolation(const double &timestamp,
+                       const int32_t &floor_index,
+                       TrajPoint &res_traj) const;
+
+    static Matrix4d getTransformFromTrajPoint(const TrajPoint &trajPoint);
+
+    vector<vector<TrajPoint> > getDurationsTrajs(const vector<Time_Duration> &durations) const;
+
+    TrajPoint getTrajPointByIndex(const uint32_t &index) const;
+
+    TrajPoint getTrajPointByNodeId(const uint32_t &nodeId) const;
+
+    vector<TrajPoint> getTrajectory() const;
+
+    boost::shared_ptr<LocalCartesian> getProj() const;
+
+    double getFrequency();
+private:
+    bool checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const;
+
+    void parseAndPush(const vector<string> &vec, uint8_t index);
+private:
+    vector<TrajPoint> trajectory_;
+
+    vector<vector<TrajPoint>> multiTrajectory_;
+
+    const string trajectory_path_;
+
+    const TrajType trajType_;
+
+    double frequency_;
+
+    TextIO* io_ = nullptr;
+
+    boost::shared_ptr<LocalCartesian> proj_;
+
+    boost::mutex Mtx_;
+};
+
+#endif // TRAJECTORY_H

libs/transform/CMakeLists.txt

+add_library(transform STATIC transform.cpp)
+

libs/transform/transform.cpp

+#include "transform.h"
+
+Matrix3d EulerToMatrix3d(const Vector3d &euler, const uint16_t fst,
+                         const uint16_t sec, const uint16_t trd)
+{
+    if(fst > 2 || sec > 2 || trd > 2){
+        return Matrix3d();
+    }
+    Matrix3d rotation;
+    AngleAxisd angle[3];
+    angle[0] = AngleAxisd(euler[0]/180*M_PI, Vector3d::UnitX());
+    angle[1] = AngleAxisd(euler[1]/180*M_PI, Vector3d::UnitY());
+    angle[2] = AngleAxisd(euler[2]/180*M_PI, Vector3d::UnitZ());
+    rotation = angle[trd] * angle[sec] * angle[fst];
+    return rotation;
+}
+
+Isometry3d Matrix4dToIsometry3d(const Matrix4d &matrix)
+{
+    Matrix3d rotation = matrix.block(0, 0, 3, 3);
+    Vector3d translation = matrix.block(0, 3, 3, 1);
+    Isometry3d isometry;
+    isometry.linear() = rotation;
+    isometry.translation() = translation;
+    return isometry;
+}
+
+Isometry3d QuaternionTransToIsometry3d(
+        const Quaterniond &rotation,
+        const Vector3d &translation)
+{
+    Isometry3d isometry;
+    isometry.linear() = rotation.toRotationMatrix();
+    isometry.translation() = translation;
+    return isometry;
+}
+
+Matrix4d Isometry3dToMatrix4d(const Isometry3d &isometry)
+{
+    Matrix4d matrix;
+    matrix.block(0, 0, 3, 3) = isometry.linear();
+    matrix.block(0, 3, 3, 1) = isometry.translation();
+    return matrix;
+}

libs/transform/transform.h

+#ifndef TRANSFORM_H
+#define TRANSFORM_H
+#include "utils/basic_types.h"
+using namespace  Eigen;
+
+Matrix3d EulerToMatrix3d(const Vector3d &euler, const uint16_t fst = 1,
+                         const uint16_t sec = 0, const uint16_t trd = 2);
+
+Isometry3d Matrix4dToIsometry3d(const Matrix4d &matrix);
+
+Isometry3d QuaternionTransToIsometry3d(
+        const Quaterniond &rotation,
+        const Vector3d &translation);
+
+Matrix4d Isometry3dToMatrix4d(const Isometry3d &isometry);
+
+#endif // TRANSFORM_H

libs/utils/CMakeLists.txt

+add_library(utils STATIC basic_functions.cpp)
+

libs/utils/basic_functions.cpp

+#include "basic_functions.h"
+
+int8_t checkTime(const double &time, const Time_Duration &timeDuration){
+    if(time < timeDuration.start_sec){
+        return -1;
+    }else if(time < timeDuration.end_sec){
+        return 1;
+    }else {
+        return -2;
+    }
+}
+
+int8_t checkTime(const double &time, const vector<Time_Duration> &timeDurations){
+    bool has_before = false, has_after = false;
+    for(const auto &durations : timeDurations){
+        int8_t ret = checkTime(time, durations);
+        if(ret == -1){
+            has_before = true;
+        }else if(ret == -2){
+            has_after = true;
+        }else if(ret == 1){
+            return 1;
+        }
+    }
+    if(has_after && !has_before){
+        return -2;
+    }else if(has_before && !has_after){
+        return -1;
+    }
+}
+
+vector<pair<uint32_t, uint32_t>> getIndexPeriod(const uint32_t size,
+                                                const uint8_t threadCnt)
+{
+    vector<pair<uint32_t, uint32_t>> ret;
+    uint32_t whole_frame_cnt = size;
+    uint32_t min_frame_cnt = whole_frame_cnt / threadCnt;
+    uint32_t max_frame_cnt = min_frame_cnt + 1;
+    uint8_t max_divide_cnt = whole_frame_cnt % threadCnt;
+    uint32_t start_index = 0, last_item_cnt = 0;
+    for(uint8_t thread_index = 0; thread_index < threadCnt; thread_index++){
+        uint32_t item_cnt;
+        if(thread_index < max_divide_cnt){
+            item_cnt = max_frame_cnt;
+        }else{
+            item_cnt = min_frame_cnt;
+        }
+        start_index += last_item_cnt;
+        ret.push_back(make_pair(start_index, item_cnt));
+        last_item_cnt = item_cnt;
+    }
+    return ret;
+}

libs/utils/basic_functions.h

+#ifndef BASIC_FUNCTIONS_H
+#define BASIC_FUNCTIONS_H
+
+#include "basic_types.h"
+#include <chrono>
+using namespace chrono;
+
+int8_t checkTime(const double &time, const Time_Duration &timeDuration);
+
+int8_t checkTime(const double &time, const vector<Time_Duration> &timeDurations);
+
+vector<pair<uint32_t, uint32_t>> getIndexPeriod(const uint32_t size,
+                                                const uint8_t threadCnt = std::thread::hardware_concurrency());
+
+template <typename T>
+Eigen::Matrix<T, 3, 1> RotationQuaternionToEulerVector(
+    const Eigen::Quaternion<T>& quaternion) {
+  Eigen::Quaternion<T> n_q = quaternion.normalized();
+  // We choose the quaternion with positive 'w', i.e., the one with a smaller
+  // angle that represents this orientation.
+  if (n_q.w() < 0.) {
+    // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
+    n_q.w() = -1. * n_q.w();
+    n_q.x() = -1. * n_q.x();
+    n_q.y() = -1. * n_q.y();
+    n_q.z() = -1. * n_q.z();
+  }
+  // We convert the normalized_quaternion(n_q) into a vector along the rotation
+  // axis with length of the rotation angle.
+  const T roll = atan2(2 * (n_q.w() * n_q.x() + n_q.y() * n_q.z()),
+                       1 - 2 * (n_q.x() * n_q.x() + n_q.y() * n_q.y()));
+  const T pitch = asin(2 * (n_q.w() * n_q.y() - n_q.z() * n_q.x()));
+  const T yaw = atan2(2 * (n_q.w() * n_q.z() + n_q.x() * n_q.y()),
+                      1 - 2 * (n_q.y() * n_q.y() + n_q.z() * n_q.z()));
+  return Eigen::Matrix<T, 3, 1>(roll, pitch, yaw);
+}
+
+template<typename ItemType>
+void periodProcessThreadFunc(vector<ItemType> *Vector,
+                             pair<uint32_t, uint32_t> period,
+                             boost::function<void(ItemType &)> processItemFunc)
+{
+    vector<ItemType> tempOutputVec;
+    uint32_t start_index = period.first;
+    uint32_t index_cnt = period.second;
+    uint32_t final_index = start_index + index_cnt;
+    for(uint32_t index = start_index; index < final_index; index++){
+        processItemFunc(Vector->at(index));
+    }
+}
+
+template<typename ItemType>
+void multiThreadProcess(vector<ItemType> &Vector,
+                        boost::function<void(ItemType &)> processItemFunc)
+{
+    auto start = system_clock::now();
+    vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(Vector.size());
+    vector<ThreadPtr> threadVec;
+    boost::mutex mutex;
+    for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
+        threadVec.emplace_back(
+                    ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFunc<ItemType>,
+                                                            &Vector,
+                                                            indexPeriod[thread_index],
+                                                            processItemFunc))));
+    }
+    for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
+        threadVec[thread_index]->join();
+        threadVec[thread_index].reset();
+
+    }
+    auto end   = system_clock::now();
+    auto duration = duration_cast<milliseconds>(end - start);
+    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds,"
+              << " output size: " << Vector.size();
+}
+
+template<typename ItemType>
+void periodProcessThreadFunc(const vector<ItemType> *inputVector,
+                             vector<ItemType> *outputVector,
+//                             boost::mutex *mutex,
+                             pair<uint32_t, uint32_t> period,
+                             boost::function<bool(const ItemType &,
+                                                  ItemType &)> processItemFunc)
+{
+    vector<ItemType> tempOutputVec;
+    uint32_t start_index = period.first;
+    uint32_t index_cnt = period.second;
+    uint32_t final_index = start_index + index_cnt;
+    for(uint32_t index = start_index; index < final_index; index++){
+        ItemType outputItem;
+        if(processItemFunc(inputVector->at(index), outputItem)){
+            outputVector->push_back(outputItem);
+        }
+    }
+    static int i = 0;
+//    boost::unique_lock<boost::mutex> guard(*mutex);
+//    outputVector->insert(outputVector->end(),
+//                         tempOutputVec.begin(),
+//                         tempOutputVec.end());
+}
+
+template<typename ItemType>
+void multiThreadProcess(const vector<ItemType> &inputVector,
+                        vector<ItemType> &outputVector,
+                        boost::function<bool(const ItemType &,
+                                             ItemType &)> processItemFunc)
+{
+    auto start = system_clock::now();
+    vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(inputVector.size());
+    vector<ThreadPtr> threadVec;
+    vector<vector<ItemType>> outputVecs(indexPeriod.size());
+    boost::mutex mutex;
+    for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
+        threadVec.emplace_back(
+                    ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFunc<ItemType>,
+                                                            &inputVector,
+                                                            &outputVecs[thread_index],
+//                                                            &mutex,
+                                                            indexPeriod[thread_index],
+                                                            processItemFunc))));
+    }
+    for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
+        threadVec[thread_index]->join();
+        threadVec[thread_index].reset();
+        outputVector.insert(outputVector.end(),
+                            outputVecs[thread_index].begin(),
+                            outputVecs[thread_index].end());
+    }
+    auto end   = system_clock::now();
+    auto duration = duration_cast<milliseconds>(end - start);
+    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds,"
+              << " output size: " << outputVector.size();
+}
+
+
+
+template<typename ItemType>
+void periodProcessThreadFuncNoOutput(const vector<ItemType> *inputVector_1,
+                                     const vector<ItemType> *inputVector_2,
+                                     pair<uint32_t, uint32_t> period,
+                                     boost::function<void(const ItemType &,
+                                                          const ItemType &)> processItemFunc)
+{
+    uint32_t start_index = period.first;
+    uint32_t index_cnt = period.second;
+    uint32_t final_index = start_index + index_cnt;
+    for(uint32_t index = start_index; index < final_index; index++){
+        processItemFunc(inputVector_1->at(index),
+                        inputVector_2->at(index));
+    }
+    LOG(INFO) << start_index / index_cnt;
+}
+
+template<typename ItemType>
+void multiThreadProcessNoOutput(const vector<ItemType> &inputVector_1,
+                                const vector<ItemType> &inputVector_2,
+                                boost::function<void(const ItemType &,
+                                                     const ItemType &)> processItemFunc)
+{
+    auto start = system_clock::now();
+    if(inputVector_1.size() != inputVector_2.size()){
+        return;
+    }
+    vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(inputVector_1.size());
+    vector<ThreadPtr> threadVec;
+    for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
+        threadVec.emplace_back(
+                    ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFuncNoOutput<ItemType>,
+                                                            &inputVector_1,
+                                                            &inputVector_2,
+                                                            indexPeriod[thread_index],
+                                                            processItemFunc))));
+    }
+    for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
+        threadVec[thread_index]->join();
+        threadVec[thread_index].reset();
+    }
+    auto end   = system_clock::now();
+    auto duration = duration_cast<milliseconds>(end - start);
+    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds";
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename ItemType>
+void syncProcessThreadFunc(const vector<ItemType> *inputVector,
+                           vector<ItemType> *outputVector,
+                           int32_t *currentIndex,
+//                           boost::mutex *mutex,
+                           boost::function<bool(const ItemType &,
+                                                ItemType &)> processItemFunc)
+{
+    uint32_t size = inputVector->size();
+    int32_t index;
+    while(true){
+        {
+//            boost::unique_lock<boost::mutex> guard(*mutex);
+            index = ++(*currentIndex);
+        }
+        if((uint32_t)index >= size){
+//            LOG(INFO) << "index: " << index;
+            break;
+        }
+        ItemType outputItem;
+        if(processItemFunc(inputVector->at((uint32_t)index), outputItem)){
+            outputVector->push_back(outputItem);
+        }
+    }
+}
+
+template<typename ItemType>
+void syncProcess(const vector<ItemType> &inputVector,
+                 vector<ItemType> &outputVector,
+                 boost::function<bool(const ItemType &,
+                                      ItemType &)> processItemFunc)
+{
+    auto start = system_clock::now();
+    int32_t currentIndex = -1;
+    boost::mutex mutex;
+    vector<ThreadPtr> threadVec;
+    vector<vector<ItemType>> outputVecs(MAX_THREAD_CNT);
+    for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
+        threadVec.emplace_back(
+                    ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFunc<ItemType>,
+                                                            &inputVector,
+                                                            &outputVecs[thread_index],
+                                                            &currentIndex,
+//                                                            &mutex,
+                                                            processItemFunc))));
+    }
+    for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
+        threadVec[thread_index]->join();
+        threadVec[thread_index].reset();
+        outputVector.insert(outputVector.end(),
+                            outputVecs[thread_index].begin(),
+                            outputVecs[thread_index].end());
+    }
+    auto end   = system_clock::now();
+    auto duration = duration_cast<milliseconds>(end - start);
+    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds,"
+              << " output size: " << outputVector.size();
+}
+
+template<typename ItemType>
+void syncProcessThreadFunc(const vector<ItemType> *inputVector,
+                           int32_t *currentIndex,
+//                           boost::mutex *mutex,
+                           boost::function<void(const ItemType &)> processItemFunc)
+{
+    uint32_t size = inputVector->size();
+    int32_t index;
+    while(true){
+        {
+//            boost::unique_lock<boost::mutex> guard(*mutex);
+            index = ++(*currentIndex);
+        }
+        if((uint32_t)index >= size){
+            LOG(INFO) << "index: " << index;
+            break;
+        }
+        processItemFunc(inputVector->at((uint32_t)index));
+    }
+}
+
+template<typename ItemType>
+void syncProcess(const vector<ItemType> &inputVector,
+                 boost::function<void(const ItemType &)> processItemFunc)
+{
+    auto start = system_clock::now();
+    int32_t currentIndex = -1;
+    boost::mutex mutex;
+    vector<ThreadPtr> threadVec;
+    for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
+        threadVec.emplace_back(
+                    ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFunc<ItemType>,
+                                                            &inputVector,
+                                                            &currentIndex,
+//                                                            &mutex,
+                                                            processItemFunc))));
+    }
+    for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
+        threadVec[thread_index]->join();
+        threadVec[thread_index].reset();
+    }
+    auto end   = system_clock::now();
+    auto duration = duration_cast<milliseconds>(end - start);
+    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds";
+}
+
+
+template<typename ItemType>
+void syncProcessThreadFuncNoOutput(const vector<ItemType> *inputVector_1,
+                                   const vector<ItemType> *inputVector_2,
+                                   int32_t *currentIndex,
+                                   boost::function<void(const ItemType &,
+                                                        const ItemType &)> processItemFunc)
+{
+    uint32_t size = inputVector_1->size();
+    int32_t index;
+    while(true){
+        index = ++(*currentIndex);
+        if((uint32_t)index >= size){
+//            LOG(INFO) << "index: " << index;
+            break;
+        }
+        processItemFunc(inputVector_1->at(index),
+                        inputVector_2->at(index));
+    }
+}
+
+template<typename ItemType>
+void syncProcessNoOutput(const vector<ItemType> &inputVector_1,
+                         const vector<ItemType> &inputVector_2,
+                         boost::function<void(const ItemType &,
+                                              const ItemType &)> processItemFunc)
+{
+    auto start = system_clock::now();
+    if(inputVector_1.size() != inputVector_2.size()){
+        return;
+    }
+    vector<ThreadPtr> threadVec;
+    int32_t currentIndex = -1;
+    for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
+        threadVec.emplace_back(
+                    ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFuncNoOutput<ItemType>,
+                                                            &inputVector_1,
+                                                            &inputVector_2,
+                                                            &currentIndex,
+                                                            processItemFunc))));
+    }
+    for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
+        threadVec[thread_index]->join();
+        threadVec[thread_index].reset();
+    }
+    auto end   = system_clock::now();
+    auto duration = duration_cast<milliseconds>(end - start);
+    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds";
+}
+
+
+template <typename PointType>
+void addOffset(const Vector3d &offset,
+               pcl::PointCloud<PointType> &pointcloud){
+    for(auto &p : pointcloud){
+        p.x += offset.x();
+        p.y += offset.y();
+        p.z += offset.z();
+    }
+    pointcloud.sensor_origin_ += Vector4f{offset.x(), offset.y(), offset.z(), 0};
+}
+
+#include <boost/filesystem.hpp>
+#include <boost/optional.hpp>
+#include <pcl/filters/voxel_grid.h>
+
+using namespace boost::filesystem;
+template<typename PointType>
+void downSamplePcd(const string &path)
+{
+    using PointCloud = pcl::PointCloud<PointType>;
+    if(!exists(path)){
+        return;
+    }
+    directory_iterator file_iter(path);
+    directory_iterator pcd_end_iter;
+    for(; file_iter != pcd_end_iter; file_iter++) {
+        string subffix = file_iter->path().extension().string();
+        if(subffix == ".pcd"){
+            string stem = file_iter->path().stem().string();
+            auto it = stem.find("view");
+            if(it != string::npos){
+                continue;
+            }
+            string pcd_path = file_iter->path().string();
+            PointCloud pointcloud;
+            pcl::io::loadPCDFile(pcd_path, pointcloud);
+            typename PointCloud::Ptr input, points_filtered;
+            pcl::VoxelGrid<PointType> grid;
+
+            points_filtered.reset(new PointCloud);
+            input.reset(new PointCloud(pointcloud));
+
+            grid.setLeafSize(0.4, 0.4, 0.4);
+            grid.setInputCloud(input);
+            grid.filter(*points_filtered);
+            string output = path + "/view/";
+            if(!exists(output)){
+                create_directories(output);
+            }
+            pcl::io::savePCDFileBinary(output + stem +"_view.pcd", *points_filtered);
+            LOG(INFO) << output + stem +"_view.pcd saved";
+        }
+    }
+
+//    string output = "/data/lidar/2147/pcd_cart_new/";
+//    if(!exists(output)){
+//        create_directories(output);
+//    }
+//    for(; file_iter != pcd_end_iter; file_iter++) {
+//        string stem = file_iter->path().stem().string();
+//        string subffix = file_iter->path().extension().string();
+//        if(subffix == ".pcd"){
+//            string pcd_path = file_iter->path().string();
+//            PointCloud pointcloud;
+//            pcl::io::loadPCDFile(pcd_path, pointcloud);
+//            Vector3d offset = Vector3d(-1, 0, 0);
+//            addOffset<PointType>(offset, pointcloud);
+//            pcl::io::savePCDFileBinary(output + stem +".pcd", pointcloud);
+//            LOG(INFO) << output + stem +".pcd saved";
+//        }
+//    }
+}
+
+#endif // BASIC_FUNCTIONS_H

libs/utils/basic_types.h

+#ifndef BASIC_TYPES_H
+#define BASIC_TYPES_H
+#pragma once
+#include <boost/shared_ptr.hpp>
+#include <eigen3/Eigen/Geometry>
+#include "glog/logging.h"
+#include <map>
+#include <iomanip>
+#include <pcl/io/pcd_io.h>
+#include <thread>
+#include <boost/make_shared.hpp>
+
+using namespace std;
+using namespace Eigen;
+#define MAX_THREAD_CNT (48)
+
+typedef boost::shared_ptr<boost::thread> ThreadPtr;
+
+/////////// AbstractParser /////////////
+
+enum LidarType
+{
+    HesaiP40 = 0,
+    HesaiXT32,
+    Hesai80
+};
+
+/////////// Trajectory /////////////
+
+#include <fstream>
+
+struct PPK_Raw_Info{
+    uint32_t gps_week;
+    double gps_sec, lat, lng, height,
+    roll, pitch, yaw;
+};
+
+struct NODE_Raw_Info{
+    uint32_t node_id;
+    double time, x, y, z, w, qx, qy, qz;
+};
+
+enum DevicePosition{
+    TopLidar = 0,
+    BackLidar = 1,
+    IMU = 2
+};
+
+struct TrajPoint{
+    double timestamp = 0;
+    uint32_t nodeId = 0;
+    Vector3d translation = Vector3d(0, 0, 0);
+    Quaterniond rotation = Quaterniond::Identity();
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+enum TrajType{
+    PPK,
+    NODE
+};
+
+/////////// MapTask /////////////
+
+struct Time_Duration{
+    double start_sec;
+    double end_sec;
+};
+
+/////////// StrategyControl /////////////
+
+enum Pointcloud_Source_Mode{
+    PCAP_MODE,
+    BAG_MODE,
+    PCAP_BAG_MODE
+};
+
+enum PointCloudSource{
+    PCAP,
+    BAG
+};
+
+enum Trajectory_Source_Mode{
+    NODE_MODE,
+    PPK_MODE,
+    PPK_UNDISORT_NODE_MAP_MODE
+};
+
+enum Filter_Mode{
+    DYNAMIC_REMOVE_MODE,
+    DIRECT_OUTPUT_MODE
+};
+
+struct StageInfo{
+    DevicePosition lidarPosition;
+    PointCloudSource pointcloudSource;
+};
+
+enum Strategy_Mode{
+    MESH,
+    PERIOD
+};
+
+enum Bag_Mode{
+    Disorted,
+    Undisorted
+};
+
+/////////// TaskServer /////////////
+
+#define NODES_PER_SUBMAP (64)
+#define SUBMAP_PACE (32)
+
+
+struct TaskDescribe{
+    string task_prefix;
+    vector<vector<Time_Duration>> time_durations_vec;
+    vector<string> output_prefix_vec;
+};
+
+/////////// DynamicRemover /////////////
+
+const double MISS_PER_HIT_PER_STAGE = 4;
+
+
+#endif // BASIC_TYPES_H

libs/utils/synchronizedqueue.h

+#ifndef SYNCHRONIZEDQUEUE_H
+#define SYNCHRONIZEDQUEUE_H
+
+#endif // SYNCHRONIZEDQUEUE_H
+
+#include <boost/thread/thread.hpp>
+#include <boost/asio.hpp>
+#include <queue>
+#include <deque>
+#pragma once
+
+template<typename T>
+class SynchronizedQueue
+{
+  public:
+
+    SynchronizedQueue () :
+      queue_(), mutex_(), cond_(), request_to_end_(false), enqueue_data_(true) { }
+
+    void
+    enqueue (const T& data)
+    {
+      boost::unique_lock<boost::mutex> lock (mutex_);
+
+      if (enqueue_data_)
+      {
+        queue_.push (data);
+        cond_.notify_one ();
+      }
+    }
+
+    bool
+    dequeue (T& result)
+    {
+      boost::unique_lock<boost::mutex> lock (mutex_);
+
+      while (queue_.empty () && (!request_to_end_))
+      {
+        cond_.wait (lock);
+      }
+
+      if (request_to_end_)
+      {
+        doEndActions ();
+        return false;
+      }
+
+      result = queue_.front ();
+      queue_.pop ();
+
+      return true;
+    }
+
+    void
+    stopQueue ()
+    {
+      boost::unique_lock<boost::mutex> lock (mutex_);
+      request_to_end_ = true;
+      cond_.notify_all ();
+    }
+
+    unsigned int
+    size ()
+    {
+      boost::unique_lock<boost::mutex> lock (mutex_);
+      return static_cast<unsigned int> (queue_.size ());
+    }
+
+    bool
+    isEmpty () const
+    {
+      boost::unique_lock<boost::mutex> lock (mutex_);
+      return (queue_.empty ());
+    }
+
+  private:
+    void
+    doEndActions ()
+    {
+      enqueue_data_ = false;
+
+      while (!queue_.empty ())
+      {
+        queue_.pop ();
+      }
+    }
+
+    std::queue<T> queue_;              // Use STL queue to store data
+    mutable boost::mutex mutex_;       // The mutex to synchronise on
+    boost::condition_variable cond_;   // The condition to wait for
+
+    bool request_to_end_;
+    bool enqueue_data_;
+};

libs/utils/sys_utils.h

+#ifndef SYS_UTILS_H
+#define SYS_UTILS_H
+
+#include <sys/resource.h>
+
+const char data_mem[] = "VmRSS:";
+
+void print_mem(int pid)
+{
+    FILE *stream;
+    char cache[256];
+    char mem_info[64];
+
+    sprintf(mem_info, "/proc/%d/status", pid);
+    stream = fopen(mem_info, "r");
+    if (stream == NULL) {
+        return;
+    }
+
+    while(fscanf(stream, "%s", cache) != EOF) {
+        if (strncmp(cache, data_mem, sizeof(data_mem)) == 0) {
+            if (fscanf(stream, "%s", cache) != EOF) {
+                LOG(INFO) << "current memory: " << string(cache);
+                break;
+            }
+        }
+    }
+}
+
+int SysMem()
+{
+    const int MAX_LEN=1024;
+    char sInfo[MAX_LEN];
+    char strInfo[1024];
+
+    //
+    long mem_used = -1;
+    long mem_free = -1;
+    long mem_total = -1;
+    char name[20];
+           float fMemRate;
+
+    FILE *fp;
+    char buf1[128];
+    char buf2[128];
+    int i = 0;
+
+    memset(sInfo, 0x00, sizeof(sInfo));
+    memset(strInfo, 0x00, sizeof(strInfo));
+
+    //读取文件中mem信息
+    fp = fopen("/proc/meminfo","r");
+    if(fp == NULL)
+    {
+     perror("fopen:");
+     return -1;
+    }
+    fgets(buf1,sizeof(buf1),fp);
+    fgets(buf2,sizeof(buf2),fp);
+    fclose(fp);
+
+    //计算
+    strcpy(strInfo," RTOL(kb)      FREE(kb)        RINUSE(kb)       RATE%%\n\n");
+    sscanf(buf1,"%s%ld",name,&mem_total);
+    sscanf(buf2,"%s%ld",name,&mem_free);
+    mem_used = mem_total - mem_free;
+    fMemRate = 1.0 * mem_used / mem_total;
+    sprintf(sInfo,"% 6ld     %6ld         %6ld      %6.1f\n",mem_total,mem_free,mem_used,fMemRate);
+    strcat(strInfo,sInfo);
+//    printf("%s\n",strInfo);
+    return mem_total;
+}
+
+#endif // SYS_UTILS_H