Merge pull request weisongwen#1 from weisongwen/master

upload code

CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/melodic/share/catkin/cmake/toplevel.cmake

GraphGNSSLib.code-workspace

@@ -0,0 +1,8 @@
+{
+	"folders": [
+		{
+			"path": "../.."
+		}
+	],
+	"settings": {}
+}

README.md

@@ -1,3 +1,130 @@
 # GraphGNSSLib
-## An Open-source Package for GNSS Positioning and Real-time Kinematic Using Factor Graph Optimization
-This repository is the implementation of the open-sourced package, the GraphGNSSLib, which makes use of the factor graph optimization (FGO) to perform the GNSS positioning and real-time kinematic (RTK) positioning. In this package, measurements from the historical and current epochs are structured into a factor graph. The package is based on C++ which is compatible with the robot operation system (ROS) platform. The package will be released recently!
+### An Open-source Package for GNSS Positioning and Real-time Kinematic Using Factor Graph Optimization
+
+This repository is the implementation of the open-sourced package, the GraphGNSSLib, which makes use of the factor graph optimization (FGO) to perform the postprocessing of GNSS positioning and real-time kinematic (RTK) positioning. In this package, measurements from the historical and current epochs are structured into a factor graph which is then solved by non-linear optimization. The package is based on C++ which is compatible with the robot operation system (ROS) platform. Meanwhile, this package combines the RTKLIB (**[version: 2.4.3 b33](http://www.rtklib.com/)**) to read/decode the GNSS [RINEX](https://en.wikipedia.org/wiki/RINEX) files. Users from Robotics field can easily have access to GNSS raw data for further study. We are still improving the code readibility with the hope that this package can benefit the research community.
+
+**Important Notes**: 
+  - Be noted that the **GNSS Positioning** mentioned throughout the package means estimating the positioing of the GNSS receiver based on the combination of pseudorange and Doppler measurements uisng FGO.
+  - Be noted that the **GNSS-RTK Positioning** mentioned throughout the package means estimating the positioing (float solution) of the GNSS receiver based on the combination of double-differenced pseudorange, carrier-phase and the Doppler measurements using FGO. Finally, the ambiguity is resolved using LAMBDA algorithm via epoch-by-epoch manner.
+
+**Authors**: [Weisong Wen](https://weisongwen.wixsite.com/weisongwen), [Li-ta Hsu](https://www.polyu-ipn-lab.com/) from the [Intelligent Positioning and Navigation Laboratory](https://www.polyu-ipn-lab.com/), The Hong Kong Polytechnic University
+
+**Related Papers:** (paper is not exactly same with code)
+  - Wen Weisong., Hsu, Li-Ta.* **Towards Robust GNSS Positioning and Real-Time Kinematic Using Factor Graph Optimization**, *ICRA 2021*, Xi'an, China. (**Accepted**)
+
+*if you use GraphGNSSLib for your academic research, please cite our related [papers](https://www.polyu-ipn-lab.com/)*
+
+<p align="center">
+  <img width="712pix" src="img/software_flowchart.png">
+</p>
+
+<center> Software flowchart of GraphGNSSLib, more information please refer to our paper.</center>
+
+## 1. Prerequisites
+### 1.1 **Ubuntu** and **ROS**
+Ubuntu 64-bit 16.04, ROS Kinetic. [ROS Installation](http://wiki.ros.org/ROS/Installation). We only test it on Ubuntu 16.04 with ROS Kinetic. 
+
+### 1.2. **Ceres Solver**
+Follow the following instructions to install Ceres-solver instead of using the latest version of Ceres-solver.
+
+**Step 1**: Download the [Ceres-solver](https://github.com/weisongwen/GraphGNSSLib/tree/master/support_files) which is compatible with GraphGNSSLib. 
+
+**Step 2**: make and install
+```bash
+sudo apt-get install cmake
+# google-glog + gflags
+sudo apt-get install libgoogle-glog-dev
+# BLAS & LAPACK
+sudo apt-get install libatlas-base-dev
+# Eigen3
+sudo apt-get install libeigen3-dev
+# make Ceres-solver
+mkdir ceres-bin
+cd ceres-bin
+cmake ../ceres-solver
+sudo make -j4
+sudo make test
+sudo make install
+```
+
+### 1.3. **Extra Libraries**
+```bash
+sudo apt-get install ros-kinetic-novatel-msgs
+```
+## 2. Build GraphGNSSLib
+Clone the repository and catkin_make:
+```bash
+mkdir GraphGNSSLib/src
+cd ~/GraphGNSSLib/src
+mkdir result
+git clone https://github.com/weisongwen/GraphGNSSLib.git
+cd ../
+# if you fail in the last catkin_make, please source and catkin_make again
+catkin_make
+source ~/GraphGNSSLib/devel/setup.bash
+catkin_make
+```
+(**if you fail in this step, try to find another computer with clean system or reinstall Ubuntu and ROS**)
+
+## 3. Run GNSS positioning via FGO using dataset [UrbanNav](https://www.polyu-ipn-lab.com/download)   
+The GNSS positioning via FGO is validated using static dataset collected near TST of Hong Kong. Several parameters are as follows:
+  - GPS second span: **46701** to **47185**
+  - satellite system: **GPS/BeiDou**
+  - measurements considered: double-differenced pseudorange and carrier-phase measurements, Doppler measurements
+  - result is saved by default
+    ```c++
+    ~/GraphGNSSLib/trajectory_psr_dop_fusion.csv
+    ```
+
+please enable the following in rtklib.h
+```bash
+#define RTK_FGO 0
+```
+- Solution 1 to run the GNSS positioning Demo
+  ```bash
+  source ~/GraphGNSSLib/devel/setup.bash
+  # read GNSS raw data and publish as ROS topic
+  # we provide several datasets, enjoy it!
+  roslaunch global_fusion dataublox_TST20190428.launch
+  # run pseudorange and doppler fusion
+  roslaunch global_fusion psr_doppler_fusion.launch
+  ```
+<p align="center">
+  <img width="712pix" src="img/SPP_trajectory1.png">
+</p>
+<center> Trajectories of three methods (GNSS positioning using WLS with the red curve, GNSS positioning using EKF with the green curve, and GNSS positioning using FGO with blue curve throughout the test. The x-axis and y-axis denote the east and north directions, respectively</center>
+
+
+## 4. Run GNSS RTK-FGO using static dataset   
+The GNSS RTK-FGO is validated using static dataset collected near TST of Hong Kong. Several parameters are as follows:
+  - GPS second span: **270149** to **270306**
+  - satellite system: **GPS/BeiDou**
+  - measurements considered: double-differenced pseudorange and carrier-phase measurements, Doppler measurements
+  - result is saved by default
+    ```c++
+    ~/GraphGNSSLib/FGO_trajectoryllh_pdrtk.csv
+    ```
+
+please enable the following in rtklib.h
+```bash
+#define RTK_FGO 1
+```
+- Solution 1 to run the RTK-FGO Demo
+  ```bash
+  source ~/GraphGNSSLib/devel/setup.bash
+  # read GNSS raw data and publish as ROS topic
+  roslaunch global_fusion dataublox_TST20200603.launch
+  # run GNSS RTK
+  roslaunch global_fusion psr_doppler_car_rtk.launch
+  ```
+<p align="center">
+  <img width="712pix" src="img/RTK_trajectory.png">
+</p>
+<center> Trajectories of three methods (RTK-EKF with the red dots and RTK-FGO with the blue dots throughout the test. The x-axis and y-axis denote the east and north directions, respectively.</center>
+
+
+## 5. Acknowledgements
+We use [Ceres-solver](http://ceres-solver.org/) for non-linear optimization and [RTKLIB](http://www.rtklib.com/) for GNSS data decoding, etc. Some functions are originated from [VINS-mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono). The [rviz_satellite](https://github.com/nobleo/rviz_satellite) is used for visualization. If there is any thing inappropriate, please contact me through [email protected] ([Weisong WEN](https://weisongwen.wixsite.com/weisongwen)).
+
+## 6. License
+The source code is released under [GPLv3](http://www.gnu.org/licenses/) license. We are still working on improving the code reliability. For any technical issues, please contact Weisong Wen <[email protected]>. For commercial inquiries, please contact Li-ta Hsu <[email protected]>.

global_fusion/CMakeLists.txt

@@ -0,0 +1,184 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(global_fusion)
+
+set(CMAKE_BUILD_TYPE "Debug")
+
+# set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS "-std=c++14")
+# C++ 14 is required
+add_definitions(-std=c++14)
+set(CMAKE_CXX_STANDARD 14)
+
+#-DEIGEN_USE_MKL_ALL")
+#set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+find_package(Eigen3)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+find_package(Gflags)
+find_package(Glog)
+include_directories(${GFLAGS_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS})
+include_directories(include)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  nlosExclusion
+)
+
+find_package(Ceres REQUIRED)
+
+add_subdirectory(./ThirdParty/GeographicLib/)
+add_subdirectory(./RTKLIB/)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
+  ./ThirdParty/GeographicLib/include/
+)
+
+
+catkin_package()
+
+# add rtklib
+add_library(convkml RTKLIB/src/convkml.c)
+add_library(convrnx RTKLIB/src/convrnx.c)
+add_library(datum RTKLIB/src/datum.c)
+add_library(download RTKLIB/src/download.c)
+add_library(ephemeris RTKLIB/src/ephemeris.c)
+add_library(geoid RTKLIB/src/geoid.c)
+add_library(ionex RTKLIB/src/ionex.c)
+add_library(lambda RTKLIB/src/lambda.cpp)
+add_dependencies(lambda nlosExclusion_gencpp)
+add_library(options RTKLIB/src/options.c)
+add_library(pntpos RTKLIB/src/pntpos.cpp)
+add_dependencies(pntpos nlosExclusion_gencpp)
+add_library(postpos RTKLIB/src/postpos.cpp)
+add_dependencies(postpos nlosExclusion_gencpp)
+add_library(ppp_ar RTKLIB/src/ppp_ar.c)
+add_library(ppp RTKLIB/src/ppp.c)
+add_library(preceph RTKLIB/src/preceph.c)
+add_library(qzslex RTKLIB/src/qzslex.c)
+add_library(rcvraw RTKLIB/src/rcvraw.c)
+add_library(rinex RTKLIB/src/rinex.c)
+add_library(rtcm RTKLIB/src/rtcm.c)
+add_library(rtcm2 RTKLIB/src/rtcm2.c)
+add_library(rtcm3 RTKLIB/src/rtcm3.c)
+add_library(rtcm3e RTKLIB/src/rtcm3e.c)
+add_library(rtkcmn RTKLIB/src/rtkcmn.c)
+add_library(rtkpos RTKLIB/src/rtkpos.cpp)
+add_dependencies(rtkpos nlosExclusion_gencpp)
+add_library(rtksvr RTKLIB/src/rtksvr.c)
+add_library(sbas RTKLIB/src/sbas.c)
+add_library(solution RTKLIB/src/solution.c)
+add_library(stream RTKLIB/src/stream.c)
+add_library(streamsvr RTKLIB/src/streamsvr.c)
+add_library(tle RTKLIB/src/tle.c)
+
+add_executable(testRTKLIBNode 
+	src/testRTKLIBNode.cpp)
+target_link_libraries(testRTKLIBNode ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+
+
+
+add_executable(gnss_preprocessor_node 
+	src/gnss_preprocessor/gnss_preprocessor.cpp
+	./RTKLIB/src/rtkpos.cpp
+	./RTKLIB/src/postpos.cpp
+	./RTKLIB/src/ppp.c)
+target_link_libraries(gnss_preprocessor_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+
+add_executable(rosbag_generator_node 
+	src/gnss_preprocessor/rosbag_generator.cpp
+	./RTKLIB/src/rtkpos.cpp
+	./RTKLIB/src/postpos.cpp
+	./RTKLIB/src/ppp.c)
+target_link_libraries(rosbag_generator_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+
+add_executable(psr_spp_node 
+	src/gnss_estimator/psr_spp.cpp)
+target_link_libraries(psr_spp_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+
+
+add_executable(psr_spp_dyna_auto_node 
+	src/gnss_estimator/psr_spp_dyna_auto.cpp)
+target_link_libraries(psr_spp_dyna_auto_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+
+
+add_executable(psr_doppler_fusion_node 
+	src/gnss_estimator/psr_doppler_fusion.cpp
+	./RTKLIB/src/rtkpos.cpp
+	./RTKLIB/src/postpos.cpp
+	./RTKLIB/src/ppp.c)
+target_link_libraries(psr_doppler_fusion_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+
+add_executable(psr_doppler_car_rtk_node 
+	src/gnss_estimator/psr_doppler_car_rtk.cpp
+	./RTKLIB/src/rtkpos.cpp
+	./RTKLIB/src/postpos.cpp
+	./RTKLIB/src/ppp.c)
+target_link_libraries(psr_doppler_car_rtk_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+
+add_executable(psr_doppler_car_rtk_dyna_node 
+	src/gnss_estimator/psr_doppler_car_rtk_dyna.cpp
+	./RTKLIB/src/rtkpos.cpp
+	./RTKLIB/src/postpos.cpp
+	./RTKLIB/src/ppp.c)
+target_link_libraries(psr_doppler_car_rtk_dyna_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+
+add_executable(evaluate_gps_solution_node 
+	src/evo/evaluate_gps_solution.cpp
+	./RTKLIB/src/rtkpos.cpp
+	./RTKLIB/src/postpos.cpp
+	./RTKLIB/src/ppp.c)
+target_link_libraries(evaluate_gps_solution_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} 
+						convkml convrnx datum download ephemeris geoid ionex lambda
+						options pntpos postpos ppp_ar ppp preceph qzslex rcvraw rinex
+						rtcm rtcm2 rtcm3 rtcm3e rtkcmn rtkpos rtksvr sbas solution
+						stream streamsvr tle
+						libGeographiccc) 
+

global_fusion/RTKLIB/.gitattributes

@@ -0,0 +1,22 @@
+# Auto detect text files and perform LF normalization
+* text=auto
+
+# Custom for Visual Studio
+*.cs     diff=csharp
+*.sln    merge=union
+*.csproj merge=union
+*.vbproj merge=union
+*.fsproj merge=union
+*.dbproj merge=union
+
+# Standard to msysgit
+*.doc	 diff=astextplain
+*.DOC	 diff=astextplain
+*.docx diff=astextplain
+*.DOCX diff=astextplain
+*.dot  diff=astextplain
+*.DOT  diff=astextplain
+*.pdf  diff=astextplain
+*.PDF	 diff=astextplain
+*.rtf	 diff=astextplain
+*.RTF	 diff=astextplain

global_fusion/RTKLIB/.gitignore

@@ -0,0 +1,8 @@
+*.o
+*.obs
+*.ini
+*.local
+Release
+Release_Build
+Debug_Build
+__history

global_fusion/RTKLIB/CMakeLists.txt

@@ -0,0 +1,34 @@
+#project (GeographicLib)
+project (RTKLIB)
+
+# Version information
+set (PROJECT_VERSION_MAJOR 1)
+set (PROJECT_VERSION_MINOR 49)
+set (PROJECT_VERSION_PATCH 0)
+set (PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
+if (PROJECT_VERSION_PATCH GREATER 0)
+  set (PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}")
+endif ()
+
+# The library version tracks the numbering given by libtool in the
+# autoconf set up.
+set (LIBVERSION_API 17)
+set (LIBVERSION_BUILD 17.1.2)
+string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
+string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
+
+cmake_minimum_required (VERSION 2.8.4) # This version was released 2011-02-16
+
+
+# set (LIBNAME RTKLIB)
+
+# include_directories(
+#   ./include/
+# )
+
+# add_library(libGeographiccc src/LocalCartesian.cpp
+#                           src/Geocentric.cpp
+#                           src/Math.cpp)
+
+# add_library(RTKLIB src/rtklib.h)
+#                   src/convkml.c)