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
+)

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.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_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