pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Compare commits

base: pi:fd031c7ecf2658022d5bddce2ef20d89fec84cbb
Branches Tags
pi:master
...
compare: pi:master
Branches Tags
pi:master

18 Commits
fd031c7ecf ... master

Author SHA1 Message Date
Иван Подмогильный
d4c15bfd0a Update 'ReadMe.md' 2022-08-31 23:28:19 +03:00
Иван Подмогильный
dfa5329c59 Update 'ReadMe.md' 2022-08-31 23:24:21 +03:00
Иван Подмогильный
c160726063 Update 'ReadMe.md' 2022-08-31 23:23:22 +03:00
Иван Подмогильный
b28888119a Update 'ReadMe.md' 2022-08-31 23:19:47 +03:00
admin1
de2a43ce6e Merge branch 'master' of https://git.drivecast.tech/pi/openvins_linux 
merge
 2022-08-31 23:14:19 +03:00
admin1
a90ffffe3a fixed .rviz file in ov_msckf 2022-08-31 23:14:00 +03:00
Иван Подмогильный
e492e9ae93 Update 'ReadMe.md' 2022-08-31 23:07:56 +03:00
Иван Подмогильный
163e6e6a16 Update 'ReadMe.md' 2022-08-31 23:03:53 +03:00
Иван Подмогильный
af9debd464 Update 'ReadMe.md' 2022-08-31 23:03:33 +03:00
Иван Подмогильный
a190949040 Update 'ReadMe.md' 2022-08-31 22:58:30 +03:00
Иван Подмогильный
991d5c6ccb Update 'ReadMe.md' 2022-08-31 17:48:08 +03:00
Иван Подмогильный
a9f23893ed Update 'ReadMe.md' 2022-08-31 17:35:24 +03:00
admin1
93c191283b added rviz config file to global fusion node 2022-08-31 16:00:42 +03:00
admin1
89f25ad747 terminal commit 2022-08-30 18:44:20 +03:00
admin1
979c7a2250 gps fusion implemented 2022-08-28 22:25:47 +03:00
admin1
b8ee6672d1 fixed cubes rendering. created additional small node for publishing the cubes. debug process made easier 2022-08-17 15:43:07 +03:00
admin1
f8843bf0fc implemented frame synchronization and frame skew to match rendered image with sensor image" 
;
 2022-08-16 12:44:20 +03:00
admin1
1c81d558a8 created rosnode to render images 2022-08-15 09:54:33 +03:00
34 changed files with 877244 additions and 341 deletions
Expand all files Collapse all files

200
ReadMe.md
View File

@@ -1,176 +1,42 @@
# OpenVINS with the additional features for AR tasks. # open_vins for AR.
Please refer to Redmine page: https://redmine.drivecast.tech/issues/375 for now to track the history and understand the feature set. Link to the original repository:
# OpenVINS # Features
1. GPS loosely coupled sensor fusion taken from VINS-Fusion.
2. AR visualizations in ROS Rviz.
3. Ability to save the image sequnece of the camera with rendered AR directly in Rviz.
[![ROS 1 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml) ## Redmine
[![ROS 2 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml) Please visit the Redmine notes of the following tasks to see the more detailed overview and the detailed developing process:
[![ROS Free Workflow](https://github.com/rpng/open_vins/actions/workflows/build.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build.yml) https://redmine.drivecast.tech/issues/376
https://redmine.drivecast.tech/issues/375
https://redmine.drivecast.tech/issues/374
Welcome to the OpenVINS project! ## Installation
The OpenVINS project houses some core computer vision code along with a state-of-the art filter-based visual-inertial
estimator. The core filter is an [Extended Kalman filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter) which
fuses inertial information with sparse visual feature tracks. These visual feature tracks are fused leveraging
the [Multi-State Constraint Kalman Filter (MSCKF)](https://ieeexplore.ieee.org/document/4209642) sliding window
formulation which allows for 3D features to update the state estimate without directly estimating the feature states in
the filter. Inspired by graph-based optimization systems, the included filter has modularity allowing for convenient
covariance management with a proper type-based state system. Please take a look at the feature list below for full
details on what the system supports.
* Github project page - https://github.com/rpng/open_vins ### Requirements
* Documentation - https://docs.openvins.com/ 1. Ubuntu 20.04
* Getting started guide - https://docs.openvins.com/getting-started.html 2. OpenCV 3 with contrib and libgdal included: https://docs.opencv.org/4.x/d7/d9f/tutorial_linux_install.html. If catkin buld will be complaining that OpenCV is not found, please set the variable in CMakeLists.txt for each package: set(OpenCV_DIR "path_to_opencv_build_directory")
* Publication reference - http://udel.edu/~pgeneva/downloads/papers/c10.pdf 3. ROS noetic, install only ROS fromm this guide (do not install open_vins): https://docs.openvins.com/gs-installing.html
4. KAIST Urban dataset. On the server: /mnt/disk-small
## News / Events ### Building
1. `mkdir -p ~/workspace/catkin_ws_ov/src/ && cd ~/workspace/catkin_ws_ov/src`
2. `git clone https://git.drivecast.tech/pi/openvins_linux.git`
3. `cd ../`
4. `catkin build`
* **March 14, 2022** - Initial dynamic initialization open sourcing, asynchronous subscription to inertial readings and publishing of odometry, support for lower frequency feature tracking. See v2.6 [PR#232](https://github.com/rpng/open_vins/pull/232) for details. ### Running
* **December 13, 2021** - New YAML configuration system, ROS2 support, Docker images, robust static initialization based on disparity, internal logging system to reduce verbosity, image transport publishers, dynamic number of features support, and other small fixes. See 1. from terminal in `~/workspace/catkin_ws_ov/` directory `source devel/setup.bash` and then `roslaunch ov_msckf subscribe.launch config:=kaist`
v2.5 [PR#209](https://github.com/rpng/open_vins/pull/209) for details. 2. from second terminal in same directory `source devel/setup.bash` and then `roslaunch global_fusion global_fusion.launch`
* **July 19, 2021** - Camera classes, masking support, alignment utility, and other small fixes. See 3. from 3d terminal in same directory `source devel/setup.bash` and then `rviz`, and open the open_vins/ov_msckf/launch/display.rviz configuration file.
v2.4 [PR#117](https://github.com/rpng/open_vins/pull/186) for details. 4. from 4th terminal in same directory `source devel/setup.bash` and then `rviz`, and open the openv_vins/global_fusion/launch/display.rviz` file.
* **December 1, 2020** - Released improved memory management, active feature pointcloud publishing, limiting number of 5. from 5th terminal in the dataset bag directory: `rosbag play merged.bag gps_fix.bag`
features in update to bound compute, and other small fixes. See
v2.3 [PR#117](https://github.com/rpng/open_vins/pull/117) for details.
* **November 18, 2020** - Released groundtruth generation utility package, [vicon2gt](https://github.com/rpng/vicon2gt)
to enable creation of groundtruth trajectories in a motion capture room for evaulating VIO methods.
* **July 7, 2020** - Released zero velocity update for vehicle applications and direct initialization when standing
still. See [PR#79](https://github.com/rpng/open_vins/pull/79) for details.
* **May 18, 2020** - Released secondary pose graph example
repository [ov_secondary](https://github.com/rpng/ov_secondary) based
on [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion). OpenVINS now publishes marginalized feature
track, feature 3d position, and first camera intrinsics and extrinsics.
See [PR#66](https://github.com/rpng/open_vins/pull/66) for details and discussion.
* **April 3, 2020** - Released [v2.0](https://github.com/rpng/open_vins/releases/tag/v2.0) update to the codebase with
some key refactoring, ros-free building, improved dataset support, and single inverse depth feature representation.
Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.0) for details.
* **January 21, 2020** - Our paper has been accepted for presentation in [ICRA 2020](https://www.icra2020.org/). We look
forward to seeing everybody there! We have also added links to a few videos of the system running on different
datasets.
* **October 23, 2019** - OpenVINS placed first in the [IROS 2019 FPV Drone Racing VIO Competition
](http://rpg.ifi.uzh.ch/uzh-fpv.html). We will be giving a short presentation at
the [workshop](https://wp.nyu.edu/workshopiros2019mav/) at 12:45pm in Macau on November 8th.
* **October 1, 2019** - We will be presenting at the [Visual-Inertial Navigation: Challenges and Applications
](http://udel.edu/~ghuang/iros19-vins-workshop/index.html) workshop at [IROS 2019](https://www.iros2019.org/). The
submitted workshop paper can be found at [this](http://udel.edu/~ghuang/iros19-vins-workshop/papers/06.pdf) link.
* **August 21, 2019** - Open sourced [ov_maplab](https://github.com/rpng/ov_maplab) for interfacing OpenVINS with
the [maplab](https://github.com/ethz-asl/maplab) library.
* **August 15, 2019** - Initial release of OpenVINS repository and documentation website!
## Project Features #### To run the kaist complex-urban dataset please download the bag file from /mnt/disk-small/podmivan/datasets/complex_urban/urban32-yeouido/bag/ directory. You need to download merged2.bag and gps_fix.bag files.
* Sliding window visual-inertial MSCKF
* Modular covariance type system
* Comprehensive documentation and derivations
* Extendable visual-inertial simulator
* On manifold SE(3) b-spline
* Arbitrary number of cameras
* Arbitrary sensor rate
* Automatic feature generation
* Five different feature representations
1. Global XYZ
2. Global inverse depth
3. Anchored XYZ
4. Anchored inverse depth
5. Anchored MSCKF inverse depth
6. Anchored single inverse depth
* Calibration of sensor intrinsics and extrinsics
* Camera to IMU transform
* Camera to IMU time offset
* Camera intrinsics
* Environmental SLAM feature
* OpenCV ARUCO tag SLAM features
* Sparse feature SLAM features
* Visual tracking support
* Monocular camera
* Stereo camera
* Binocular (synchronized) cameras
* KLT or descriptor based
* Masked tracking
* Static and dynamic state initialization
* Zero velocity detection and updates
* Out of the box evaluation on EurocMav, TUM-VI, UZH-FPV, KAIST Urban and VIO datasets
* Extensive evaluation suite (ATE, RPE, NEES, RMSE, etc..)
## Codebase Extensions
* **[vicon2gt](https://github.com/rpng/vicon2gt)** - This utility was created to generate groundtruth trajectories using
a motion capture system (e.g. Vicon or OptiTrack) for use in evaluating visual-inertial estimation systems.
Specifically we calculate the inertial IMU state (full 15 dof) at camera frequency rate and generate a groundtruth
trajectory similar to those provided by the EurocMav datasets. Performs fusion of inertial and motion capture
information and estimates all unknown spacial-temporal calibrations between the two sensors.
* **[ov_maplab](https://github.com/rpng/ov_maplab)** - This codebase contains the interface wrapper for exporting
visual-inertial runs from [OpenVINS](https://github.com/rpng/open_vins) into the ViMap structure taken
by [maplab](https://github.com/ethz-asl/maplab). The state estimates and raw images are appended to the ViMap as
OpenVINS runs through a dataset. After completion of the dataset, features are re-extract and triangulate with
maplab's feature system. This can be used to merge multi-session maps, or to perform a batch optimization after first
running the data through OpenVINS. Some example have been provided along with a helper script to export trajectories
into the standard groundtruth format.
* **[ov_secondary](https://github.com/rpng/ov_secondary)** - This is an example secondary thread which provides loop
closure in a loosely coupled manner for [OpenVINS](https://github.com/rpng/open_vins). This is a modification of the
code originally developed by the HKUST aerial robotics group and can be found in
their [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) repository. Here we stress that this is a
loosely coupled method, thus no information is returned to the estimator to improve the underlying OpenVINS odometry.
This codebase has been modified in a few key areas including: exposing more loop closure parameters, subscribing to
camera intrinsics, simplifying configuration such that only topics need to be supplied, and some tweaks to the loop
closure detection to improve frequency.
## Demo Videos
<a href="http://www.youtube.com/watch?v=KCX51GvYGss">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/KCX51GvYGss.jpg" width="120" height="90" />
</a>
<a href="http://www.youtube.com/watch?v=Lc7VQHngSuQ">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/Lc7VQHngSuQ.jpg" width="120" height="90" />
</a>
<a href="http://www.youtube.com/watch?v=vaia7iPaRW8">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/vaia7iPaRW8.jpg" width="120" height="90" />
</a>
<a href="http://www.youtube.com/watch?v=MCzTF9ye2zw">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/MCzTF9ye2zw.jpg" width="120" height="90"/>
</a>
<a href="http://www.youtube.com/watch?v=eSQLWcNrx_I">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/eSQLWcNrx_I.jpg" width="120" height="90" />
</a>
<br/>
<a href="http://www.youtube.com/watch?v=187AXuuGNNw">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/187AXuuGNNw.jpg" width="120" height="90" />
</a>
<a href="http://www.youtube.com/watch?v=oUoLlrFryk0">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/oUoLlrFryk0.jpg" width="120" height="90" />
</a>
<a href="http://www.youtube.com/watch?v=ExPIGwORm4E">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/ExPIGwORm4E.jpg" width="120" height="90" />
</a>
<a href="http://www.youtube.com/watch?v=lXHl-qgLGl8">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/lXHl-qgLGl8.jpg" width="120" height="90" />
</a>
## Credit / Licensing
This code was written by the [Robot Perception and Navigation Group (RPNG)](https://sites.udel.edu/robot/) at the
University of Delaware. If you have any issues with the code please open an issue on our github page with relevant
implementation details and references. For researchers that have leveraged or compared to this work, please cite the
following:
```txt
@Conference{Geneva2020ICRA,
Title = {{OpenVINS}: A Research Platform for Visual-Inertial Estimation},
Author = {Patrick Geneva and Kevin Eckenhoff and Woosik Lee and Yulin Yang and Guoquan Huang},
Booktitle = {Proc. of the IEEE International Conference on Robotics and Automation},
Year = {2020},
Address = {Paris, France},
Url = {\url{https://github.com/rpng/open_vins}}
}
```
The codebase and documentation is licensed under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt).
You must preserve the copyright and license notices in your derivative work and make available the complete source code with modifications under the same license ([see this](https://choosealicense.com/licenses/gpl-3.0/); this is not legal advice).
There is a problem with publishing the cubes to two different topics: to the VIO and to the GPS-VIO frames. I have to find the workaround for this problem. For now you can see the AR cubes in the rviz which shows VIO (from ov_msckf directory)
### Saving the rendered AR to image sequnece
1. To the catkin_ws_ov/src directory clone the https://git.drivecast.tech/pi/rviz_camera_stream_ar repository. And build it from catkin_ws_ov directory with `catkin build` command
2. In rviz with running ov_msckf turn on the CameraPub display. The image sequnece will be stored to ~/.ros/ directory

232
cubes_data.txt Normal file
View File

@@ -0,0 +1,232 @@
{
{-5.473, 15.819, 0.000},
{-15.586, 25.062, 0.000},
{-22.395, 43.562, 0.000},
{-36.035, 50.009, 0.000},
{-41.113, 62.946, 0.000},
{-47.675, 62.477, 0.000},
{-56.081, 76.612, 0.000},
{-68.155, 82.382, 0.000},
{-76.903, 97.032, 0.000},
{-93.218, 103.532, 0.000},
{-121.903, 137.353, 0.000},
{-134.567, 140.840, 0.000},
{-158.030, 169.709, 0.000},
{-170.521, 177.370, 0.000},
{-225.440, 236.153, 0.000},
{-259.810, 259.846, 0.000},
{-287.220, 297.381, 0.000},
{-313.876, 312.375, 0.000},
{-446.905, 450.175, 0.000},
{-489.056, 477.187, 0.000},
{-544.828, 555.099, 0.000},
{-568.349, 556.457, 0.000},
{-638.075, 643.542, 0.000},
{-702.377, 676.867, 0.000},
{-753.507, 750.003, 0.000},
{-807.197, 768.580, 0.000},
{-863.780, 829.878, 0.000},
{-877.002, 860.185, 0.000},
{-905.568, 864.266, 0.000},
{-915.407, 887.186, 0.000},
{-931.954, 899.596, 0.000},
{-936.091, 907.870, 0.000},
{-971.656, 952.685, 0.000},
{-1012.476, 947.723, 0.000},
{-1016.629, 985.880, 0.000},
{-1065.223, 987.986, 0.000},
{-1078.723, 1012.979, 0.000},
{-1145.907, 1035.300, 0.000},
{-1234.198, 1116.285, 0.000},
{-1338.651, 1152.282, 0.000},
{-1422.556, 1202.064, 0.000},
{-1488.881, 1208.139, 0.000},
{-1613.822, 1281.229, 0.000},
{-1682.561, 1274.593, 0.000},
{-1927.827, 1384.315, 0.000},
{-2035.692, 1400.027, 0.000},
{-2224.536, 1462.204, 0.000},
{-2313.168, 1458.187, 0.000},
{-2436.409, 1496.333, 0.000},
{-2503.565, 1473.606, 0.000},
{-2605.994, 1481.901, 0.000},
{-2563.683, 1431.523, 0.000},
{-2599.753, 1395.453, 0.000},
{-2679.961, 1407.933, 0.000},
{-2630.649, 1438.753, 0.000},
{-2581.338, 1506.557, 0.000},
{-2528.483, 1197.294, 0.000},
{-2553.878, 1178.248, 0.000},
{-2553.878, 1108.412, 0.000},
{-2450.511, 1023.108, 0.000},
{-2450.511, 967.579, 0.000},
{-2437.486, 890.971, 0.000},
{-2450.511, 819.504, 0.000},
{-2365.134, 690.688, 0.000},
{-2296.590, 541.293, 0.000},
{-2283.679, 620.835, 0.000},
{-2151.879, 318.478, 0.000},
{-2176.409, 287.816, 0.000},
{-2102.820, 251.022, 0.000},
{-2127.350, 189.698, 0.000},
{-2028.905, 122.896, 0.000},
{-2046.821, 66.008, 0.000},
{-1958.025, 1.247, 0.000},
{-1886.644, -52.849, 0.000},
{-1863.596, -64.373, 0.000},
{-1783.239, -134.282, 0.000},
{-1705.906, -151.254, 0.000},
{-1617.356, -206.598, 0.000},
{-1565.032, -222.410, 0.000},
{-1465.740, -282.723, 0.000},
{-1361.237, -309.086, 0.000},
{-1276.829, -355.967, 0.000},
{-1236.832, -350.967, 0.000},
{-1170.082, -368.522, 0.000},
{-1145.877, -386.675, 0.000},
{-1133.776, -374.573, 0.000},
{-1064.985, -381.088, 0.000},
{-1060.602, -409.499, 0.000},
{-943.535, -368.153, 0.000},
{-852.780, -365.980, 0.000},
{-747.917, -375.279, 0.000},
{-694.244, -346.169, 0.000},
{-724.264, -360.724, 0.000},
{-601.465, -334.072, 0.000},
{-557.205, -336.465, 0.000},
{-564.189, -332.895, 0.000},
{-520.468, -340.201, 0.000},
{-476.908, -317.140, 0.000},
{-417.040, -318.781, 0.000},
{-377.842, -296.022, 0.000},
{-309.273, -302.223, 0.000},
{-267.572, -281.072, 0.000},
{-233.495, -287.929, 0.000},
{-211.795, -257.754, 0.000},
{-209.412, -234.544, 0.000},
{-144.978, -252.842, 0.000},
{-86.550, -255.100, 0.000},
{-57.889, -200.404, 0.000},
{-12.395, -168.373, 0.000},
{-8.988, -117.709, 0.000},
{-5.494, -73.048, 0.000},
{12.541, -24.783, 0.000},
{-8.001, 22.391, 0.000},
{-7.999, -85.193, 0.000},
{-61.402, 98.350, 0.000}
}

229
cubes_data_raw.txt Normal file
View File

@@ -0,0 +1,229 @@
[ INFO] [1661681758.542538804]: Setting goal: Frame:global, Position(-5,473, 15,819, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681761.366104427]: Setting goal: Frame:global, Position(-15,586, 25,062, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681763.725180091]: Setting goal: Frame:global, Position(-22,395, 43,562, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681767.070619773]: Setting goal: Frame:global, Position(-36,035, 50,009, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681769.626612777]: Setting goal: Frame:global, Position(-41,113, 62,946, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681772.109520946]: Setting goal: Frame:global, Position(-47,675, 62,477, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681774.112724899]: Setting goal: Frame:global, Position(-56,081, 76,612, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681777.302217201]: Setting goal: Frame:global, Position(-68,155, 82,382, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681779.861960488]: Setting goal: Frame:global, Position(-76,903, 97,032, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681782.800923288]: Setting goal: Frame:global, Position(-93,218, 103,532, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681785.718310396]: Setting goal: Frame:global, Position(-121,903, 137,353, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681787.896060892]: Setting goal: Frame:global, Position(-134,567, 140,840, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681790.588332526]: Setting goal: Frame:global, Position(-158,030, 169,709, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681793.814911800]: Setting goal: Frame:global, Position(-170,521, 177,370, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681797.723063981]: Setting goal: Frame:global, Position(-225,440, 236,153, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681799.701969454]: Setting goal: Frame:global, Position(-259,810, 259,846, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681802.030425810]: Setting goal: Frame:global, Position(-287,220, 297,381, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681803.991512869]: Setting goal: Frame:global, Position(-313,876, 312,375, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681808.052154432]: Setting goal: Frame:global, Position(-446,905, 450,175, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681810.418569814]: Setting goal: Frame:global, Position(-489,056, 477,187, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681812.432212612]: Setting goal: Frame:global, Position(-544,828, 555,099, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681814.265249289]: Setting goal: Frame:global, Position(-568,349, 556,457, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681816.377320247]: Setting goal: Frame:global, Position(-638,075, 643,542, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681818.254672277]: Setting goal: Frame:global, Position(-702,377, 676,867, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681819.856601473]: Setting goal: Frame:global, Position(-753,507, 750,003, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681823.198940409]: Setting goal: Frame:global, Position(-807,197, 768,580, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681826.688902486]: Setting goal: Frame:global, Position(-863,780, 829,878, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681829.149484447]: Setting goal: Frame:global, Position(-877,002, 860,185, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681834.754228929]: Setting goal: Frame:global, Position(-905,568, 864,266, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681836.884455530]: Setting goal: Frame:global, Position(-915,407, 887,186, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681840.089902108]: Setting goal: Frame:global, Position(-931,954, 899,596, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681842.834586387]: Setting goal: Frame:global, Position(-936,091, 907,870, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681864.902074094]: Setting goal: Frame:global, Position(-971,656, 952,685, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661681866.697015512]: Setting goal: Frame:global, Position(-1012,476, 947,723, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632127.852490740]: Setting goal: Frame:global, Position(-1016,629, 985,880, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632132.184678829]: Setting goal: Frame:global, Position(-1065,223, 987,986, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632136.124920122]: Setting goal: Frame:global, Position(-1078,723, 1012,979, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632138.984009160]: Setting goal: Frame:global, Position(-1145,907, 1035,300, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632141.946256550]: Setting goal: Frame:global, Position(-1234,198, 1116,285, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632145.759114091]: Setting goal: Frame:global, Position(-1338,651, 1152,282, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632150.011012866]: Setting goal: Frame:global, Position(-1422,556, 1202,064, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632153.716795166]: Setting goal: Frame:global, Position(-1488,881, 1208,139, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632161.896759115]: Setting goal: Frame:global, Position(-1613,822, 1281,229, 0,000), Orientation(0,000, 0,000, -0,424, 0,906) = Angle: -0,876
[ INFO] [1661632164.281107195]: Setting goal: Frame:global, Position(-1682,561, 1274,593, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632167.458584482]: Setting goal: Frame:global, Position(-1927,827, 1384,315, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632169.913266535]: Setting goal: Frame:global, Position(-2035,692, 1400,027, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632173.307407310]: Setting goal: Frame:global, Position(-2224,536, 1462,204, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632176.351214609]: Setting goal: Frame:global, Position(-2313,168, 1458,187, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632178.555881179]: Setting goal: Frame:global, Position(-2436,409, 1496,333, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632181.142276742]: Setting goal: Frame:global, Position(-2503,565, 1473,606, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632184.598631613]: Setting goal: Frame:global, Position(-2605,994, 1481,901, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632186.871425127]: Setting goal: Frame:global, Position(-2563,683, 1431,523, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632190.036524941]: Setting goal: Frame:global, Position(-2599,753, 1395,453, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632192.308387207]: Setting goal: Frame:global, Position(-2679,961, 1407,933, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632194.107661072]: Setting goal: Frame:global, Position(-2630,649, 1438,753, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632197.652973828]: Setting goal: Frame:global, Position(-2581,338, 1506,557, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632214.582124274]: Setting goal: Frame:global, Position(-2528,483, 1197,294, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632217.238021932]: Setting goal: Frame:global, Position(-2553,878, 1178,248, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632220.141462575]: Setting goal: Frame:global, Position(-2553,878, 1108,412, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632223.218969221]: Setting goal: Frame:global, Position(-2450,511, 1023,108, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632226.331857599]: Setting goal: Frame:global, Position(-2450,511, 967,579, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632228.970472284]: Setting goal: Frame:global, Position(-2437,486, 890,971, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632231.916169640]: Setting goal: Frame:global, Position(-2450,511, 819,504, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632234.836929434]: Setting goal: Frame:global, Position(-2365,134, 690,688, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632237.302645790]: Setting goal: Frame:global, Position(-2296,590, 541,293, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632239.792049222]: Setting goal: Frame:global, Position(-2283,679, 620,835, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632246.224784100]: Setting goal: Frame:global, Position(-2151,879, 318,478, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632250.234365944]: Setting goal: Frame:global, Position(-2176,409, 287,816, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632252.310166616]: Setting goal: Frame:global, Position(-2102,820, 251,022, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632255.149335470]: Setting goal: Frame:global, Position(-2127,350, 189,698, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632258.932289901]: Setting goal: Frame:global, Position(-2028,905, 122,896, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632263.144988177]: Setting goal: Frame:global, Position(-2046,821, 66,008, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632266.190110230]: Setting goal: Frame:global, Position(-1958,025, 1,247, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632269.561415294]: Setting goal: Frame:global, Position(-1886,644, -52,849, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632273.841288193]: Setting goal: Frame:global, Position(-1863,596, -64,373, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632277.266957976]: Setting goal: Frame:global, Position(-1783,239, -134,282, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632280.305223489]: Setting goal: Frame:global, Position(-1705,906, -151,254, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632283.186066798]: Setting goal: Frame:global, Position(-1617,356, -206,598, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632285.362086849]: Setting goal: Frame:global, Position(-1565,032, -222,410, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632290.289428103]: Setting goal: Frame:global, Position(-1465,740, -282,723, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632292.839988655]: Setting goal: Frame:global, Position(-1361,237, -309,086, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632295.635589289]: Setting goal: Frame:global, Position(-1276,829, -355,967, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632298.676040024]: Setting goal: Frame:global, Position(-1236,832, -350,967, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632303.850072944]: Setting goal: Frame:global, Position(-1170,082, -368,522, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632306.229545164]: Setting goal: Frame:global, Position(-1145,877, -386,675, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632309.304212150]: Setting goal: Frame:global, Position(-1133,776, -374,573, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632345.234874674]: Setting goal: Frame:global, Position(-1064,985, -381,088, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632349.971078988]: Setting goal: Frame:global, Position(-1060,602, -409,499, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632353.595991074]: Setting goal: Frame:global, Position(-943,535, -368,153, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632363.489405457]: Setting goal: Frame:global, Position(-852,780, -365,980, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632365.849965890]: Setting goal: Frame:global, Position(-747,917, -375,279, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632368.972748346]: Setting goal: Frame:global, Position(-694,244, -346,169, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632374.466785047]: Setting goal: Frame:global, Position(-724,264, -360,724, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632377.413183395]: Setting goal: Frame:global, Position(-601,465, -334,072, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632386.301568678]: Setting goal: Frame:global, Position(-557,205, -336,465, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632419.258236717]: Setting goal: Frame:global, Position(-564,189, -332,895, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632421.753513455]: Setting goal: Frame:global, Position(-520,468, -340,201, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632427.936695382]: Setting goal: Frame:global, Position(-476,908, -317,140, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632431.928703642]: Setting goal: Frame:global, Position(-417,040, -318,781, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632435.141877704]: Setting goal: Frame:global, Position(-377,842, -296,022, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632439.502306212]: Setting goal: Frame:global, Position(-309,273, -302,223, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632442.564849056]: Setting goal: Frame:global, Position(-267,572, -281,072, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632446.109785919]: Setting goal: Frame:global, Position(-233,495, -287,929, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632450.923839760]: Setting goal: Frame:global, Position(-211,795, -257,754, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632455.337038121]: Setting goal: Frame:global, Position(-209,412, -234,544, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632469.064972823]: Setting goal: Frame:global, Position(-144,978, -252,842, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632473.912780017]: Setting goal: Frame:global, Position(-86,550, -255,100, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632478.449110565]: Setting goal: Frame:global, Position(-57,889, -200,404, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632482.808543684]: Setting goal: Frame:global, Position(-12,395, -168,373, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632486.184607744]: Setting goal: Frame:global, Position(-8,988, -117,709, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632489.869629377]: Setting goal: Frame:global, Position(-5,494, -73,048, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632493.780854085]: Setting goal: Frame:global, Position(12,541, -24,783, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632498.982292442]: Setting goal: Frame:global, Position(-8,001, 22,391, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632503.438900266]: Setting goal: Frame:global, Position(-7,999, -85,193, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000
[ INFO] [1661632508.194052203]: Setting goal: Frame:global, Position(-61,402, 98,350, 0,000), Orientation(0,000, 0,000, 0,000, 1,000) = Angle: 0,000

39
global_fusion/CMakeLists.txt Normal file
View File

@@ -0,0 +1,39 @@
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_DEBUG "-O3 -Wall -g")
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
rosbag
tf
std_msgs
geometry_msgs
sensor_msgs
nav_msgs
)
find_package(Ceres REQUIRED)
add_subdirectory(./Thirdparty/GeographicLib/)
include_directories(
${catkin_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
./Thirdparty/GeographicLib/include/
)
catkin_package()
add_executable(global_fusion_node
src/globalOptNode.cpp
src/globalOpt.cpp
# src/KITTIGPSTest.cpp
)
target_link_libraries(global_fusion_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} libGeographiccc)

36
global_fusion/Thirdparty/GeographicLib/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,36 @@
project (GeographicLib)
# 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
# (7) Set the default "real" precision. This should probably be left
# at 2 (double).
set (GEOGRAPHICLIB_PRECISION 2 CACHE STRING
"Precision: 1 = float, 2 = double, 3 = extended, 4 = quadruple, 5 = variable")
set_property (CACHE GEOGRAPHICLIB_PRECISION PROPERTY STRINGS 1 2 3 4 5)
set (LIBNAME Geographic)
include_directories(
./include/
)
add_library(libGeographiccc src/LocalCartesian.cpp
src/Geocentric.cpp
src/Math.cpp)

12
global_fusion/Thirdparty/GeographicLib/include/Config.h vendored Normal file
View File

@@ -0,0 +1,12 @@
// This will be overwritten by ./configure
#define GEOGRAPHICLIB_VERSION_STRING "1.49"
#define GEOGRAPHICLIB_VERSION_MAJOR 1
#define GEOGRAPHICLIB_VERSION_MINOR 49
#define GEOGRAPHICLIB_VERSION_PATCH 0
// Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler
#define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 1
// Define WORDS_BIGENDIAN to be 1 if your machine is big endian
/* #undef WORDS_BIGENDIAN */

403
global_fusion/Thirdparty/GeographicLib/include/Constants.hpp vendored Normal file
View File

@@ -0,0 +1,403 @@
/**
* \file Constants.hpp
* \brief Header for GeographicLib::Constants class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_CONSTANTS_HPP)
#define GEOGRAPHICLIB_CONSTANTS_HPP 1
#include "Config.h"
/**
* @relates GeographicLib::Constants
* Pack the version components into a single integer. Users should not rely on
* this particular packing of the components of the version number; see the
* documentation for GEOGRAPHICLIB_VERSION, below.
**********************************************************************/
#define GEOGRAPHICLIB_VERSION_NUM(a,b,c) ((((a) * 10000 + (b)) * 100) + (c))
/**
* @relates GeographicLib::Constants
* The version of GeographicLib as a single integer, packed as MMmmmmpp where
* MM is the major version, mmmm is the minor version, and pp is the patch
* level. Users should not rely on this particular packing of the components
* of the version number. Instead they should use a test such as \code
#if GEOGRAPHICLIB_VERSION >= GEOGRAPHICLIB_VERSION_NUM(1,37,0)
...
#endif
* \endcode
**********************************************************************/
#define GEOGRAPHICLIB_VERSION \
GEOGRAPHICLIB_VERSION_NUM(GEOGRAPHICLIB_VERSION_MAJOR, \
GEOGRAPHICLIB_VERSION_MINOR, \
GEOGRAPHICLIB_VERSION_PATCH)
/**
* @relates GeographicLib::Constants
* Is the C++11 static_assert available?
**********************************************************************/
#if !defined(GEOGRAPHICLIB_HAS_STATIC_ASSERT)
# if __cplusplus >= 201103 || defined(__GXX_EXPERIMENTAL_CXX0X__)
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1
# elif defined(_MSC_VER) && _MSC_VER >= 1600
// For reference, here is a table of Visual Studio and _MSC_VER
// correspondences:
//
// _MSC_VER Visual Studio
// 1100 vc5
// 1200 vc6
// 1300 vc7
// 1310 vc7.1 (2003)
// 1400 vc8 (2005)
// 1500 vc9 (2008)
// 1600 vc10 (2010)
// 1700 vc11 (2012)
// 1800 vc12 (2013)
// 1900 vc14 (2015)
// 1910+ vc15 (2017)
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1
# else
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 0
# endif
#endif
/**
* @relates GeographicLib::Constants
* A compile-time assert. Use C++11 static_assert, if available.
**********************************************************************/
#if !defined(GEOGRAPHICLIB_STATIC_ASSERT)
# if GEOGRAPHICLIB_HAS_STATIC_ASSERT
# define GEOGRAPHICLIB_STATIC_ASSERT static_assert
# else
# define GEOGRAPHICLIB_STATIC_ASSERT(cond,reason) \
{ enum{ GEOGRAPHICLIB_STATIC_ASSERT_ENUM = 1/int(cond) }; }
# endif
#endif
#if defined(_MSC_VER) && defined(GEOGRAPHICLIB_SHARED_LIB) && \
GEOGRAPHICLIB_SHARED_LIB
# if GEOGRAPHICLIB_SHARED_LIB > 1
# error GEOGRAPHICLIB_SHARED_LIB must be 0 or 1
# elif defined(GeographicLib_EXPORTS)
# define GEOGRAPHICLIB_EXPORT __declspec(dllexport)
# else
# define GEOGRAPHICLIB_EXPORT __declspec(dllimport)
# endif
#else
# define GEOGRAPHICLIB_EXPORT
#endif
// Use GEOGRAPHICLIB_DEPRECATED to mark functions, types or variables as
// deprecated. Code inspired by Apache Subversion's svn_types.h file (via
// MPFR).
#if defined(__GNUC__)
# if __GNUC__ > 4
# define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated(msg)))
# else
# define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated))
# endif
#elif defined(_MSC_VER) && _MSC_VER >= 1300
# define GEOGRAPHICLIB_DEPRECATED(msg) __declspec(deprecated(msg))
#else
# define GEOGRAPHICLIB_DEPRECATED(msg)
#endif
#include <stdexcept>
#include <string>
#include "Math.hpp"
/**
* \brief Namespace for %GeographicLib
*
* All of %GeographicLib is defined within the GeographicLib namespace. In
* addition all the header files are included via %GeographicLib/Class.hpp.
* This minimizes the likelihood of conflicts with other packages.
**********************************************************************/
namespace GeographicLib {
/**
* \brief %Constants needed by %GeographicLib
*
* Define constants specifying the WGS84 ellipsoid, the UTM and UPS
* projections, and various unit conversions.
*
* Example of use:
* \include example-Constants.cpp
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Constants {
private:
typedef Math::real real;
Constants(); // Disable constructor
public:
/**
* A synonym for Math::degree<real>().
**********************************************************************/
static Math::real degree() { return Math::degree(); }
/**
* @return the number of radians in an arcminute.
**********************************************************************/
static Math::real arcminute()
{ return Math::degree() / 60; }
/**
* @return the number of radians in an arcsecond.
**********************************************************************/
static Math::real arcsecond()
{ return Math::degree() / 3600; }
/** \name Ellipsoid parameters
**********************************************************************/
///@{
/**
* @tparam T the type of the returned value.
* @return the equatorial radius of WGS84 ellipsoid (6378137 m).
**********************************************************************/
template<typename T> static T WGS84_a()
{ return 6378137 * meter<T>(); }
/**
* A synonym for WGS84_a<real>().
**********************************************************************/
static Math::real WGS84_a() { return WGS84_a<real>(); }
/**
* @tparam T the type of the returned value.
* @return the flattening of WGS84 ellipsoid (1/298.257223563).
**********************************************************************/
template<typename T> static T WGS84_f() {
// Evaluating this as 1000000000 / T(298257223563LL) reduces the
// round-off error by about 10%. However, expressing the flattening as
// 1/298.257223563 is well ingrained.
return 1 / ( T(298257223563LL) / 1000000000 );
}
/**
* A synonym for WGS84_f<real>().
**********************************************************************/
static Math::real WGS84_f() { return WGS84_f<real>(); }
/**
* @tparam T the type of the returned value.
* @return the gravitational constant of the WGS84 ellipsoid, \e GM, in
* m<sup>3</sup> s<sup>&minus;2</sup>.
**********************************************************************/
template<typename T> static T WGS84_GM()
{ return T(3986004) * 100000000 + 41800000; }
/**
* A synonym for WGS84_GM<real>().
**********************************************************************/
static Math::real WGS84_GM() { return WGS84_GM<real>(); }
/**
* @tparam T the type of the returned value.
* @return the angular velocity of the WGS84 ellipsoid, &omega;, in rad
* s<sup>&minus;1</sup>.
**********************************************************************/
template<typename T> static T WGS84_omega()
{ return 7292115 / (T(1000000) * 100000); }
/**
* A synonym for WGS84_omega<real>().
**********************************************************************/
static Math::real WGS84_omega() { return WGS84_omega<real>(); }
/**
* @tparam T the type of the returned value.
* @return the equatorial radius of GRS80 ellipsoid, \e a, in m.
**********************************************************************/
template<typename T> static T GRS80_a()
{ return 6378137 * meter<T>(); }
/**
* A synonym for GRS80_a<real>().
**********************************************************************/
static Math::real GRS80_a() { return GRS80_a<real>(); }
/**
* @tparam T the type of the returned value.
* @return the gravitational constant of the GRS80 ellipsoid, \e GM, in
* m<sup>3</sup> s<sup>&minus;2</sup>.
**********************************************************************/
template<typename T> static T GRS80_GM()
{ return T(3986005) * 100000000; }
/**
* A synonym for GRS80_GM<real>().
**********************************************************************/
static Math::real GRS80_GM() { return GRS80_GM<real>(); }
/**
* @tparam T the type of the returned value.
* @return the angular velocity of the GRS80 ellipsoid, &omega;, in rad
* s<sup>&minus;1</sup>.
*
* This is about 2 &pi; 366.25 / (365.25 &times; 24 &times; 3600) rad
* s<sup>&minus;1</sup>. 365.25 is the number of days in a Julian year and
* 365.35/366.25 converts from solar days to sidereal days. Using the
* number of days in a Gregorian year (365.2425) results in a worse
* approximation (because the Gregorian year includes the precession of the
* earth's axis).
**********************************************************************/
template<typename T> static T GRS80_omega()
{ return 7292115 / (T(1000000) * 100000); }
/**
* A synonym for GRS80_omega<real>().
**********************************************************************/
static Math::real GRS80_omega() { return GRS80_omega<real>(); }
/**
* @tparam T the type of the returned value.
* @return the dynamical form factor of the GRS80 ellipsoid,
* <i>J</i><sub>2</sub>.
**********************************************************************/
template<typename T> static T GRS80_J2()
{ return T(108263) / 100000000; }
/**
* A synonym for GRS80_J2<real>().
**********************************************************************/
static Math::real GRS80_J2() { return GRS80_J2<real>(); }
/**
* @tparam T the type of the returned value.
* @return the central scale factor for UTM (0.9996).
**********************************************************************/
template<typename T> static T UTM_k0()
{return T(9996) / 10000; }
/**
* A synonym for UTM_k0<real>().
**********************************************************************/
static Math::real UTM_k0() { return UTM_k0<real>(); }
/**
* @tparam T the type of the returned value.
* @return the central scale factor for UPS (0.994).
**********************************************************************/
template<typename T> static T UPS_k0()
{ return T(994) / 1000; }
/**
* A synonym for UPS_k0<real>().
**********************************************************************/
static Math::real UPS_k0() { return UPS_k0<real>(); }
///@}
/** \name SI units
**********************************************************************/
///@{
/**
* @tparam T the type of the returned value.
* @return the number of meters in a meter.
*
* This is unity, but this lets the internal system of units be changed if
* necessary.
**********************************************************************/
template<typename T> static T meter() { return T(1); }
/**
* A synonym for meter<real>().
**********************************************************************/
static Math::real meter() { return meter<real>(); }
/**
* @return the number of meters in a kilometer.
**********************************************************************/
static Math::real kilometer()
{ return 1000 * meter<real>(); }
/**
* @return the number of meters in a nautical mile (approximately 1 arc
* minute)
**********************************************************************/
static Math::real nauticalmile()
{ return 1852 * meter<real>(); }
/**
* @tparam T the type of the returned value.
* @return the number of square meters in a square meter.
*
* This is unity, but this lets the internal system of units be changed if
* necessary.
**********************************************************************/
template<typename T> static T square_meter()
{ return meter<real>() * meter<real>(); }
/**
* A synonym for square_meter<real>().
**********************************************************************/
static Math::real square_meter()
{ return square_meter<real>(); }
/**
* @return the number of square meters in a hectare.
**********************************************************************/
static Math::real hectare()
{ return 10000 * square_meter<real>(); }
/**
* @return the number of square meters in a square kilometer.
**********************************************************************/
static Math::real square_kilometer()
{ return kilometer() * kilometer(); }
/**
* @return the number of square meters in a square nautical mile.
**********************************************************************/
static Math::real square_nauticalmile()
{ return nauticalmile() * nauticalmile(); }
///@}
/** \name Anachronistic British units
**********************************************************************/
///@{
/**
* @return the number of meters in an international foot.
**********************************************************************/
static Math::real foot()
{ return real(254 * 12) / 10000 * meter<real>(); }
/**
* @return the number of meters in a yard.
**********************************************************************/
static Math::real yard() { return 3 * foot(); }
/**
* @return the number of meters in a fathom.
**********************************************************************/
static Math::real fathom() { return 2 * yard(); }
/**
* @return the number of meters in a chain.
**********************************************************************/
static Math::real chain() { return 22 * yard(); }
/**
* @return the number of meters in a furlong.
**********************************************************************/
static Math::real furlong() { return 10 * chain(); }
/**
* @return the number of meters in a statute mile.
**********************************************************************/
static Math::real mile() { return 8 * furlong(); }
/**
* @return the number of square meters in an acre.
**********************************************************************/
static Math::real acre() { return chain() * furlong(); }
/**
* @return the number of square meters in a square statute mile.
**********************************************************************/
static Math::real square_mile() { return mile() * mile(); }
///@}
/** \name Anachronistic US units
**********************************************************************/
///@{
/**
* @return the number of meters in a US survey foot.
**********************************************************************/
static Math::real surveyfoot()
{ return real(1200) / 3937 * meter<real>(); }
///@}
};
/**
* \brief Exception handling for %GeographicLib
*
* A class to handle exceptions. It's derived from std::runtime_error so it
* can be caught by the usual catch clauses.
*
* Example of use:
* \include example-GeographicErr.cpp
**********************************************************************/
class GeographicErr : public std::runtime_error {
public:
/**
* Constructor
*
* @param[in] msg a string message, which is accessible in the catch
* clause via what().
**********************************************************************/
GeographicErr(const std::string& msg) : std::runtime_error(msg) {}
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_CONSTANTS_HPP

267
global_fusion/Thirdparty/GeographicLib/include/Geocentric.hpp vendored Normal file
View File

@@ -0,0 +1,267 @@
/**
* \file Geocentric.hpp
* \brief Header for GeographicLib::Geocentric class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_GEOCENTRIC_HPP)
#define GEOGRAPHICLIB_GEOCENTRIC_HPP 1
#include <vector>
#include "Constants.hpp"
namespace GeographicLib {
/**
* \brief %Geocentric coordinates
*
* Convert between geodetic coordinates latitude = \e lat, longitude = \e
* lon, height = \e h (measured vertically from the surface of the ellipsoid)
* to geocentric coordinates (\e X, \e Y, \e Z). The origin of geocentric
* coordinates is at the center of the earth. The \e Z axis goes thru the
* north pole, \e lat = 90&deg;. The \e X axis goes thru \e lat = 0,
* \e lon = 0. %Geocentric coordinates are also known as earth centered,
* earth fixed (ECEF) coordinates.
*
* The conversion from geographic to geocentric coordinates is
* straightforward. For the reverse transformation we use
* - H. Vermeille,
* <a href="https://doi.org/10.1007/s00190-002-0273-6"> Direct
* transformation from geocentric coordinates to geodetic coordinates</a>,
* J. Geodesy 76, 451--454 (2002).
* .
* Several changes have been made to ensure that the method returns accurate
* results for all finite inputs (even if \e h is infinite). The changes are
* described in Appendix B of
* - C. F. F. Karney,
* <a href="https://arxiv.org/abs/1102.1215v1">Geodesics
* on an ellipsoid of revolution</a>,
* Feb. 2011;
* preprint
* <a href="https://arxiv.org/abs/1102.1215v1">arxiv:1102.1215v1</a>.
* .
* Vermeille similarly updated his method in
* - H. Vermeille,
* <a href="https://doi.org/10.1007/s00190-010-0419-x">
* An analytical method to transform geocentric into
* geodetic coordinates</a>, J. Geodesy 85, 105--117 (2011).
* .
* See \ref geocentric for more information.
*
* The errors in these routines are close to round-off. Specifically, for
* points within 5000 km of the surface of the ellipsoid (either inside or
* outside the ellipsoid), the error is bounded by 7 nm (7 nanometers) for
* the WGS84 ellipsoid. See \ref geocentric for further information on the
* errors.
*
* Example of use:
* \include example-Geocentric.cpp
*
* <a href="CartConvert.1.html">CartConvert</a> is a command-line utility
* providing access to the functionality of Geocentric and LocalCartesian.
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Geocentric {
private:
typedef Math::real real;
friend class LocalCartesian;
friend class MagneticCircle; // MagneticCircle uses Rotation
friend class MagneticModel; // MagneticModel uses IntForward
friend class GravityCircle; // GravityCircle uses Rotation
friend class GravityModel; // GravityModel uses IntForward
friend class NormalGravity; // NormalGravity uses IntForward
static const size_t dim_ = 3;
static const size_t dim2_ = dim_ * dim_;
real _a, _f, _e2, _e2m, _e2a, _e4a, _maxrad;
static void Rotation(real sphi, real cphi, real slam, real clam,
real M[dim2_]);
static void Rotate(real M[dim2_], real x, real y, real z,
real& X, real& Y, real& Z) {
// Perform [X,Y,Z]^t = M.[x,y,z]^t
// (typically local cartesian to geocentric)
X = M[0] * x + M[1] * y + M[2] * z;
Y = M[3] * x + M[4] * y + M[5] * z;
Z = M[6] * x + M[7] * y + M[8] * z;
}
static void Unrotate(real M[dim2_], real X, real Y, real Z,
real& x, real& y, real& z) {
// Perform [x,y,z]^t = M^t.[X,Y,Z]^t
// (typically geocentric to local cartesian)
x = M[0] * X + M[3] * Y + M[6] * Z;
y = M[1] * X + M[4] * Y + M[7] * Z;
z = M[2] * X + M[5] * Y + M[8] * Z;
}
void IntForward(real lat, real lon, real h, real& X, real& Y, real& Z,
real M[dim2_]) const;
void IntReverse(real X, real Y, real Z, real& lat, real& lon, real& h,
real M[dim2_]) const;
public:
/**
* Constructor for a ellipsoid with
*
* @param[in] a equatorial radius (meters).
* @param[in] f flattening of ellipsoid. Setting \e f = 0 gives a sphere.
* Negative \e f gives a prolate ellipsoid.
* @exception GeographicErr if \e a or (1 &minus; \e f) \e a is not
* positive.
**********************************************************************/
Geocentric(real a, real f);
/**
* A default constructor (for use by NormalGravity).
**********************************************************************/
Geocentric() : _a(-1) {}
/**
* Convert from geodetic to geocentric coordinates.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] X geocentric coordinate (meters).
* @param[out] Y geocentric coordinate (meters).
* @param[out] Z geocentric coordinate (meters).
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Forward(real lat, real lon, real h, real& X, real& Y, real& Z)
const {
if (Init())
IntForward(lat, lon, h, X, Y, Z, NULL);
}
/**
* Convert from geodetic to geocentric coordinates and return rotation
* matrix.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] X geocentric coordinate (meters).
* @param[out] Y geocentric coordinate (meters).
* @param[out] Z geocentric coordinate (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in geocentric \e X, \e Y, \e Z coordinates; call this representation
* \e v0.
* .
* Then we have \e v0 = \e M &sdot; \e v1.
**********************************************************************/
void Forward(real lat, real lon, real h, real& X, real& Y, real& Z,
std::vector<real>& M)
const {
if (!Init())
return;
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntForward(lat, lon, h, X, Y, Z, t);
std::copy(t, t + dim2_, M.begin());
} else
IntForward(lat, lon, h, X, Y, Z, NULL);
}
/**
* Convert from geocentric to geodetic to coordinates.
*
* @param[in] X geocentric coordinate (meters).
* @param[in] Y geocentric coordinate (meters).
* @param[in] Z geocentric coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
*
* In general there are multiple solutions and the result which maximizes
* \e h is returned. If there are still multiple solutions with different
* latitudes (applies only if \e Z = 0), then the solution with \e lat > 0
* is returned. If there are still multiple solutions with different
* longitudes (applies only if \e X = \e Y = 0) then \e lon = 0 is
* returned. The value of \e h returned satisfies \e h &ge; &minus; \e a
* (1 &minus; <i>e</i><sup>2</sup>) / sqrt(1 &minus; <i>e</i><sup>2</sup>
* sin<sup>2</sup>\e lat). The value of \e lon returned is in the range
* [&minus;180&deg;, 180&deg;].
**********************************************************************/
void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h)
const {
if (Init())
IntReverse(X, Y, Z, lat, lon, h, NULL);
}
/**
* Convert from geocentric to geodetic to coordinates.
*
* @param[in] X geocentric coordinate (meters).
* @param[in] Y geocentric coordinate (meters).
* @param[in] Z geocentric coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in geocentric \e X, \e Y, \e Z coordinates; call this representation
* \e v0.
* .
* Then we have \e v1 = <i>M</i><sup>T</sup> &sdot; \e v0, where
* <i>M</i><sup>T</sup> is the transpose of \e M.
**********************************************************************/
void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h,
std::vector<real>& M)
const {
if (!Init())
return;
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntReverse(X, Y, Z, lat, lon, h, t);
std::copy(t, t + dim2_, M.begin());
} else
IntReverse(X, Y, Z, lat, lon, h, NULL);
}
/** \name Inspector functions
**********************************************************************/
///@{
/**
* @return true if the object has been initialized.
**********************************************************************/
bool Init() const { return _a > 0; }
/**
* @return \e a the equatorial radius of the ellipsoid (meters). This is
* the value used in the constructor.
**********************************************************************/
Math::real MajorRadius() const
{ return Init() ? _a : Math::NaN(); }
/**
* @return \e f the flattening of the ellipsoid. This is the
* value used in the constructor.
**********************************************************************/
Math::real Flattening() const
{ return Init() ? _f : Math::NaN(); }
///@}
/**
* A global instantiation of Geocentric with the parameters for the WGS84
* ellipsoid.
**********************************************************************/
static const Geocentric& WGS84();
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_GEOCENTRIC_HPP

236
global_fusion/Thirdparty/GeographicLib/include/LocalCartesian.hpp vendored Normal file
View File

@@ -0,0 +1,236 @@
/**
* \file LocalCartesian.hpp
* \brief Header for GeographicLib::LocalCartesian class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_LOCALCARTESIAN_HPP)
#define GEOGRAPHICLIB_LOCALCARTESIAN_HPP 1
#include "Geocentric.hpp"
#include "Constants.hpp"
namespace GeographicLib {
/**
* \brief Local cartesian coordinates
*
* Convert between geodetic coordinates latitude = \e lat, longitude = \e
* lon, height = \e h (measured vertically from the surface of the ellipsoid)
* to local cartesian coordinates (\e x, \e y, \e z). The origin of local
* cartesian coordinate system is at \e lat = \e lat0, \e lon = \e lon0, \e h
* = \e h0. The \e z axis is normal to the ellipsoid; the \e y axis points
* due north. The plane \e z = - \e h0 is tangent to the ellipsoid.
*
* The conversions all take place via geocentric coordinates using a
* Geocentric object (by default Geocentric::WGS84()).
*
* Example of use:
* \include example-LocalCartesian.cpp
*
* <a href="CartConvert.1.html">CartConvert</a> is a command-line utility
* providing access to the functionality of Geocentric and LocalCartesian.
**********************************************************************/
class GEOGRAPHICLIB_EXPORT LocalCartesian {
private:
typedef Math::real real;
static const size_t dim_ = 3;
static const size_t dim2_ = dim_ * dim_;
Geocentric _earth;
real _lat0, _lon0, _h0;
real _x0, _y0, _z0, _r[dim2_];
void IntForward(real lat, real lon, real h, real& x, real& y, real& z,
real M[dim2_]) const;
void IntReverse(real x, real y, real z, real& lat, real& lon, real& h,
real M[dim2_]) const;
void MatrixMultiply(real M[dim2_]) const;
public:
/**
* Constructor setting the origin.
*
* @param[in] lat0 latitude at origin (degrees).
* @param[in] lon0 longitude at origin (degrees).
* @param[in] h0 height above ellipsoid at origin (meters); default 0.
* @param[in] earth Geocentric object for the transformation; default
* Geocentric::WGS84().
*
* \e lat0 should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
LocalCartesian(real lat0, real lon0, real h0 = 0,
const Geocentric& earth = Geocentric::WGS84())
: _earth(earth)
{ Reset(lat0, lon0, h0); }
/**
* Default constructor.
*
* @param[in] earth Geocentric object for the transformation; default
* Geocentric::WGS84().
*
* Sets \e lat0 = 0, \e lon0 = 0, \e h0 = 0.
**********************************************************************/
explicit LocalCartesian(const Geocentric& earth = Geocentric::WGS84())
: _earth(earth)
{ Reset(real(0), real(0), real(0)); }
/**
* Reset the origin.
*
* @param[in] lat0 latitude at origin (degrees).
* @param[in] lon0 longitude at origin (degrees).
* @param[in] h0 height above ellipsoid at origin (meters); default 0.
*
* \e lat0 should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Reset(real lat0, real lon0, real h0 = 0);
/**
* Convert from geodetic to local cartesian coordinates.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] x local cartesian coordinate (meters).
* @param[out] y local cartesian coordinate (meters).
* @param[out] z local cartesian coordinate (meters).
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Forward(real lat, real lon, real h, real& x, real& y, real& z)
const {
IntForward(lat, lon, h, x, y, z, NULL);
}
/**
* Convert from geodetic to local cartesian coordinates and return rotation
* matrix.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] x local cartesian coordinate (meters).
* @param[out] y local cartesian coordinate (meters).
* @param[out] z local cartesian coordinate (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in \e x, \e y, \e z coordinates (where the components are relative to
* the local coordinate system at (\e lat0, \e lon0, \e h0)); call this
* representation \e v0.
* .
* Then we have \e v0 = \e M &sdot; \e v1.
**********************************************************************/
void Forward(real lat, real lon, real h, real& x, real& y, real& z,
std::vector<real>& M)
const {
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntForward(lat, lon, h, x, y, z, t);
std::copy(t, t + dim2_, M.begin());
} else
IntForward(lat, lon, h, x, y, z, NULL);
}
/**
* Convert from local cartesian to geodetic coordinates.
*
* @param[in] x local cartesian coordinate (meters).
* @param[in] y local cartesian coordinate (meters).
* @param[in] z local cartesian coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
*
* The value of \e lon returned is in the range [&minus;180&deg;,
* 180&deg;].
**********************************************************************/
void Reverse(real x, real y, real z, real& lat, real& lon, real& h)
const {
IntReverse(x, y, z, lat, lon, h, NULL);
}
/**
* Convert from local cartesian to geodetic coordinates and return rotation
* matrix.
*
* @param[in] x local cartesian coordinate (meters).
* @param[in] y local cartesian coordinate (meters).
* @param[in] z local cartesian coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in \e x, \e y, \e z coordinates (where the components are relative to
* the local coordinate system at (\e lat0, \e lon0, \e h0)); call this
* representation \e v0.
* .
* Then we have \e v1 = <i>M</i><sup>T</sup> &sdot; \e v0, where
* <i>M</i><sup>T</sup> is the transpose of \e M.
**********************************************************************/
void Reverse(real x, real y, real z, real& lat, real& lon, real& h,
std::vector<real>& M)
const {
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntReverse(x, y, z, lat, lon, h, t);
std::copy(t, t + dim2_, M.begin());
} else
IntReverse(x, y, z, lat, lon, h, NULL);
}
/** \name Inspector functions
**********************************************************************/
///@{
/**
* @return latitude of the origin (degrees).
**********************************************************************/
Math::real LatitudeOrigin() const { return _lat0; }
/**
* @return longitude of the origin (degrees).
**********************************************************************/
Math::real LongitudeOrigin() const { return _lon0; }
/**
* @return height of the origin (meters).
**********************************************************************/
Math::real HeightOrigin() const { return _h0; }
/**
* @return \e a the equatorial radius of the ellipsoid (meters). This is
* the value of \e a inherited from the Geocentric object used in the
* constructor.
**********************************************************************/
Math::real MajorRadius() const { return _earth.MajorRadius(); }
/**
* @return \e f the flattening of the ellipsoid. This is the value
* inherited from the Geocentric object used in the constructor.
**********************************************************************/
Math::real Flattening() const { return _earth.Flattening(); }
///@}
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_LOCALCARTESIAN_HPP

945
global_fusion/Thirdparty/GeographicLib/include/Math.hpp vendored Normal file
View File

@@ -0,0 +1,945 @@
/**
* \file Math.hpp
* \brief Header for GeographicLib::Math class
*
* Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
// Constants.hpp includes Math.hpp. Place this include outside Math.hpp's
// include guard to enforce this ordering.
#include "Constants.hpp"
#if !defined(GEOGRAPHICLIB_MATH_HPP)
#define GEOGRAPHICLIB_MATH_HPP 1
/**
* Are C++11 math functions available?
**********************************************************************/
#if !defined(GEOGRAPHICLIB_CXX11_MATH)
// Recent versions of g++ -std=c++11 (4.7 and later?) set __cplusplus to 201103
// and support the new C++11 mathematical functions, std::atanh, etc. However
// the Android toolchain, which uses g++ -std=c++11 (4.8 as of 2014-03-11,
// according to Pullan Lu), does not support std::atanh. Android toolchains
// might define __ANDROID__ or ANDROID; so need to check both. With OSX the
// version is GNUC version 4.2 and __cplusplus is set to 201103, so remove the
// version check on GNUC.
# if defined(__GNUC__) && __cplusplus >= 201103 && \
!(defined(__ANDROID__) || defined(ANDROID) || defined(__CYGWIN__))
# define GEOGRAPHICLIB_CXX11_MATH 1
// Visual C++ 12 supports these functions
# elif defined(_MSC_VER) && _MSC_VER >= 1800
# define GEOGRAPHICLIB_CXX11_MATH 1
# else
# define GEOGRAPHICLIB_CXX11_MATH 0
# endif
#endif
#if !defined(GEOGRAPHICLIB_WORDS_BIGENDIAN)
# define GEOGRAPHICLIB_WORDS_BIGENDIAN 0
#endif
#if !defined(GEOGRAPHICLIB_HAVE_LONG_DOUBLE)
# define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 0
#endif
#if !defined(GEOGRAPHICLIB_PRECISION)
/**
* The precision of floating point numbers used in %GeographicLib. 1 means
* float (single precision); 2 (the default) means double; 3 means long double;
* 4 is reserved for quadruple precision. Nearly all the testing has been
* carried out with doubles and that's the recommended configuration. In order
* for long double to be used, GEOGRAPHICLIB_HAVE_LONG_DOUBLE needs to be
* defined. Note that with Microsoft Visual Studio, long double is the same as
* double.
**********************************************************************/
# define GEOGRAPHICLIB_PRECISION 2
#endif
#include <cmath>
#include <algorithm>
#include <limits>
#if GEOGRAPHICLIB_PRECISION == 4
#include <boost/version.hpp>
#if BOOST_VERSION >= 105600
#include <boost/cstdfloat.hpp>
#endif
#include <boost/multiprecision/float128.hpp>
#include <boost/math/special_functions.hpp>
__float128 fmaq(__float128, __float128, __float128);
#elif GEOGRAPHICLIB_PRECISION == 5
#include <mpreal.h>
#endif
#if GEOGRAPHICLIB_PRECISION > 3
// volatile keyword makes no sense for multiprec types
#define GEOGRAPHICLIB_VOLATILE
// Signal a convergence failure with multiprec types by throwing an exception
// at loop exit.
#define GEOGRAPHICLIB_PANIC \
(throw GeographicLib::GeographicErr("Convergence failure"), false)
#else
#define GEOGRAPHICLIB_VOLATILE volatile
// Ignore convergence failures with standard floating points types by allowing
// loop to exit cleanly.
#define GEOGRAPHICLIB_PANIC false
#endif
namespace GeographicLib {
/**
* \brief Mathematical functions needed by %GeographicLib
*
* Define mathematical functions in order to localize system dependencies and
* to provide generic versions of the functions. In addition define a real
* type to be used by %GeographicLib.
*
* Example of use:
* \include example-Math.cpp
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Math {
private:
void dummy() {
GEOGRAPHICLIB_STATIC_ASSERT(GEOGRAPHICLIB_PRECISION >= 1 &&
GEOGRAPHICLIB_PRECISION <= 5,
"Bad value of precision");
}
Math(); // Disable constructor
public:
#if GEOGRAPHICLIB_HAVE_LONG_DOUBLE
/**
* The extended precision type for real numbers, used for some testing.
* This is long double on computers with this type; otherwise it is double.
**********************************************************************/
typedef long double extended;
#else
typedef double extended;
#endif
#if GEOGRAPHICLIB_PRECISION == 2
/**
* The real type for %GeographicLib. Nearly all the testing has been done
* with \e real = double. However, the algorithms should also work with
* float and long double (where available). (<b>CAUTION</b>: reasonable
* accuracy typically cannot be obtained using floats.)
**********************************************************************/
typedef double real;
#elif GEOGRAPHICLIB_PRECISION == 1
typedef float real;
#elif GEOGRAPHICLIB_PRECISION == 3
typedef extended real;
#elif GEOGRAPHICLIB_PRECISION == 4
typedef boost::multiprecision::float128 real;
#elif GEOGRAPHICLIB_PRECISION == 5
typedef mpfr::mpreal real;
#else
typedef double real;
#endif
/**
* @return the number of bits of precision in a real number.
**********************************************************************/
static int digits() {
#if GEOGRAPHICLIB_PRECISION != 5
return std::numeric_limits<real>::digits;
#else
return std::numeric_limits<real>::digits();
#endif
}
/**
* Set the binary precision of a real number.
*
* @param[in] ndigits the number of bits of precision.
* @return the resulting number of bits of precision.
*
* This only has an effect when GEOGRAPHICLIB_PRECISION = 5. See also
* Utility::set_digits for caveats about when this routine should be
* called.
**********************************************************************/
static int set_digits(int ndigits) {
#if GEOGRAPHICLIB_PRECISION != 5
(void)ndigits;
#else
mpfr::mpreal::set_default_prec(ndigits >= 2 ? ndigits : 2);
#endif
return digits();
}
/**
* @return the number of decimal digits of precision in a real number.
**********************************************************************/
static int digits10() {
#if GEOGRAPHICLIB_PRECISION != 5
return std::numeric_limits<real>::digits10;
#else
return std::numeric_limits<real>::digits10();
#endif
}
/**
* Number of additional decimal digits of precision for real relative to
* double (0 for float).
**********************************************************************/
static int extra_digits() {
return
digits10() > std::numeric_limits<double>::digits10 ?
digits10() - std::numeric_limits<double>::digits10 : 0;
}
/**
* true if the machine is big-endian.
**********************************************************************/
static const bool bigendian = GEOGRAPHICLIB_WORDS_BIGENDIAN;
/**
* @tparam T the type of the returned value.
* @return &pi;.
**********************************************************************/
template<typename T> static T pi() {
using std::atan2;
static const T pi = atan2(T(0), T(-1));
return pi;
}
/**
* A synonym for pi<real>().
**********************************************************************/
static real pi() { return pi<real>(); }
/**
* @tparam T the type of the returned value.
* @return the number of radians in a degree.
**********************************************************************/
template<typename T> static T degree() {
static const T degree = pi<T>() / 180;
return degree;
}
/**
* A synonym for degree<real>().
**********************************************************************/
static real degree() { return degree<real>(); }
/**
* Square a number.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return <i>x</i><sup>2</sup>.
**********************************************************************/
template<typename T> static T sq(T x)
{ return x * x; }
/**
* The hypotenuse function avoiding underflow and overflow.
*
* @tparam T the type of the arguments and the returned value.
* @param[in] x
* @param[in] y
* @return sqrt(<i>x</i><sup>2</sup> + <i>y</i><sup>2</sup>).
**********************************************************************/
template<typename T> static T hypot(T x, T y) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::hypot; return hypot(x, y);
#else
using std::abs; using std::sqrt;
x = abs(x); y = abs(y);
if (x < y) std::swap(x, y); // Now x >= y >= 0
y /= (x ? x : 1);
return x * sqrt(1 + y * y);
// For an alternative (square-root free) method see
// C. Moler and D. Morrision (1983) https://doi.org/10.1147/rd.276.0577
// and A. A. Dubrulle (1983) https://doi.org/10.1147/rd.276.0582
#endif
}
/**
* exp(\e x) &minus; 1 accurate near \e x = 0.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return exp(\e x) &minus; 1.
**********************************************************************/
template<typename T> static T expm1(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::expm1; return expm1(x);
#else
using std::exp; using std::abs; using std::log;
GEOGRAPHICLIB_VOLATILE T
y = exp(x),
z = y - 1;
// The reasoning here is similar to that for log1p. The expression
// mathematically reduces to exp(x) - 1, and the factor z/log(y) = (y -
// 1)/log(y) is a slowly varying quantity near y = 1 and is accurately
// computed.
return abs(x) > 1 ? z : (z == 0 ? x : x * z / log(y));
#endif
}
/**
* log(1 + \e x) accurate near \e x = 0.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return log(1 + \e x).
**********************************************************************/
template<typename T> static T log1p(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::log1p; return log1p(x);
#else
using std::log;
GEOGRAPHICLIB_VOLATILE T
y = 1 + x,
z = y - 1;
// Here's the explanation for this magic: y = 1 + z, exactly, and z
// approx x, thus log(y)/z (which is nearly constant near z = 0) returns
// a good approximation to the true log(1 + x)/x. The multiplication x *
// (log(y)/z) introduces little additional error.
return z == 0 ? x : x * log(y) / z;
#endif
}
/**
* The inverse hyperbolic sine function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return asinh(\e x).
**********************************************************************/
template<typename T> static T asinh(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::asinh; return asinh(x);
#else
using std::abs; T y = abs(x); // Enforce odd parity
y = log1p(y * (1 + y/(hypot(T(1), y) + 1)));
return x < 0 ? -y : y;
#endif
}
/**
* The inverse hyperbolic tangent function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return atanh(\e x).
**********************************************************************/
template<typename T> static T atanh(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::atanh; return atanh(x);
#else
using std::abs; T y = abs(x); // Enforce odd parity
y = log1p(2 * y/(1 - y))/2;
return x < 0 ? -y : y;
#endif
}
/**
* The cube root function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return the real cube root of \e x.
**********************************************************************/
template<typename T> static T cbrt(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::cbrt; return cbrt(x);
#else
using std::abs; using std::pow;
T y = pow(abs(x), 1/T(3)); // Return the real cube root
return x < 0 ? -y : y;
#endif
}
/**
* Fused multiply and add.
*
* @tparam T the type of the arguments and the returned value.
* @param[in] x
* @param[in] y
* @param[in] z
* @return <i>xy</i> + <i>z</i>, correctly rounded (on those platforms with
* support for the <code>fma</code> instruction).
*
* On platforms without the <code>fma</code> instruction, no attempt is
* made to improve on the result of a rounded multiplication followed by a
* rounded addition.
**********************************************************************/
template<typename T> static T fma(T x, T y, T z) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::fma; return fma(x, y, z);
#else
return x * y + z;
#endif
}
/**
* Normalize a two-vector.
*
* @tparam T the type of the argument and the returned value.
* @param[in,out] x on output set to <i>x</i>/hypot(<i>x</i>, <i>y</i>).
* @param[in,out] y on output set to <i>y</i>/hypot(<i>x</i>, <i>y</i>).
**********************************************************************/
template<typename T> static void norm(T& x, T& y)
{ T h = hypot(x, y); x /= h; y /= h; }
/**
* The error-free sum of two numbers.
*
* @tparam T the type of the argument and the returned value.
* @param[in] u
* @param[in] v
* @param[out] t the exact error given by (\e u + \e v) - \e s.
* @return \e s = round(\e u + \e v).
*
* See D. E. Knuth, TAOCP, Vol 2, 4.2.2, Theorem B. (Note that \e t can be
* the same as one of the first two arguments.)
**********************************************************************/
template<typename T> static T sum(T u, T v, T& t) {
GEOGRAPHICLIB_VOLATILE T s = u + v;
GEOGRAPHICLIB_VOLATILE T up = s - v;
GEOGRAPHICLIB_VOLATILE T vpp = s - up;
up -= u;
vpp -= v;
t = -(up + vpp);
// u + v = s + t
// = round(u + v) + t
return s;
}
/**
* Evaluate a polynomial.
*
* @tparam T the type of the arguments and returned value.
* @param[in] N the order of the polynomial.
* @param[in] p the coefficient array (of size \e N + 1).
* @param[in] x the variable.
* @return the value of the polynomial.
*
* Evaluate <i>y</i> = &sum;<sub><i>n</i>=0..<i>N</i></sub>
* <i>p</i><sub><i>n</i></sub> <i>x</i><sup><i>N</i>&minus;<i>n</i></sup>.
* Return 0 if \e N &lt; 0. Return <i>p</i><sub>0</sub>, if \e N = 0 (even
* if \e x is infinite or a nan). The evaluation uses Horner's method.
**********************************************************************/
template<typename T> static T polyval(int N, const T p[], T x)
// This used to employ Math::fma; but that's too slow and it seemed not to
// improve the accuracy noticeably. This might change when there's direct
// hardware support for fma.
{ T y = N < 0 ? 0 : *p++; while (--N >= 0) y = y * x + *p++; return y; }
/**
* Normalize an angle.
*
* @tparam T the type of the argument and returned value.
* @param[in] x the angle in degrees.
* @return the angle reduced to the range([&minus;180&deg;, 180&deg;].
*
* The range of \e x is unrestricted.
**********************************************************************/
template<typename T> static T AngNormalize(T x) {
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4
using std::remainder;
x = remainder(x, T(360)); return x != -180 ? x : 180;
#else
using std::fmod;
T y = fmod(x, T(360));
#if defined(_MSC_VER) && _MSC_VER < 1900
// Before version 14 (2015), Visual Studio had problems dealing
// with -0.0. Specifically
// VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0
// sincosd has a similar fix.
// python 2.7 on Windows 32-bit machines has the same problem.
if (x == 0) y = x;
#endif
return y <= -180 ? y + 360 : (y <= 180 ? y : y - 360);
#endif
}
/**
* Normalize a latitude.
*
* @tparam T the type of the argument and returned value.
* @param[in] x the angle in degrees.
* @return x if it is in the range [&minus;90&deg;, 90&deg;], otherwise
* return NaN.
**********************************************************************/
template<typename T> static T LatFix(T x)
{ using std::abs; return abs(x) > 90 ? NaN<T>() : x; }
/**
* The exact difference of two angles reduced to
* (&minus;180&deg;, 180&deg;].
*
* @tparam T the type of the arguments and returned value.
* @param[in] x the first angle in degrees.
* @param[in] y the second angle in degrees.
* @param[out] e the error term in degrees.
* @return \e d, the truncated value of \e y &minus; \e x.
*
* This computes \e z = \e y &minus; \e x exactly, reduced to
* (&minus;180&deg;, 180&deg;]; and then sets \e z = \e d + \e e where \e d
* is the nearest representable number to \e z and \e e is the truncation
* error. If \e d = &minus;180, then \e e &gt; 0; If \e d = 180, then \e e
* &le; 0.
**********************************************************************/
template<typename T> static T AngDiff(T x, T y, T& e) {
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4
using std::remainder;
T t, d = AngNormalize(sum(remainder(-x, T(360)),
remainder( y, T(360)), t));
#else
T t, d = AngNormalize(sum(AngNormalize(-x), AngNormalize(y), t));
#endif
// Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
// abs(t) <= eps (eps = 2^-45 for doubles). The only case where the
// addition of t takes the result outside the range (-180,180] is d = 180
// and t > 0. The case, d = -180 + eps, t = -eps, can't happen, since
// sum would have returned the exact result in such a case (i.e., given t
// = 0).
return sum(d == 180 && t > 0 ? -180 : d, t, e);
}
/**
* Difference of two angles reduced to [&minus;180&deg;, 180&deg;]
*
* @tparam T the type of the arguments and returned value.
* @param[in] x the first angle in degrees.
* @param[in] y the second angle in degrees.
* @return \e y &minus; \e x, reduced to the range [&minus;180&deg;,
* 180&deg;].
*
* The result is equivalent to computing the difference exactly, reducing
* it to (&minus;180&deg;, 180&deg;] and rounding the result. Note that
* this prescription allows &minus;180&deg; to be returned (e.g., if \e x
* is tiny and negative and \e y = 180&deg;).
**********************************************************************/
template<typename T> static T AngDiff(T x, T y)
{ T e; return AngDiff(x, y, e); }
/**
* Coarsen a value close to zero.
*
* @tparam T the type of the argument and returned value.
* @param[in] x
* @return the coarsened value.
*
* The makes the smallest gap in \e x = 1/16 - nextafter(1/16, 0) =
* 1/2<sup>57</sup> for reals = 0.7 pm on the earth if \e x is an angle in
* degrees. (This is about 1000 times more resolution than we get with
* angles around 90&deg;.) We use this to avoid having to deal with near
* singular cases when \e x is non-zero but tiny (e.g.,
* 10<sup>&minus;200</sup>). This converts -0 to +0; however tiny negative
* numbers get converted to -0.
**********************************************************************/
template<typename T> static T AngRound(T x) {
using std::abs;
static const T z = 1/T(16);
if (x == 0) return 0;
GEOGRAPHICLIB_VOLATILE T y = abs(x);
// The compiler mustn't "simplify" z - (z - y) to y
y = y < z ? z - (z - y) : y;
return x < 0 ? -y : y;
}
/**
* Evaluate the sine and cosine function with the argument in degrees
*
* @tparam T the type of the arguments.
* @param[in] x in degrees.
* @param[out] sinx sin(<i>x</i>).
* @param[out] cosx cos(<i>x</i>).
*
* The results obey exactly the elementary properties of the trigonometric
* functions, e.g., sin 9&deg; = cos 81&deg; = &minus; sin 123456789&deg;.
* If x = &minus;0, then \e sinx = &minus;0; this is the only case where
* &minus;0 is returned.
**********************************************************************/
template<typename T> static void sincosd(T x, T& sinx, T& cosx) {
// In order to minimize round-off errors, this function exactly reduces
// the argument to the range [-45, 45] before converting it to radians.
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
// Disable for gcc because of bug in glibc version < 2.22, see
// https://sourceware.org/bugzilla/show_bug.cgi?id=17569
// Once this fix is widely deployed, should insert a runtime test for the
// glibc version number. For example
// #include <gnu/libc-version.h>
// std::string version(gnu_get_libc_version()); => "2.22"
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
// Possibly could call the gnu extension sincos
T s = sin(r), c = cos(r);
#if defined(_MSC_VER) && _MSC_VER < 1900
// Before version 14 (2015), Visual Studio had problems dealing
// with -0.0. Specifically
// VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0
// VC 12 and 64-bit compile: sin(-0.0) -> +0.0
// AngNormalize has a similar fix.
// python 2.7 on Windows 32-bit machines has the same problem.
if (x == 0) s = x;
#endif
switch (unsigned(q) & 3U) {
case 0U: sinx = s; cosx = c; break;
case 1U: sinx = c; cosx = -s; break;
case 2U: sinx = -s; cosx = -c; break;
default: sinx = -c; cosx = s; break; // case 3U
}
// Set sign of 0 results. -0 only produced for sin(-0)
if (x != 0) { sinx += T(0); cosx += T(0); }
}
/**
* Evaluate the sine function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return sin(<i>x</i>).
**********************************************************************/
template<typename T> static T sind(T x) {
// See sincosd
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
unsigned p = unsigned(q);
r = p & 1U ? cos(r) : sin(r);
if (p & 2U) r = -r;
if (x != 0) r += T(0);
return r;
}
/**
* Evaluate the cosine function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return cos(<i>x</i>).
**********************************************************************/
template<typename T> static T cosd(T x) {
// See sincosd
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
unsigned p = unsigned(q + 1);
r = p & 1U ? cos(r) : sin(r);
if (p & 2U) r = -r;
return T(0) + r;
}
/**
* Evaluate the tangent function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return tan(<i>x</i>).
*
* If \e x = &plusmn;90&deg;, then a suitably large (but finite) value is
* returned.
**********************************************************************/
template<typename T> static T tand(T x) {
static const T overflow = 1 / sq(std::numeric_limits<T>::epsilon());
T s, c;
sincosd(x, s, c);
return c != 0 ? s / c : (s < 0 ? -overflow : overflow);
}
/**
* Evaluate the atan2 function with the result in degrees
*
* @tparam T the type of the arguments and the returned value.
* @param[in] y
* @param[in] x
* @return atan2(<i>y</i>, <i>x</i>) in degrees.
*
* The result is in the range (&minus;180&deg; 180&deg;]. N.B.,
* atan2d(&plusmn;0, &minus;1) = +180&deg;; atan2d(&minus;&epsilon;,
* &minus;1) = &minus;180&deg;, for &epsilon; positive and tiny;
* atan2d(&plusmn;0, +1) = &plusmn;0&deg;.
**********************************************************************/
template<typename T> static T atan2d(T y, T x) {
// In order to minimize round-off errors, this function rearranges the
// arguments so that result of atan2 is in the range [-pi/4, pi/4] before
// converting it to degrees and mapping the result to the correct
// quadrant.
using std::atan2; using std::abs;
int q = 0;
if (abs(y) > abs(x)) { std::swap(x, y); q = 2; }
if (x < 0) { x = -x; ++q; }
// here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4]
T ang = atan2(y, x) / degree();
switch (q) {
// Note that atan2d(-0.0, 1.0) will return -0. However, we expect that
// atan2d will not be called with y = -0. If need be, include
//
// case 0: ang = 0 + ang; break;
//
// and handle mpfr as in AngRound.
case 1: ang = (y >= 0 ? 180 : -180) - ang; break;
case 2: ang = 90 - ang; break;
case 3: ang = -90 + ang; break;
}
return ang;
}
/**
* Evaluate the atan function with the result in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return atan(<i>x</i>) in degrees.
**********************************************************************/
template<typename T> static T atand(T x)
{ return atan2d(x, T(1)); }
/**
* Evaluate <i>e</i> atanh(<i>e x</i>)
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return <i>e</i> atanh(<i>e x</i>)
*
* If <i>e</i><sup>2</sup> is negative (<i>e</i> is imaginary), the
* expression is evaluated in terms of atan.
**********************************************************************/
template<typename T> static T eatanhe(T x, T es);
/**
* Copy the sign.
*
* @tparam T the type of the argument.
* @param[in] x gives the magitude of the result.
* @param[in] y gives the sign of the result.
* @return value with the magnitude of \e x and with the sign of \e y.
*
* This routine correctly handles the case \e y = &minus;0, returning
* &minus|<i>x</i>|.
**********************************************************************/
template<typename T> static T copysign(T x, T y) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::copysign; return copysign(x, y);
#else
using std::abs;
// NaN counts as positive
return abs(x) * (y < 0 || (y == 0 && 1/y < 0) ? -1 : 1);
#endif
}
/**
* tan&chi; in terms of tan&phi;
*
* @tparam T the type of the argument and the returned value.
* @param[in] tau &tau; = tan&phi;
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return &tau;&prime; = tan&chi;
*
* See Eqs. (7--9) of
* C. F. F. Karney,
* <a href="https://doi.org/10.1007/s00190-011-0445-3">
* Transverse Mercator with an accuracy of a few nanometers,</a>
* J. Geodesy 85(8), 475--485 (Aug. 2011)
* (preprint
* <a href="https://arxiv.org/abs/1002.1417">arXiv:1002.1417</a>).
**********************************************************************/
template<typename T> static T taupf(T tau, T es);
/**
* tan&phi; in terms of tan&chi;
*
* @tparam T the type of the argument and the returned value.
* @param[in] taup &tau;&prime; = tan&chi;
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return &tau; = tan&phi;
*
* See Eqs. (19--21) of
* C. F. F. Karney,
* <a href="https://doi.org/10.1007/s00190-011-0445-3">
* Transverse Mercator with an accuracy of a few nanometers,</a>
* J. Geodesy 85(8), 475--485 (Aug. 2011)
* (preprint
* <a href="https://arxiv.org/abs/1002.1417">arXiv:1002.1417</a>).
**********************************************************************/
template<typename T> static T tauf(T taup, T es);
/**
* Test for finiteness.
*
* @tparam T the type of the argument.
* @param[in] x
* @return true if number is finite, false if NaN or infinite.
**********************************************************************/
template<typename T> static bool isfinite(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::isfinite; return isfinite(x);
#else
using std::abs;
#if defined(_MSC_VER)
return abs(x) <= (std::numeric_limits<T>::max)();
#else
// There's a problem using MPFR C++ 3.6.3 and g++ -std=c++14 (reported on
// 2015-05-04) with the parens around std::numeric_limits<T>::max. Of
// course, these parens are only needed to deal with Windows stupidly
// defining max as a macro. So don't insert the parens on non-Windows
// platforms.
return abs(x) <= std::numeric_limits<T>::max();
#endif
#endif
}
/**
* The NaN (not a number)
*
* @tparam T the type of the returned value.
* @return NaN if available, otherwise return the max real of type T.
**********************************************************************/
template<typename T> static T NaN() {
#if defined(_MSC_VER)
return std::numeric_limits<T>::has_quiet_NaN ?
std::numeric_limits<T>::quiet_NaN() :
(std::numeric_limits<T>::max)();
#else
return std::numeric_limits<T>::has_quiet_NaN ?
std::numeric_limits<T>::quiet_NaN() :
std::numeric_limits<T>::max();
#endif
}
/**
* A synonym for NaN<real>().
**********************************************************************/
static real NaN() { return NaN<real>(); }
/**
* Test for NaN.
*
* @tparam T the type of the argument.
* @param[in] x
* @return true if argument is a NaN.
**********************************************************************/
template<typename T> static bool isnan(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::isnan; return isnan(x);
#else
return x != x;
#endif
}
/**
* Infinity
*
* @tparam T the type of the returned value.
* @return infinity if available, otherwise return the max real.
**********************************************************************/
template<typename T> static T infinity() {
#if defined(_MSC_VER)
return std::numeric_limits<T>::has_infinity ?
std::numeric_limits<T>::infinity() :
(std::numeric_limits<T>::max)();
#else
return std::numeric_limits<T>::has_infinity ?
std::numeric_limits<T>::infinity() :
std::numeric_limits<T>::max();
#endif
}
/**
* A synonym for infinity<real>().
**********************************************************************/
static real infinity() { return infinity<real>(); }
/**
* Swap the bytes of a quantity
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return x with its bytes swapped.
**********************************************************************/
template<typename T> static T swab(T x) {
union {
T r;
unsigned char c[sizeof(T)];
} b;
b.r = x;
for (int i = sizeof(T)/2; i--; )
std::swap(b.c[i], b.c[sizeof(T) - 1 - i]);
return b.r;
}
#if GEOGRAPHICLIB_PRECISION == 4
typedef boost::math::policies::policy
< boost::math::policies::domain_error
<boost::math::policies::errno_on_error>,
boost::math::policies::pole_error
<boost::math::policies::errno_on_error>,
boost::math::policies::overflow_error
<boost::math::policies::errno_on_error>,
boost::math::policies::evaluation_error
<boost::math::policies::errno_on_error> >
boost_special_functions_policy;
static real hypot(real x, real y)
{ return boost::math::hypot(x, y, boost_special_functions_policy()); }
static real expm1(real x)
{ return boost::math::expm1(x, boost_special_functions_policy()); }
static real log1p(real x)
{ return boost::math::log1p(x, boost_special_functions_policy()); }
static real asinh(real x)
{ return boost::math::asinh(x, boost_special_functions_policy()); }
static real atanh(real x)
{ return boost::math::atanh(x, boost_special_functions_policy()); }
static real cbrt(real x)
{ return boost::math::cbrt(x, boost_special_functions_policy()); }
static real fma(real x, real y, real z)
{ return fmaq(__float128(x), __float128(y), __float128(z)); }
static real copysign(real x, real y)
{ return boost::math::copysign(x, y); }
static bool isnan(real x) { return boost::math::isnan(x); }
static bool isfinite(real x) { return boost::math::isfinite(x); }
#endif
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_MATH_HPP

172
global_fusion/Thirdparty/GeographicLib/src/Geocentric.cpp vendored Normal file
View File

@@ -0,0 +1,172 @@
/**
* \file Geocentric.cpp
* \brief Implementation for GeographicLib::Geocentric class
*
* Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "Geocentric.hpp"
namespace GeographicLib {
using namespace std;
Geocentric::Geocentric(real a, real f)
: _a(a)
, _f(f)
, _e2(_f * (2 - _f))
, _e2m(Math::sq(1 - _f)) // 1 - _e2
, _e2a(abs(_e2))
, _e4a(Math::sq(_e2))
, _maxrad(2 * _a / numeric_limits<real>::epsilon())
{
if (!(Math::isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(Math::isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
}
const Geocentric& Geocentric::WGS84() {
static const Geocentric wgs84(Constants::WGS84_a(), Constants::WGS84_f());
return wgs84;
}
void Geocentric::IntForward(real lat, real lon, real h,
real& X, real& Y, real& Z,
real M[dim2_]) const {
real sphi, cphi, slam, clam;
Math::sincosd(Math::LatFix(lat), sphi, cphi);
Math::sincosd(lon, slam, clam);
real n = _a/sqrt(1 - _e2 * Math::sq(sphi));
Z = (_e2m * n + h) * sphi;
X = (n + h) * cphi;
Y = X * slam;
X *= clam;
if (M)
Rotation(sphi, cphi, slam, clam, M);
}
void Geocentric::IntReverse(real X, real Y, real Z,
real& lat, real& lon, real& h,
real M[dim2_]) const {
real
R = Math::hypot(X, Y),
slam = R != 0 ? Y / R : 0,
clam = R != 0 ? X / R : 1;
h = Math::hypot(R, Z); // Distance to center of earth
real sphi, cphi;
if (h > _maxrad) {
// We really far away (> 12 million light years); treat the earth as a
// point and h, above, is an acceptable approximation to the height.
// This avoids overflow, e.g., in the computation of disc below. It's
// possible that h has overflowed to inf; but that's OK.
//
// Treat the case X, Y finite, but R overflows to +inf by scaling by 2.
R = Math::hypot(X/2, Y/2);
slam = R != 0 ? (Y/2) / R : 0;
clam = R != 0 ? (X/2) / R : 1;
real H = Math::hypot(Z/2, R);
sphi = (Z/2) / H;
cphi = R / H;
} else if (_e4a == 0) {
// Treat the spherical case. Dealing with underflow in the general case
// with _e2 = 0 is difficult. Origin maps to N pole same as with
// ellipsoid.
real H = Math::hypot(h == 0 ? 1 : Z, R);
sphi = (h == 0 ? 1 : Z) / H;
cphi = R / H;
h -= _a;
} else {
// Treat prolate spheroids by swapping R and Z here and by switching
// the arguments to phi = atan2(...) at the end.
real
p = Math::sq(R / _a),
q = _e2m * Math::sq(Z / _a),
r = (p + q - _e4a) / 6;
if (_f < 0) swap(p, q);
if ( !(_e4a * q == 0 && r <= 0) ) {
real
// Avoid possible division by zero when r = 0 by multiplying
// equations for s and t by r^3 and r, resp.
S = _e4a * p * q / 4, // S = r^3 * s
r2 = Math::sq(r),
r3 = r * r2,
disc = S * (2 * r3 + S);
real u = r;
if (disc >= 0) {
real T3 = S + r3;
// Pick the sign on the sqrt to maximize abs(T3). This minimizes
// loss of precision due to cancellation. The result is unchanged
// because of the way the T is used in definition of u.
T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
// N.B. cbrt always returns the real root. cbrt(-8) = -2.
real T = Math::cbrt(T3); // T = r * t
// T can be zero; but then r2 / T -> 0.
u += T + (T != 0 ? r2 / T : 0);
} else {
// T is complex, but the way u is defined the result is real.
real ang = atan2(sqrt(-disc), -(S + r3));
// There are three possible cube roots. We choose the root which
// avoids cancellation. Note that disc < 0 implies that r < 0.
u += 2 * r * cos(ang / 3);
}
real
v = sqrt(Math::sq(u) + _e4a * q), // guaranteed positive
// Avoid loss of accuracy when u < 0. Underflow doesn't occur in
// e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive
// Need to guard against w going negative due to roundoff in uv - q.
w = max(real(0), _e2a * (uv - q) / (2 * v)),
// Rearrange expression for k to avoid loss of accuracy due to
// subtraction. Division by 0 not possible because uv > 0, w >= 0.
k = uv / (sqrt(uv + Math::sq(w)) + w),
k1 = _f >= 0 ? k : k - _e2,
k2 = _f >= 0 ? k + _e2 : k,
d = k1 * R / k2,
H = Math::hypot(Z/k1, R/k2);
sphi = (Z/k1) / H;
cphi = (R/k2) / H;
h = (1 - _e2m/k1) * Math::hypot(d, Z);
} else { // e4 * q == 0 && r <= 0
// This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
// (prolate, rotation axis) and the generation of 0/0 in the general
// formulas for phi and h. using the general formula and division by 0
// in formula for h. So handle this case by taking the limits:
// f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p)
// f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
real
zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
xx = sqrt( _f < 0 ? _e4a - p : p ),
H = Math::hypot(zz, xx);
sphi = zz / H;
cphi = xx / H;
if (Z < 0) sphi = -sphi; // for tiny negative Z (not for prolate)
h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
}
}
lat = Math::atan2d(sphi, cphi);
lon = Math::atan2d(slam, clam);
if (M)
Rotation(sphi, cphi, slam, clam, M);
}
void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
real M[dim2_]) {
// This rotation matrix is given by the following quaternion operations
// qrot(lam, [0,0,1]) * qrot(phi, [0,-1,0]) * [1,1,1,1]/2
// or
// qrot(pi/2 + lam, [0,0,1]) * qrot(-pi/2 + phi , [-1,0,0])
// where
// qrot(t,v) = [cos(t/2), sin(t/2)*v[1], sin(t/2)*v[2], sin(t/2)*v[3]]
// Local X axis (east) in geocentric coords
M[0] = -slam; M[3] = clam; M[6] = 0;
// Local Y axis (north) in geocentric coords
M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
// Local Z axis (up) in geocentric coords
M[2] = clam * cphi; M[5] = slam * cphi; M[8] = sphi;
}
} // namespace GeographicLib

62
global_fusion/Thirdparty/GeographicLib/src/LocalCartesian.cpp vendored Normal file
View File

@@ -0,0 +1,62 @@
/**
* \file LocalCartesian.cpp
* \brief Implementation for GeographicLib::LocalCartesian class
*
* Copyright (c) Charles Karney (2008-2015) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "LocalCartesian.hpp"
namespace GeographicLib {
using namespace std;
void LocalCartesian::Reset(real lat0, real lon0, real h0) {
_lat0 = Math::LatFix(lat0);
_lon0 = Math::AngNormalize(lon0);
_h0 = h0;
_earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0);
real sphi, cphi, slam, clam;
Math::sincosd(_lat0, sphi, cphi);
Math::sincosd(_lon0, slam, clam);
Geocentric::Rotation(sphi, cphi, slam, clam, _r);
}
void LocalCartesian::MatrixMultiply(real M[dim2_]) const {
// M = r' . M
real t[dim2_];
copy(M, M + dim2_, t);
for (size_t i = 0; i < dim2_; ++i) {
size_t row = i / dim_, col = i % dim_;
M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6];
}
}
void LocalCartesian::IntForward(real lat, real lon, real h,
real& x, real& y, real& z,
real M[dim2_]) const {
real xc, yc, zc;
_earth.IntForward(lat, lon, h, xc, yc, zc, M);
xc -= _x0; yc -= _y0; zc -= _z0;
x = _r[0] * xc + _r[3] * yc + _r[6] * zc;
y = _r[1] * xc + _r[4] * yc + _r[7] * zc;
z = _r[2] * xc + _r[5] * yc + _r[8] * zc;
if (M)
MatrixMultiply(M);
}
void LocalCartesian::IntReverse(real x, real y, real z,
real& lat, real& lon, real& h,
real M[dim2_]) const {
real
xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z,
yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z,
zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z;
_earth.IntReverse(xc, yc, zc, lat, lon, h, M);
if (M)
MatrixMultiply(M);
}
} // namespace GeographicLib

63
global_fusion/Thirdparty/GeographicLib/src/Math.cpp vendored Normal file
View File

@@ -0,0 +1,63 @@
/**
* \file Math.cpp
* \brief Implementation for GeographicLib::Math class
*
* Copyright (c) Charles Karney (2015) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "Math.hpp"
#if defined(_MSC_VER)
// Squelch warnings about constant conditional expressions
# pragma warning (disable: 4127)
#endif
namespace GeographicLib {
using namespace std;
template<typename T> T Math::eatanhe(T x, T es) {
return es > T(0) ? es * atanh(es * x) : -es * atan(es * x);
}
template<typename T> T Math::taupf(T tau, T es) {
T tau1 = hypot(T(1), tau),
sig = sinh( eatanhe(tau / tau1, es ) );
return hypot(T(1), sig) * tau - sig * tau1;
}
template<typename T> T Math::tauf(T taup, T es) {
static const int numit = 5;
static const T tol = sqrt(numeric_limits<T>::epsilon()) / T(10);
T e2m = T(1) - sq(es),
// To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use
// tau = taup/_e2m as a starting guess. (This starting guess is the
// geocentric latitude which, to first order in the flattening, is equal
// to the conformal latitude.) Only 1 iteration is needed for |lat| <
// 3.35 deg, otherwise 2 iterations are needed. If, instead, tau = taup
// is used the mean number of iterations increases to 1.99 (2 iterations
// are needed except near tau = 0).
tau = taup/e2m,
stol = tol * max(T(1), abs(taup));
// min iterations = 1, max iterations = 2; mean = 1.94
for (int i = 0; i < numit || GEOGRAPHICLIB_PANIC; ++i) {
T taupa = taupf(tau, es),
dtau = (taup - taupa) * (1 + e2m * sq(tau)) /
( e2m * hypot(T(1), tau) * hypot(T(1), taupa) );
tau += dtau;
if (!(abs(dtau) >= stol))
break;
}
return tau;
}
/// \cond SKIP
// Instantiate
template Math::real Math::eatanhe<Math::real>(Math::real, Math::real);
template Math::real Math::taupf<Math::real>(Math::real, Math::real);
template Math::real Math::tauf<Math::real>(Math::real, Math::real);
/// \endcond
} // namespace GeographicLib

872298
global_fusion/data/vio_global.csv Normal file
View File

File diff suppressed because it is too large Load Diff

207
global_fusion/launch/display.rviz Normal file
View File

@@ -0,0 +1,207 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Marker1
- /Marker1/Status1
- /Path1
- /Camera1
Splitter Ratio: 0.5
Tree Height: 322
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Camera
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
cam0:
Value: true
cam0_gps:
Value: true
cam1:
Value: true
global:
Value: true
gps_global:
Value: true
imu:
Value: true
imu_gps:
Value: true
Marker Alpha: 1
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
gps_global:
imu_gps:
cam0_gps:
{}
Update Interval: 0
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /ov_msckf/vis_markers
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /global_fusion_node/global_path
Unreliable: false
Value: true
- Class: rviz/Camera
Enabled: true
Image Rendering: background and overlay
Image Topic: /global_fusion_node/sensor_left
Name: Camera
Overlay Alpha: 0.5
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Visibility:
Grid: true
Marker: true
Path: true
TF: true
Value: true
Zoom Factor: 1
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: gps_global
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1631.015869140625
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.0853980779647827
Target Frame: <Fixed Frame>
Yaw: 5.583582878112793
Saved: ~
Window Geometry:
Camera:
collapsed: false
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000003ae00000358fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000001ce000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000212000001840000001600ffffff000000010000010f00000358fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000358000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000027e00fffffffb0000000800540069006d00650100000000000004500000000000000000000003cc0000035800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: 0
Y: 27

6
global_fusion/launch/global_fusion.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<node name="global_fusion_node" pkg="global_fusion" type="global_fusion_node" output="screen" required="true">
<remap from="/gps" to="/gps/fix"/>
<remap from="/vins_estimator/odometry" to="/ov_msckf/odomimugps"/>
</node>
</launch>

6
global_fusion/launch/global_fusion_kitti.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<node name="global_fusion_node" pkg="global_fusion" type="global_fusion_node" output="screen" required="true">
<remap from="/gps" to="/kitti/oxts/gps/fix"/>
<remap from="/vins_estimator/odometry" to="/ov_msckf/loop_pose"/>
</node>
</launch>

38
global_fusion/package.xml Normal file
View File

@@ -0,0 +1,38 @@
<?xml version="1.0"?>
<package format="3">
<!-- Package Information -->
<name>global_fusion</name>
<version>1.0.0</version>
<description>
Package to perform global fusion.
</description>
<url type="website">https://docs.openvins.com/</url>
<url type="bugtracker">https://github.com/rpng/open_vins/issues</url>
<url type="repository">https://github.com/rpng/open_vins</url>
<!-- Code Authors -->
<author email="pgeneva@udel.edu">Patrick Geneva</author>
<maintainer email="pgeneva@udel.edu">Patrick Geneva</maintainer>
<!-- Licensing -->
<license>GNU General Public License v3.0</license>
<!-- ROS1: Dependencies needed to compile this package. -->
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<depend condition="$ROS_VERSION == 1">cmake_modules</depend>
<depend condition="$ROS_VERSION == 1">roscpp</depend>
<depend condition="$ROS_VERSION == 1">rosbag</depend>
<depend condition="$ROS_VERSION == 1">tf</depend>
<depend condition="$ROS_VERSION == 1">std_msgs</depend>
<!-- ROS2: Dependencies needed to compile this package. -->
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<!-- Note the export is required to expose the executables -->
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

122
global_fusion/src/Factors.h Normal file
View File

@@ -0,0 +1,122 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#ifndef GLOBAL_FUSION_FACTORS_H
#define GLOBAL_FUSION_FACTORS_H
#endif //GLOBAL_FUSION_FACTORS_H
#pragma once
#include <ceres/ceres.h>
#include <ceres/rotation.h>
template <typename T> inline
void QuaternionInverse(const T q[4], T q_inverse[4])
{
q_inverse[0] = q[0];
q_inverse[1] = -q[1];
q_inverse[2] = -q[2];
q_inverse[3] = -q[3];
};
struct TError
{
TError(double t_x, double t_y, double t_z, double var)
:t_x(t_x), t_y(t_y), t_z(t_z), var(var){}
template <typename T>
bool operator()(const T* tj, T* residuals) const
{
residuals[0] = (tj[0] - T(t_x)) / T(var);
residuals[1] = (tj[1] - T(t_y)) / T(var);
residuals[2] = (tj[2] - T(t_z)) / T(var);
return true;
}
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var)
{
return (new ceres::AutoDiffCostFunction<
TError, 3, 3>(
new TError(t_x, t_y, t_z, var)));
}
double t_x, t_y, t_z, var;
};
struct RelativeRTError
{
RelativeRTError(double t_x, double t_y, double t_z,
double q_w, double q_x, double q_y, double q_z,
double t_var, double q_var)
:t_x(t_x), t_y(t_y), t_z(t_z),
q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),
t_var(t_var), q_var(q_var){}
template <typename T>
bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const
{
T t_w_ij[3];
t_w_ij[0] = tj[0] - ti[0];
t_w_ij[1] = tj[1] - ti[1];
t_w_ij[2] = tj[2] - ti[2];
T i_q_w[4];
QuaternionInverse(w_q_i, i_q_w);
T t_i_ij[3];
ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);
residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);
residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);
residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);
T relative_q[4];
relative_q[0] = T(q_w);
relative_q[1] = T(q_x);
relative_q[2] = T(q_y);
relative_q[3] = T(q_z);
T q_i_j[4];
ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);
T relative_q_inv[4];
QuaternionInverse(relative_q, relative_q_inv);
T error_q[4];
ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q);
residuals[3] = T(2) * error_q[1] / T(q_var);
residuals[4] = T(2) * error_q[2] / T(q_var);
residuals[5] = T(2) * error_q[3] / T(q_var);
return true;
}
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
const double q_w, const double q_x, const double q_y, const double q_z,
const double t_var, const double q_var)
{
return (new ceres::AutoDiffCostFunction<
RelativeRTError, 6, 4, 3, 4, 3>(
new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));
}
double t_x, t_y, t_z, t_norm;
double q_w, q_x, q_y, q_z;
double t_var, q_var;
};

188
global_fusion/src/KITTIGPSTest.cpp Normal file
View File

@@ -0,0 +1,188 @@
//
// Created by admin1 on 27.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include <iostream>
#include <stdio.h>
#include <cmath>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
using namespace std;
ros::Publisher pubGPS;
int main(int argc, char** argv)
{
ros::init(argc, argv, "kitti_glob");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
pubGPS = n.advertise<sensor_msgs::NavSatFix>("/gps", 1000);
if(argc != 3)
{
printf("please intput: rosrun vins kitti_gps_test [config file] [data folder] \n"
"for example: rosrun vins kitti_gps_test "
"~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml "
"/media/tony-ws1/disk_D/kitti/2011_10_03/2011_10_03_drive_0027_sync/ \n");
return 1;
}
string config_file = argv[1];
printf("config_file: %s\n", argv[1]);
string sequence = argv[2];
printf("read sequence: %s\n", argv[2]);
string dataPath = sequence + "/";
// load image list
FILE* file;
file = std::fopen((dataPath + "image_00/timestamps.txt").c_str() , "r");
if(file == NULL){
printf("cannot find file: %simage_00/timestamps.txt \n", dataPath.c_str());
ROS_BREAK();
return 0;
}
vector<double> imageTimeList;
int year, month, day;
int hour, minute;
double second;
while (fscanf(file, "%d-%d-%d %d:%d:%lf", &year, &month, &day, &hour, &minute, &second) != EOF)
{
//printf("%lf\n", second);
imageTimeList.push_back(hour * 60 * 60 + minute * 60 + second);
}
std::fclose(file);
// load gps list
vector<double> GPSTimeList;
{
FILE* file;
file = std::fopen((dataPath + "oxts/timestamps.txt").c_str() , "r");
if(file == NULL){
printf("cannot find file: %soxts/timestamps.txt \n", dataPath.c_str());
ROS_BREAK();
return 0;
}
int year, month, day;
int hour, minute;
double second;
while (fscanf(file, "%d-%d-%d %d:%d:%lf", &year, &month, &day, &hour, &minute, &second) != EOF)
{
//printf("%lf\n", second);
GPSTimeList.push_back(hour * 60 * 60 + minute * 60 + second);
}
std::fclose(file);
}
readParameters(config_file);
estimator.setParameter();
registerPub(n);
FILE* outFile;
outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");
if(outFile == NULL)
printf("Output path dosen't exist: %s\n", OUTPUT_FOLDER.c_str());
string leftImagePath, rightImagePath;
cv::Mat imLeft, imRight;
double baseTime;
for (size_t i = 0; i < imageTimeList.size(); i++)
{
if(ros::ok())
{
if(imageTimeList[0] < GPSTimeList[0])
baseTime = imageTimeList[0];
else
baseTime = GPSTimeList[0];
//printf("base time is %f\n", baseTime);
printf("process image %d\n", (int)i);
stringstream ss;
ss << setfill('0') << setw(10) << i;
leftImagePath = dataPath + "image_00/data/" + ss.str() + ".png";
rightImagePath = dataPath + "image_01/data/" + ss.str() + ".png";
printf("%s\n", leftImagePath.c_str() );
printf("%s\n", rightImagePath.c_str() );
imLeft = cv::imread(leftImagePath, cv::IMREAD_GRAYSCALE );
imRight = cv::imread(rightImagePath, cv::IMREAD_GRAYSCALE );
double imgTime = imageTimeList[i] - baseTime;
// load gps
FILE* GPSFile;
string GPSFilePath = dataPath + "oxts/data/" + ss.str() + ".txt";
GPSFile = std::fopen(GPSFilePath.c_str() , "r");
if(GPSFile == NULL){
printf("cannot find file: %s\n", GPSFilePath.c_str());
ROS_BREAK();
return 0;
}
double lat, lon, alt, roll, pitch, yaw;
double vn, ve, vf, vl, vu;
double ax, ay, az, af, al, au;
double wx, wy, wz, wf, wl, wu;
double pos_accuracy, vel_accuracy;
double navstat, numsats;
double velmode, orimode;
fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &lat, &lon, &alt, &roll, &pitch, &yaw);
//printf("lat:%lf lon:%lf alt:%lf roll:%lf pitch:%lf yaw:%lf \n", lat, lon, alt, roll, pitch, yaw);
fscanf(GPSFile, "%lf %lf %lf %lf %lf ", &vn, &ve, &vf, &vl, &vu);
//printf("vn:%lf ve:%lf vf:%lf vl:%lf vu:%lf \n", vn, ve, vf, vl, vu);
fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &ax, &ay, &az, &af, &al, &au);
//printf("ax:%lf ay:%lf az:%lf af:%lf al:%lf au:%lf\n", ax, ay, az, af, al, au);
fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &wx, &wy, &wz, &wf, &wl, &wu);
//printf("wx:%lf wy:%lf wz:%lf wf:%lf wl:%lf wu:%lf\n", wx, wy, wz, wf, wl, wu);
fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &pos_accuracy, &vel_accuracy, &navstat, &numsats, &velmode, &orimode);
//printf("pos_accuracy:%lf vel_accuracy:%lf navstat:%lf numsats:%lf velmode:%lf orimode:%lf\n",
// pos_accuracy, vel_accuracy, navstat, numsats, velmode, orimode);
std::fclose(GPSFile);
sensor_msgs::NavSatFix gps_position;
gps_position.header.frame_id = "NED";
gps_position.header.stamp = ros::Time(imgTime);
gps_position.status.status = navstat;
gps_position.status.service = numsats;
gps_position.latitude = lat;
gps_position.longitude = lon;
gps_position.altitude = alt;
gps_position.position_covariance[0] = pos_accuracy;
//printf("pos_accuracy %f \n", pos_accuracy);
pubGPS.publish(gps_position);
estimator.inputImage(imgTime, imLeft, imRight);
Eigen::Matrix<double, 4, 4> pose;
estimator.getPoseInWorldFrame(pose);
if(outFile != NULL)
fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f \n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),
pose(1,0), pose(1,1), pose(1,2),pose(1,3),
pose(2,0), pose(2,1), pose(2,2),pose(2,3));
// cv::imshow("leftImage", imLeft);
// cv::imshow("rightImage", imRight);
// cv::waitKey(2);
}
else
break;
}
if(outFile != NULL)
fclose (outFile);
return 0;
}

281
global_fusion/src/globalOpt.cpp Normal file
View File

@@ -0,0 +1,281 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include "globalOpt.h"
#include <utility>
#include "Factors.h"
GlobalOptimization::GlobalOptimization() {
initGPS = false;
newGPS = false;
// I suppose the matrix which transforms the gps coordinates to vio (or reverse)
WGPS_T_WVIO = Eigen::Matrix4d::Identity();
threadOpt = std::thread(&GlobalOptimization::optimize, this);
}
GlobalOptimization::~GlobalOptimization() {
threadOpt.detach();
}
void GlobalOptimization::GPS2XYZ(double latitude, double longitude, double altitude, double* xyz){
if (!initGPS)
{
geoConverter.Reset(latitude, longitude, altitude);
initGPS = true;
}
geoConverter.Forward(latitude, longitude, altitude, xyz[0], xyz[1], xyz[2]);
}
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ){
mPoseMap.lock();
vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
// std::cout << "inputOdom tCov.size() " << tCov.size() << std::endl;
// localTranslationCovariance[t] = tCov;
// localRotationCovariance[t] = rCov;
Eigen::Quaterniond globalQ;
globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
globalPoseMap[t] = globalPose;
lastP = globalP;
lastQ = globalQ;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(t);
pose_stamped.header.frame_id = "gps_global";
pose_stamped.pose.position.x = lastP.x();
pose_stamped.pose.position.y = lastP.y();
pose_stamped.pose.position.z = lastP.z();
pose_stamped.pose.orientation.x = lastQ.x();
pose_stamped.pose.orientation.y = lastQ.y();
pose_stamped.pose.orientation.z = lastQ.z();
pose_stamped.pose.orientation.w = lastQ.w();
global_path.header = pose_stamped.header;
global_path.poses.push_back(pose_stamped);
mPoseMap.unlock();
};
void GlobalOptimization::getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ){
odomP = lastP;
odomQ = lastQ;
}
void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy){
double xyz[3];
GPS2XYZ(latitude, longitude, altitude, xyz);
vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
//printf("new gps: t: %f x: %f y: %f z:%f \n", t, tmp[0], tmp[1], tmp[2]);
GPSPositionMap[t] = tmp;
newGPS = true;
}
[[noreturn]] void GlobalOptimization::optimize(){
while(true){
if (newGPS){
newGPS = false;
printf("global optimization\n");
TicToc globalOptimizationTime;
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//add param
mPoseMap.lock();
int length = localPoseMap.size();
double t_array[length][3];
double q_array[length][4];
map<double, vector<double>>::iterator iter;
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++){
t_array[i][0] = iter->second[0];
t_array[i][1] = iter->second[1];
t_array[i][2] = iter->second[2];
q_array[i][0] = iter->second[3];
q_array[i][1] = iter->second[4];
q_array[i][2] = iter->second[5];
q_array[i][3] = iter->second[6];
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
}
map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
int i = 0;
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
{
//vio factor
iterVIONext = iterVIO;
iterVIONext++;
if(iterVIONext != localPoseMap.end())
{
Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4],
iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4],
iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
Eigen::Matrix4d iTj = wTi.inverse() * wTj;
Eigen::Quaterniond iQj;
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);
// auto tCov = localTranslationCovariance[iterVIO->first];
// Eigen::VectorXd translationCovariance(tCov.size());
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// translationCovariance[r*3 + c] = tCov[r*3 + c];
// }
// }
// std::cout << "Eigen translation covariance norm is: " << translationCovariance.norm() << std::endl;
//
// auto rCov = localRotationCovariance[iterVIO->first];
// Eigen::VectorXd rotationCovariance(rCov.size());
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// rotationCovariance[r*3 + c] = rCov[r*3 + c];
// }
// }
// std::cout << "Eigen rotation covariance norm is: " << rotationCovariance.norm() << std::endl;
// std::cout << "Eigen rotation covariance first 3 elems: " << rotationCovariance[0] << " " <<
// rotationCovariance[1] << " " << rotationCovariance[2] << std::endl;
ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
iQj.w(), iQj.x(), iQj.y(), iQj.z(),
0.1, 0.01);
problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);
/*
double **para = new double *[4];
para[0] = q_array[i];
para[1] = t_array[i];
para[3] = q_array[i+1];
para[4] = t_array[i+1];
double *tmp_r = new double[6];
double **jaco = new double *[4];
jaco[0] = new double[6 * 4];
jaco[1] = new double[6 * 3];
jaco[2] = new double[6 * 4];
jaco[3] = new double[6 * 3];
vio_function->Evaluate(para, tmp_r, jaco);
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 1>>(tmp_r).transpose() << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>>(jaco[0]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>>(jaco[1]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>>(jaco[2]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>>(jaco[3]) << std::endl
<< std::endl;
*/
}
//gps factor
double t = iterVIO->first;
iterGPS = GPSPositionMap.find(t);
if (iterGPS != GPSPositionMap.end())
{
ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],
iterGPS->second[2], iterGPS->second[3]);
//printf("inverse weight %f \n", iterGPS->second[3]);
problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
/*
double **para = new double *[1];
para[0] = t_array[i];
double *tmp_r = new double[3];
double **jaco = new double *[1];
jaco[0] = new double[3 * 3];
gps_function->Evaluate(para, tmp_r, jaco);
std::cout << Eigen::Map<Eigen::Matrix<double, 3, 1>>(tmp_r).transpose() << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(jaco[0]) << std::endl
<< std::endl;
*/
}
}
//mPoseMap.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
// update global pose
//mPoseMap.lock();
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
iter->second = globalPose;
if(i == length - 1)
{
Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();
Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
double t = iter->first;
WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4],
localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();
WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);
WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4],
globalPose[5], globalPose[6]).toRotationMatrix();
WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
}
}
updateGlobalPath();
//printf("global time %f \n", globalOptimizationTime.toc());
mPoseMap.unlock();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}
void GlobalOptimization::updateGlobalPath()
{
global_path.poses.clear();
map<double, vector<double>>::iterator iter;
for (iter = globalPoseMap.begin(); iter != globalPoseMap.end(); iter++)
{
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(iter->first);
pose_stamped.header.frame_id = "gps_global";
pose_stamped.pose.position.x = iter->second[0];
pose_stamped.pose.position.y = iter->second[1];
pose_stamped.pose.position.z = iter->second[2];
pose_stamped.pose.orientation.w = iter->second[3];
pose_stamped.pose.orientation.x = iter->second[4];
pose_stamped.pose.orientation.y = iter->second[5];
pose_stamped.pose.orientation.z = iter->second[6];
global_path.poses.push_back(pose_stamped);
}
}

69
global_fusion/src/globalOpt.h Normal file
View File

@@ -0,0 +1,69 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#ifndef GLOBAL_FUSION_GLOBALOPT_H
#define GLOBAL_FUSION_GLOBALOPT_H
#endif //GLOBAL_FUSION_GLOBALOPT_H
#pragma once
#include <vector>
#include <map>
#include <iostream>
#include <mutex>
#include <thread>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <ceres/ceres.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include "LocalCartesian.hpp"
#include "tic_toc.h"
using namespace std;
class GlobalOptimization
{
public:
GlobalOptimization();
~GlobalOptimization();
void inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy);
void inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ);
void getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &domQ);
nav_msgs::Path global_path;
private:
// this function is interesting. I suppose it transforms from the gps coordinate system to the
// local (vins) coordinate system. Which has the center at the start of the sequence by the way.
void GPS2XYZ(double latitude, double longitude, double altitude, double* xyz);
[[noreturn]] void optimize();
void updateGlobalPath();
// format t, tx, ty, tz, qw, qx, qy, qz
map<double, vector<double>> localPoseMap;
map<double, vector<double>> localTranslationCovariance;
map<double, vector<double>> localRotationCovariance;
map<double, vector<double>> globalPoseMap;
map<double, vector<double>> GPSPositionMap;
bool initGPS;
bool newGPS;
GeographicLib::LocalCartesian geoConverter;
std::mutex mPoseMap;
Eigen::Matrix4d WGPS_T_WVIO;
Eigen::Vector3d lastP;
Eigen::Quaterniond lastQ;
std::thread threadOpt;
};

243
global_fusion/src/globalOptNode.cpp Normal file
View File

@@ -0,0 +1,243 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include "ros/ros.h"
#include "globalOpt.h"
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Image.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <iostream>
#include <stdio.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <fstream>
#include <queue>
#include <mutex>
#include <math.h>
GlobalOptimization globalEstimator;
ros::Publisher pub_global_odometry, pub_global_path, pub_car, pub_imu_tf_global;
nav_msgs::Path *global_path;
// we need to use pointer becuase otherwise TransformBroadcaster will be initializing here, before
// ros::init() called
std::shared_ptr<tf::TransformBroadcaster> TfBr;
double last_vio_t = -1;
std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue;
std::mutex m_buf;
void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car)
{
visualization_msgs::MarkerArray markerArray_msg;
visualization_msgs::Marker car_mesh;
car_mesh.header.stamp = ros::Time(t);
car_mesh.header.frame_id = "gps_global";
car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
car_mesh.action = visualization_msgs::Marker::ADD;
car_mesh.id = 0;
car_mesh.mesh_resource = "package://global_fusion/models/car.dae";
Eigen::Matrix3d rot;
rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;
Eigen::Quaterniond Q;
Q = q_w_car * rot;
car_mesh.pose.position.x = t_w_car.x();
car_mesh.pose.position.y = t_w_car.y();
car_mesh.pose.position.z = t_w_car.z();
car_mesh.pose.orientation.w = Q.w();
car_mesh.pose.orientation.x = Q.x();
car_mesh.pose.orientation.y = Q.y();
car_mesh.pose.orientation.z = Q.z();
car_mesh.color.a = 1.0;
car_mesh.color.r = 1.0;
car_mesh.color.g = 0.0;
car_mesh.color.b = 0.0;
float major_scale = 2.0;
car_mesh.scale.x = major_scale;
car_mesh.scale.y = major_scale;
car_mesh.scale.z = major_scale;
markerArray_msg.markers.push_back(car_mesh);
pub_car.publish(markerArray_msg);
}
void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
//printf("gps_callback! \n");
m_buf.lock();
gpsQueue.push(GPS_msg);
m_buf.unlock();
}
void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
//printf("vio_callback! \n");
double t = pose_msg->header.stamp.toSec();
last_vio_t = t;
Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
Eigen::Quaterniond vio_q;
vio_q.w() = pose_msg->pose.pose.orientation.w;
vio_q.x() = pose_msg->pose.pose.orientation.x;
vio_q.y() = pose_msg->pose.pose.orientation.y;
vio_q.z() = pose_msg->pose.pose.orientation.z;
// auto cov = pose_msg->pose.covariance;
// auto rCovArr = pose_msg->twist.covariance;
// auto tCovArr = pose_msg->pose.covariance;
// convert boost::array to std::array
// std::vector<double> transCov(9);
// std::vector<double> rotCov(9);
// std::vector<double> tCov(tCovArr.size());
// Presuming that the first 3 block shows the translation cov, and the rest shows rotation cov
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// transCov[r*3 + c] = cov[r*6 + c];
// }
// }
//
// for (int r = 3; r < 6; r++){
// for (int c = 3; c < 6; c++){
// rotCov[(r-3)*3 + (c-3)] = cov[r*6 + c];
// }
// }
// std::cout << "The obtained below matrix from cov" << std::endl;
// // debugging. Output the below matrix
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// std::cout << rotCov[r*3 + c] << " ";
// }
// std::cout << std::endl;
// }
// ROS_INFO_STREAM("the filled vector size" << rCov.size() << " the filled vector first element: " << rCov[0]);
globalEstimator.inputOdom(t, vio_t, vio_q);
m_buf.lock();
while(!gpsQueue.empty())
{
sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
double gps_t = GPS_msg->header.stamp.toSec();
printf("vio t: %f, gps t: %f \n", t, gps_t);
// 10ms sync tolerance
// PI: Let's change the sync tolerance from 10 ms to 100 ms
// if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
if(gps_t >= t - 0.1 && gps_t <= t + 0.1)
{
//printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());
double latitude = GPS_msg->latitude;
double longitude = GPS_msg->longitude;
double altitude = GPS_msg->altitude;
//int numSats = GPS_msg->status.service;
// it was made for KITTI dataset before, which only gives the one parameter. But here we have 3 variances
// (the other values in the covariance matrix are 0).
double pos_accuracy;
pos_accuracy = std::sqrt( pow(GPS_msg->position_covariance[0], 2.0) + pow(GPS_msg->position_covariance[4], 2) + pow(GPS_msg->position_covariance[8], 2) );
if(pos_accuracy <= 0)
pos_accuracy = 1;
//printf("receive covariance %lf \n", pos_accuracy);
//if(GPS_msg->status.status > 8)
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
break;
}
else if(gps_t < t - 0.01)
gpsQueue.pop();
else if(gps_t > t + 0.01)
break;
}
m_buf.unlock();
Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
globalEstimator.getGlobalOdom(global_t, global_q);
nav_msgs::Odometry odometry;
odometry.header = pose_msg->header;
odometry.header.frame_id = "gps_global";
odometry.pose.pose.position.x = global_t.x();
odometry.pose.pose.position.y = global_t.y();
odometry.pose.pose.position.z = global_t.z();
odometry.pose.pose.orientation.x = global_q.x();
odometry.pose.pose.orientation.y = global_q.y();
odometry.pose.pose.orientation.z = global_q.z();
odometry.pose.pose.orientation.w = global_q.w();
pub_global_odometry.publish(odometry);
pub_global_path.publish(*global_path);
publish_car_model(t, global_t, global_q);
// write result to file
std::ofstream foutC("/home/admin1/workspace/catkin_ws_ov/src/open_vins/global_fusion/data/vio_global.csv", ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
foutC.precision(5);
foutC << global_t.x() << ","
<< global_t.y() << ","
<< global_t.z() << ","
<< global_q.w() << ","
<< global_q.x() << ","
<< global_q.y() << ","
<< global_q.z() << endl;
foutC.close();
}
void gps_img_callback(sensor_msgs::ImageConstPtr msg){
Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
globalEstimator.getGlobalOdom(global_t, global_q);
// Publish the tf so that Camera can bind to it.
tf::StampedTransform trans;
trans.frame_id_ = "gps_global";
trans.child_frame_id_ = "imu_gps";
trans.stamp_ = msg->header.stamp;
tf::Quaternion quat(global_q.x(), global_q.y(), global_q.z(), global_q.w());
trans.setRotation(quat);
tf::Vector3 orig(global_t.x(), global_t.y(), global_t.z());
trans.setOrigin(orig);
TfBr->sendTransform(trans);
// pub_imu_tf_global.publish(trans);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "globalEstimator");
ros::NodeHandle n("~");
global_path = &globalEstimator.global_path;
TfBr = std::make_shared<tf::TransformBroadcaster>();
ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
ros::Subscriber sub_img = n.subscribe("/global_fusion_node/sensor_left", 100, gps_img_callback);
pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
// pub_imu_tf_global = n.advertise<tf::StampedTransform>("tf_imu_global", 100);
pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);
ros::spin();
return 0;
}

47
global_fusion/src/tic_toc.h Normal file
View File

@@ -0,0 +1,47 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*******************************************************/
#ifndef GLOBAL_FUSION_TIC_TOC_H
#define GLOBAL_FUSION_TIC_TOC_H
#endif //GLOBAL_FUSION_TIC_TOC_H
#pragma once
#include <ctime>
#include <cstdlib>
#include <chrono>
class TicToc
{
public:
TicToc()
{
tic();
}
void tic()
{
start = std::chrono::system_clock::now();
}
double toc()
{
end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end - start;
return elapsed_seconds.count() * 1000;
}
private:
std::chrono::time_point<std::chrono::system_clock> start, end;
};

6
ov_eval/launch/record.launch
View File

@@ -3,11 +3,11 @@
<!-- what ros bag to play --> <!-- what ros bag to play -->
<arg name="bag_name" default="merged" /> <arg name="bag_name" default="merged" />
<arg name="bag_path" default="/home/pi/work_drivecast/datasets/complex_urban_dataset/urban32-yeouido/bag2" /> <arg name="bag_path" default="/home/admin1/podmivan/datasets/complex-urban/urban32-yeouido/bag" />
<!-- where to save the recorded poses --> <!-- where to save the recorded poses -->
<arg name="path_save" default="/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/" /> <arg name="path_save" default="/home/admin1/workspace/catkin_ws_ov/src/open_vins/ov_data/" />
<!-- record the trajectory --> <!-- record the trajectory -->
@@ -20,7 +20,7 @@
<node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="screen" required="true"> <node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="screen" required="true">
<param name="alignment_type" type="str" value="posyaw" /> <param name="alignment_type" type="str" value="posyaw" />
<param name="path_gt" type="str" value="/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt" /> <param name="path_gt" type="str" value="/home/admin1/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt" />
<param name="verbosity" type="str" value="DEBUG" /> <param name="verbosity" type="str" value="DEBUG" />
</node> </node>

2
ov_msckf/CMakeLists.txt
View File

@@ -10,7 +10,7 @@ if (NOT OpenCV_FOUND)
endif () endif ()
find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time)
find_package(Ceres REQUIRED) find_package(Ceres REQUIRED)
include_directories(/home/pi/work_drivecast/slams/ORB-SLAM3_Linux/Thirdparty/Sophus) include_directories(/home/admin1/podmivan/SLAM/ORB_SLAM3/Thirdparty/Sophus)
message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION} " | CERES: " ${Ceres_VERSION}) message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION} " | CERES: " ${Ceres_VERSION})
# If we will compile with aruco support # If we will compile with aruco support

14
ov_msckf/cmake/ROS1.cmake
View File

@@ -126,6 +126,20 @@ if (catkin_FOUND AND ENABLE_ROS)
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
) )
add_executable(image_render_combine src/imrender_combine.cpp)
target_link_libraries(image_render_combine ov_msckf_lib ${thirdparty_libraries})
install(TARGETS image_render_combine
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
add_executable(render_cubes src/render_cubes.cpp)
target_link_libraries(render_cubes ov_msckf_lib ${thirdparty_libraries})
install(TARGETS render_cubes
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
endif () endif ()
add_executable(run_simulation src/run_simulation.cpp) add_executable(run_simulation src/run_simulation.cpp)

64
ov_msckf/launch/display.rviz
View File

@@ -4,10 +4,12 @@ Panels:
Name: Displays Name: Displays
Property Tree Widget: Property Tree Widget:
Expanded: Expanded:
- /CameraPub11
- /Camera1 - /Camera1
- /Camera1/Status1 - /Camera1/Status1
- /Marker1
Splitter Ratio: 0.6011396050453186 Splitter Ratio: 0.6011396050453186
Tree Height: 345 Tree Height: 452
- Class: rviz/Selection - Class: rviz/Selection
Name: Selection Name: Selection
- Class: rviz/Tool Properties - Class: rviz/Tool Properties
@@ -26,7 +28,7 @@ Panels:
Experimental: false Experimental: false
Name: Time Name: Time
SyncMode: 0 SyncMode: 0
SyncSource: "" SyncSource: SLAM Points
Preferences: Preferences:
PromptSaveOnExit: true PromptSaveOnExit: true
Toolbars: Toolbars:
@@ -52,6 +54,32 @@ Visualization Manager:
Plane Cell Count: 80 Plane Cell Count: 80
Reference Frame: <Fixed Frame> Reference Frame: <Fixed Frame>
Value: true Value: true
- Background Color: 0; 0; 0
Camera Info Topic: /ov_msckf/cam0_info
Class: rviz_camera_stream/CameraPub
Display namespace: ""
Enabled: true
Frame Rate: 10
Image Encoding: rgb8
Image Topic: /ov_msckf/rendered
Name: CameraPub1
Near Clip Distance: 0.009999999776482582
Queue Size: 2
Value: true
Visibility:
ACTIVE Features: true
ARUCO Points: true
Camera: true
Current Depths: true
Grid: true
Image Tracks: true
MSCKF Points: true
Marker: true
Path GT: true
Path VIO: true
SIM Points: true
SLAM Points: true
Value: true
- Class: rviz/Image - Class: rviz/Image
Enabled: true Enabled: true
Image Topic: /ov_msckf/trackhist Image Topic: /ov_msckf/trackhist
@@ -61,12 +89,11 @@ Visualization Manager:
Name: Image Tracks Name: Image Tracks
Normalize Range: true Normalize Range: true
Queue Size: 2 Queue Size: 2
Transport Hint: compressed Transport Hint: raw
Unreliable: false Unreliable: false
Value: true Value: true
Camera Info Topic: /ov_msckf/camera_info
- Class: rviz/Image - Class: rviz/Image
Enabled: true Enabled: false
Image Topic: /ov_msckf/loop_depth_colored Image Topic: /ov_msckf/loop_depth_colored
Max Value: 1 Max Value: 1
Median window: 5 Median window: 5
@@ -76,8 +103,7 @@ Visualization Manager:
Queue Size: 2 Queue Size: 2
Transport Hint: compressed Transport Hint: compressed
Unreliable: false Unreliable: false
Value: true Value: false
- Camera Info Topic: /ov_msckf/camera_info
- Alpha: 1 - Alpha: 1
Buffer Length: 1 Buffer Length: 1
Class: rviz/Path Class: rviz/Path
@@ -267,7 +293,6 @@ Visualization Manager:
Use rainbow: true Use rainbow: true
Value: true Value: true
- Class: rviz/Camera - Class: rviz/Camera
Camera Info Topic: /ov_msckf/camera_info
Enabled: true Enabled: true
Image Rendering: background and overlay Image Rendering: background and overlay
Image Topic: /ov_msckf/trackhist Image Topic: /ov_msckf/trackhist
@@ -280,17 +305,34 @@ Visualization Manager:
Visibility: Visibility:
ACTIVE Features: true ACTIVE Features: true
ARUCO Points: true ARUCO Points: true
CameraPub1: true
Current Depths: true Current Depths: true
Grid: true Grid: true
Image Tracks: true Image Tracks: true
MSCKF Points: true MSCKF Points: true
Marker: true
Path GT: true Path GT: true
Path VIO: true Path VIO: true
SIM Points: true SIM Points: true
SLAM Points: true SLAM Points: true
TF: true
Value: true Value: true
Zoom Factor: 1 Zoom Factor: 1
- Class: rviz/Marker
Enabled: true
Marker Topic: /ov_msckf/vio_vis_markers
Name: Marker
Namespaces:
gps_namespace: true
Queue Size: 100
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: visualization_marker
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: true Enabled: true
Global Options: Global Options:
Background Color: 44; 44; 44 Background Color: 44; 44; 44
@@ -319,7 +361,7 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz/Orbit Class: rviz/Orbit
Distance: 12.363367080688477 Distance: 5925.900390625
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
@@ -351,7 +393,7 @@ Window Geometry:
Hide Right Dock: true Hide Right Dock: true
Image Tracks: Image Tracks:
collapsed: false collapsed: false
QMainWindow State: 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 QMainWindow State: 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
Selection: Selection:
collapsed: false collapsed: false
Time: Time:

23
ov_msckf/launch/subscribe.launch
View File

@@ -16,8 +16,8 @@
<!-- <arg name="bag" default="/datasets/$(arg config)/$(arg dataset).bag" />--> <!-- <arg name="bag" default="/datasets/$(arg config)/$(arg dataset).bag" />-->
<!-- saving trajectory path and timing information --> <!-- saving trajectory path and timing information -->
<arg name="dosave" default="false" /> <arg name="dosave" default="true" />
<arg name="dotime" default="false" /> <arg name="dotime" default="true" />
<arg name="path_est" default="/tmp/traj_estimate.txt" /> <arg name="path_est" default="/tmp/traj_estimate.txt" />
<arg name="path_time" default="/tmp/traj_timing.txt" /> <arg name="path_time" default="/tmp/traj_timing.txt" />
@@ -28,6 +28,8 @@
<!-- MASTER NODE! --> <!-- MASTER NODE! -->
<!-- <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">--> <!-- <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">-->
<node name="image_render_combine" pkg="ov_msckf" type="image_render_combine" output="screen" clear_params="true" required="true"> </node>
<node name="render_cubes" pkg="ov_msckf" type="render_cubes" output="screen" clear_params="true" required="true"></node>
<node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true"> <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">
<!-- master configuration object --> <!-- master configuration object -->
@@ -66,5 +68,22 @@
</node> </node>
</group> </group>
<group ns="camera1">
<!-- <node pkg="tf" type="static_transform_publisher" name="camera_broadcaster"
args="0.1 0.1 -0.5 0 0 0 1 map camera1 10" /> -->
<node name="camera_info" pkg="rostopic" type="rostopic"
args="pub camera_info sensor_msgs/CameraInfo
'{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'cam0'},
height: 480, width: 640, distortion_model: 'plumb_bob',
D: [-5.6143027800000002e-02, 1.3952563200000001e-01, -1.2155906999999999e-03],
K: [500.0, 0.0, 320, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0],
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
P: [500.0, 0.0, 320, 0.0, 0.0, 500, 240, 0.0, 0.0, 0.0, 1.0, 0.0],
binning_x: 0, binning_y: 0,
roi: {x_offset: 0, y_offset: 0, height: 480, width: 640, do_rectify: false}}' -r 2"
output="screen"/>
</group>
</launch> </launch>

94
ov_msckf/launch/subscribe_kitti.launch Normal file
View File

@@ -0,0 +1,94 @@
<launch>
<!-- what config we are going to run (should match folder name) -->
<arg name="verbosity" default="INFO" /> <!-- ALL, DEBUG, INFO, WARNING, ERROR, SILENT -->
<arg name="config" default="uzh-fpv" /> <!-- euroc_mav, tum_vi, rpng_aruco -->
<arg name="config_path" default="$(find ov_msckf)/../config/$(arg config)/estimator_config.yaml" />
<!-- mono or stereo and what ros bag to play -->
<arg name="max_cameras" default="2" />
<arg name="use_stereo" default="true" />
<arg name="bag_start" default="0" /> <!-- v1-2: 0, mh1: 40, mh2: 35, mh3: 17.5, mh4-5: 15 -->
<arg name="dataset" default="indoor_forward_3_snapdragon_with_gt" /> <!-- V1_01_easy, V1_02_medium, V2_02_medium -->
<arg name="dobag" default="false" /> <!-- if we should play back the bag -->
<!-- <arg name="bag" default="/media/patrick/RPNG\ FLASH\ 3/$(arg config)/$(arg dataset).bag" /> -->
<arg name="bag" default="/home/pi/work_drivecast/datasets/$(arg config)/$(arg dataset).bag" />
<!-- <arg name="bag" default="/datasets/$(arg config)/$(arg dataset).bag" />-->
<!-- saving trajectory path and timing information -->
<arg name="dosave" default="true" />
<arg name="dotime" default="true" />
<arg name="path_est" default="/tmp/traj_estimate.txt" />
<arg name="path_time" default="/tmp/traj_timing.txt" />
<!-- if we should viz the groundtruth -->
<arg name="dolivetraj" default="false" />
<arg name="path_gt" default="$(find ov_data)/$(arg config)/$(arg dataset).txt" />
<!-- MASTER NODE! -->
<!-- <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">-->
<node name="image_render_combine" pkg="ov_msckf" type="image_render_combine" output="screen" clear_params="true" required="true"> </node>
<node name="render_cubes" pkg="ov_msckf" type="render_cubes" output="screen" clear_params="true" required="true"></node>
<node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">
<!-- master configuration object -->
<param name="verbosity" type="string" value="$(arg verbosity)" />
<param name="config_path" type="string" value="$(arg config_path)" />
<!-- world/filter parameters -->
<param name="use_stereo" type="bool" value="$(arg use_stereo)" />
<param name="max_cameras" type="int" value="$(arg max_cameras)" />
<!-- timing statistics recording -->
<param name="record_timing_information" type="bool" value="$(arg dotime)" />
<param name="record_timing_filepath" type="string" value="$(arg path_time)" />
<remap from="/imu/data_raw" to="/kitti/oxts/imu"/>
<remap from="/stereo/left/image_raw" to="/kitti/camera_color_left/image_raw"/>
<remap from="/stereo/right/image_raw" to="/kitti/camera_color_right/image_raw"/>
</node>
<!-- play the dataset -->
<group if="$(arg dobag)">
<node pkg="rosbag" type="play" name="rosbag" args="-d 1 -s $(arg bag_start) $(arg bag)" required="true"/>
</group>
<!-- record the trajectory if enabled -->
<group if="$(arg dosave)">
<node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
<param name="topic" type="str" value="/ov_msckf/poseimu" />
<param name="topic_type" type="str" value="PoseWithCovarianceStamped" />
<param name="output" type="str" value="$(arg path_est)" />
</node>
</group>
<!-- path viz of aligned gt -->
<group if="$(arg dolivetraj)">
<node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="log" clear_params="true">
<param name="alignment_type" type="str" value="posyaw" />
<param name="path_gt" type="str" value="$(arg path_gt)" />
</node>
</group>
<group ns="camera1">
<!-- <node pkg="tf" type="static_transform_publisher" name="camera_broadcaster"
args="0.1 0.1 -0.5 0 0 0 1 map camera1 10" /> -->
<node name="camera_info" pkg="rostopic" type="rostopic"
args="pub camera_info sensor_msgs/CameraInfo
'{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'cam0'},
height: 480, width: 640, distortion_model: 'plumb_bob',
D: [-5.6143027800000002e-02, 1.3952563200000001e-01, -1.2155906999999999e-03],
K: [500.0, 0.0, 320, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0],
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
P: [500.0, 0.0, 320, 0.0, 0.0, 500, 240, 0.0, 0.0, 0.0, 1.0, 0.0],
binning_x: 0, binning_y: 0,
roi: {x_offset: 0, y_offset: 0, height: 480, width: 640, do_rectify: false}}' -r 2"
output="screen"/>
</group>
</launch>

139
ov_msckf/src/imrender_combine.cpp Normal file
View File

@@ -0,0 +1,139 @@
//
// Created by Podmogilnyi Ivan on 11.08.2022.
//
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
std::vector<sensor_msgs::Image> sensor_images;
ros::Publisher pub_res, pub_mask, pub_recv_curr_image;
// sMsg - sensor message. rMsg - render message
void rendered_image_callback(sensor_msgs::Image::ConstPtr sMsg, sensor_msgs::Image::ConstPtr rMsg){
// publish the left image to the topic needed for
// pub_recv_curr_image.publish(sMsg);
cv::Mat image, mask;
cv_bridge::CvImagePtr bridge_img = cv_bridge::toCvCopy(rMsg, "bgr8");
image = bridge_img->image;
ROS_INFO_STREAM("The image encoding is: " << bridge_img->encoding);
mask = image.clone();
cv::cvtColor(mask, mask, cv::COLOR_BGR2GRAY);
// find the appropriate image from msgs
cv::Mat current_sensor_image;
cv_bridge::CvImagePtr bridge_sensor_img = cv_bridge::toCvCopy(sMsg, "bgr8");
current_sensor_image = bridge_sensor_img->image;
ROS_INFO_STREAM("The message sequence number: " << rMsg->header.seq);
ROS_INFO_STREAM("The sensor_message sequence number: " << sMsg->header.seq);
ROS_INFO_STREAM("The message stamp is: " << std::fixed << std::setprecision(9) << rMsg->header.stamp.toSec());
ROS_INFO_STREAM("The sensor_message stamp is: " << std::fixed << std::setprecision(9) << sMsg->header.stamp.toSec());
bool found_flag = true;
// since there's the exact match in the timestamps, we don't need to find the closest
// timestamp, therefore, no need to use the Synchronized algorithm from message_filters in ROS.
// for (int i=sensor_images.size()-1;i>=0;i--){
// if (sensor_images[i].header.stamp == msg.header.stamp){
// cv_bridge::CvImagePtr bridge_sensor_img = cv_bridge::toCvCopy(sensor_images[i], "bgr8");
// current_sensor_image = bridge_sensor_img->image;
// found_flag = true;
// break;
// }
// }
if (found_flag){
ROS_INFO_STREAM("imrender: 1");
ROS_INFO_STREAM("imrender: 1.2");
cv::Mat masked(image.rows, image.cols, CV_8UC3, cv::Scalar(0, 0, 0));
masked = current_sensor_image.clone();
ROS_INFO_STREAM("imrender: 2" << image.rows << " " << image.cols << " " << current_sensor_image.rows <<
" " << current_sensor_image.cols << " " << masked.rows << " " << masked.cols);
//copy the non zero pixels to masked from image.
for (int i=0;i<mask.rows;i++){
for (int j=0;j<mask.cols;j++){
if (mask.at<cv::uint8_t>(i, j) != 0){
masked.at<cv::Vec3b>(i, j) = image.at<cv::Vec3b>(i, j);
}
}
}
ROS_INFO_STREAM("current image type: " << current_sensor_image.type());
ROS_INFO_STREAM("rendered image type: " << image.type());
ROS_INFO_STREAM("masked image type: " << masked.type());
// cv::bitwise_and(current_sensor_image, image, masked, mask);
ROS_INFO_STREAM("imrender: 3");
std::string fn = std::to_string(rMsg->header.stamp.toSec()) + "_cam0.png";
cv::imwrite(fn, masked);
std_msgs::Header header;
header.stamp = rMsg->header.stamp;
header.frame_id = "cam0";
sensor_msgs::ImagePtr masked_msg = cv_bridge::CvImage(header, "bgr8", masked).toImageMsg();
sensor_msgs::ImagePtr mask_msg = cv_bridge::CvImage(header, "mono8", mask).toImageMsg();
ROS_INFO_STREAM("imrender: 4");
pub_res.publish(*masked_msg);
pub_mask.publish(*mask_msg);
ROS_INFO_STREAM("imrender: 5");
// cv::imwrite(fn, masked);
}
else{
ROS_INFO("Not found an image with the appropriate timestamp.");
}
}
void sensor_image_callback(sensor_msgs::Image msg){
sensor_images.push_back(msg);
ROS_INFO_STREAM("The sensor_images size is: " << sensor_images.size());
ROS_INFO_STREAM("The sensor_image seq number is: " << msg.header.seq);
// cv::Mat image;
// cv_bridge::CvImagePtr bridge_msg = cv_bridge::toCvCopy(msg, "bgr8");
// ROS_INFO_STREAM("imrender: 6" << std::endl << "received image encoding is: " << msg.encoding);
// current_sensor_image = bridge_msg->image;
// ROS_INFO_STREAM("imrender: 7" << std::endl << "the number of channels in opencv image: " << current_sensor_image.channels());
// if (current_sensor_image.channels() == 1){
// cv::cvtColor(current_sensor_image, current_sensor_image, cv::COLOR_GRAY2BGR);
// }
//// cv::imshow("currr_image", current_sensor_image);
//// cv::waitKey(1);
// auto current_sensor_image_msg = cv_bridge::CvImage(msg.header, "bgr8", current_sensor_image).toImageMsg();
// pub_recv_curr_image.publish(*current_sensor_image_msg);
// ROS_INFO_STREAM("imrender: 8");
}
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
int main(int argc, char* argv[]){
ros::init(argc, argv, "image_render_combine");
ros::NodeHandle nh;
sensor_images = {};
ROS_INFO("Hello world!");
ros::Subscriber sub_rend_image, sub_sensor_image;
// sub_sensor_image = nh.subscribe("/stereo/left/image_raw", 1000, &sensor_image_callback);
// sub_rend_image = nh.subscribe("/ov_msckf/rendered", 1000, &rendered_image_callback);
auto sensor_image_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, "/stereo/left/image_raw", 1);
auto rend_image_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, "/ov_msckf/rendered", 1);
auto sync = std::make_shared<message_filters::Synchronizer<sync_pol>>(sync_pol(10), *sensor_image_sub, *rend_image_sub);
sync->registerCallback(boost::bind(&rendered_image_callback, _1, _2));
pub_res = nh.advertise<sensor_msgs::Image>("/ov_msckf/combined", 2);
pub_mask = nh.advertise<sensor_msgs::Image>("/ov_msckf/mask", 2);
// pub_recv_curr_image = nh.advertise<sensor_msgs::Image>("/global_fusion_node/sensor_left", 2);
ros::spin();
}

466
ov_msckf/src/render_cubes.cpp Normal file
View File

@@ -0,0 +1,466 @@
//
// Created by Podmogilnyi Ivan on 17.08.2022.
//
// C++ std library
#include <fstream>
#include <filesystem>
// ROS library
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Path.h>
// open_vis library
#include <utils/quat_ops.h>
// thirdparty libraries
#include <Eigen/Core>
float yCubesVisPos = -2.0;
float skew = 5.0;
std::vector<std::vector<double>> cubeVisPoints = {
{71.215, 32.694, yCubesVisPos+2.0},
// {81.668, 26.227, yCubesVisPos},
{104.061, 36.691, yCubesVisPos+1.5},
// {90.726, 32.825, yCubesVisPos},
{109.541, 60.415, yCubesVisPos+1.0},
// {149.264, 69.490, yCubesVisPos},
{183.605, 91.474, yCubesVisPos+0.5},
// {198.294, 97.113, yCubesVisPos},
{251.391, 154.108, yCubesVisPos-1.0},
// {310.030, 190.208, yCubesVisPos},
{330.554, 183.244, yCubesVisPos-2.0},
{746.086, 450.563, yCubesVisPos-2.0},
// {792.608, 527.140, yCubesVisPos},
{883.669, 517.444, yCubesVisPos-2.0},
{957.811, 652.638, yCubesVisPos-2.0},
{1086.045, 669.317, yCubesVisPos-2.0},
{1125.310, 728.214, yCubesVisPos-2.0},
{731.996, 425.021, yCubesVisPos-2.0},
{1186.527, 700.189, yCubesVisPos},
// {1205.432, 681.457, yCubesVisPos},
{1184.375, 735.604, yCubesVisPos},
// {1232.697, 715.647, yCubesVisPos},
{1244.767, 784.257, yCubesVisPos},
// CUBES FOR THE GPS ALIGNED TRAJECTORY
{-5.473, 15.819, yCubesVisPos},
{-15.586, 25.062, yCubesVisPos},
{-22.395, 43.562, yCubesVisPos},
{-36.035, 50.009, yCubesVisPos},
{-41.113, 62.946, yCubesVisPos},
{-47.675, 62.477, yCubesVisPos},
{-56.081, 76.612, yCubesVisPos},
{-68.155, 82.382, yCubesVisPos},
{-76.903, 97.032, yCubesVisPos},
{-93.218, 103.532, yCubesVisPos},
{-121.903, 137.353, yCubesVisPos},
{-134.567, 140.840, yCubesVisPos},
{-158.030, 169.709, yCubesVisPos},
{-170.521, 177.370, yCubesVisPos},
{-225.440, 236.153, yCubesVisPos},
{-259.810, 259.846, yCubesVisPos},
{-287.220, 297.381, yCubesVisPos},
{-313.876, 312.375, yCubesVisPos},
{-446.905, 450.175, yCubesVisPos},
{-489.056, 477.187, yCubesVisPos},
{-544.828, 555.099, yCubesVisPos},
{-568.349, 556.457, yCubesVisPos},
{-638.075, 643.542, yCubesVisPos},
{-702.377, 676.867, yCubesVisPos},
{-753.507, 750.003, yCubesVisPos},
{-807.197, 768.580, yCubesVisPos},
{-863.780, 829.878, yCubesVisPos},
{-877.002, 860.185, yCubesVisPos},
{-905.568, 864.266, yCubesVisPos},
{-915.407, 887.186, yCubesVisPos},
{-931.954, 899.596, yCubesVisPos},
{-936.091, 907.870, yCubesVisPos},
{-971.656, 952.685, yCubesVisPos},
{-1012.476, 947.723, yCubesVisPos},
{-1016.629, 985.880, yCubesVisPos},
{-1065.223, 987.986, yCubesVisPos},
{-1078.723, 1012.979, yCubesVisPos},
{-1145.907, 1035.300, yCubesVisPos},
{-1234.198, 1116.285, yCubesVisPos},
{-1338.651, 1152.282, yCubesVisPos},
{-1422.556, 1202.064, yCubesVisPos},
{-1488.881, 1208.139, yCubesVisPos},
{-1613.822, 1281.229, yCubesVisPos},
{-1682.561, 1274.593, yCubesVisPos},
{-1927.827, 1384.315, yCubesVisPos},
{-2035.692, 1400.027, yCubesVisPos},
{-2224.536, 1462.204, yCubesVisPos},
{-2313.168, 1458.187, yCubesVisPos},
{-2436.409, 1496.333, yCubesVisPos},
{-2503.565, 1473.606, yCubesVisPos},
{-2605.994, 1481.901, yCubesVisPos},
{-2563.683, 1431.523, yCubesVisPos},
{-2599.753, 1395.453, yCubesVisPos},
{-2679.961, 1407.933, yCubesVisPos},
{-2630.649, 1438.753, yCubesVisPos},
{-2581.338, 1506.557, yCubesVisPos},
{-2528.483, 1197.294, yCubesVisPos},
{-2553.878, 1178.248, yCubesVisPos},
{-2553.878, 1108.412, yCubesVisPos},
{-2450.511, 1023.108, yCubesVisPos},
{-2450.511, 967.579, yCubesVisPos},
{-2437.486, 890.971, yCubesVisPos},
{-2450.511, 819.504, yCubesVisPos},
{-2365.134, 690.688, yCubesVisPos},
{-2296.590, 541.293, yCubesVisPos},
{-2283.679, 620.835, yCubesVisPos},
{-2151.879, 318.478, yCubesVisPos},
{-2176.409, 287.816, yCubesVisPos},
{-2102.820, 251.022, yCubesVisPos},
{-2127.350, 189.698, yCubesVisPos},
{-2028.905, 122.896, yCubesVisPos},
{-2046.821, 66.008, yCubesVisPos},
{-1958.025, 1.247, yCubesVisPos},
{-1886.644, -52.849, yCubesVisPos},
{-1863.596, -64.373, yCubesVisPos},
{-1783.239, -134.282, yCubesVisPos},
{-1705.906, -151.254, yCubesVisPos},
{-1617.356, -206.598, yCubesVisPos},
{-1565.032, -222.410, yCubesVisPos},
{-1465.740, -282.723, yCubesVisPos},
{-1361.237, -309.086, yCubesVisPos},
{-1276.829, -355.967, yCubesVisPos},
{-1236.832, -350.967, yCubesVisPos},
{-1170.082, -368.522, yCubesVisPos},
{-1145.877, -386.675, yCubesVisPos},
{-1133.776, -374.573, yCubesVisPos},
{-1064.985, -381.088, yCubesVisPos},
{-1060.602, -409.499, yCubesVisPos},
{-943.535, -368.153, yCubesVisPos},
{-852.780, -365.980, yCubesVisPos},
{-747.917, -375.279, yCubesVisPos},
{-694.244, -346.169, yCubesVisPos},
{-724.264, -360.724, yCubesVisPos},
{-601.465, -334.072, yCubesVisPos},
{-557.205, -336.465, yCubesVisPos},
{-564.189, -332.895, yCubesVisPos},
{-520.468, -340.201, yCubesVisPos},
{-476.908, -317.140, yCubesVisPos},
{-417.040, -318.781, yCubesVisPos},
{-377.842, -296.022, yCubesVisPos},
{-309.273, -302.223, yCubesVisPos},
{-267.572, -281.072, yCubesVisPos},
{-233.495, -287.929, yCubesVisPos},
{-211.795, -257.754, yCubesVisPos},
{-209.412, -234.544, yCubesVisPos},
{-144.978, -252.842, yCubesVisPos},
{-86.550, -255.100, yCubesVisPos},
{-57.889, -200.404, yCubesVisPos},
{-12.395, -168.373, yCubesVisPos},
{-8.988, -117.709, yCubesVisPos},
{-5.494, -73.048, yCubesVisPos},
{12.541, -24.783, yCubesVisPos},
{-8.001, 22.391, yCubesVisPos},
{-7.999, -85.193, yCubesVisPos},
{-61.402, 98.350, yCubesVisPos}
};
std::vector<std::vector<double>> cashedCubeVisPoints;
ros::Publisher pub_cubes, pub_cubes_gps, pub_aligned_traj, pub_sensor_left_gps;
ros::Subscriber sub_imu, subs_T_align, sub_stereo_left;
visualization_msgs::Marker Marker, Marker_gps;
std::ofstream fT_aligned;
nav_msgs::Path curr_path;
void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
ROS_INFO_STREAM("received imu");
if (pub_cubes.getNumSubscribers() != 0 || pub_cubes_gps.getNumSubscribers() != 0){
pub_cubes_gps.publish(Marker_gps);
pub_cubes.publish(Marker);
}
}
void T_align_callback(const geometry_msgs::TransformStamped &msg){
// the main thing
const uint pointsLen = cubeVisPoints.size();
std::cout << "1" << std::endl;
Eigen::MatrixXd points(pointsLen, 4);
Eigen::Matrix4d T_align;
Eigen::Vector4d qRot;
qRot << msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w;
Eigen::Matrix3d mRot = ov_core::quat_2_Rot(qRot);
for (int i=0;i<3;i++){
for (int j=0;j<3;j++){
T_align(i,j) = mRot(i,j);
}
}
std::cout << "2" << std::endl;
T_align(0,3) = msg.transform.translation.x;
T_align(1,3) = msg.transform.translation.y;
T_align(2,3) = msg.transform.translation.z;
T_align(3,3) = 1;
std::cout << "3" << std::endl;
for (int i=0;i<cubeVisPoints.size();i++){
points(i, 0) = cashedCubeVisPoints[0][0];
points(i, 1) = cashedCubeVisPoints[0][1];
points(i, 2) = cashedCubeVisPoints[0][2];
points(i, 3) = 1;
}
std::cout << "4" << std::endl;
std::cout << "Extracted T_align: " << T_align << std::endl;
std::stringstream T_align_output;
T_align_output << T_align << std::endl;
ROS_INFO(T_align_output.str().c_str());
fT_aligned << msg.header.stamp.toSec() << std::endl << T_align_output.str().c_str() << std::endl;
points = (T_align * points.transpose()).transpose();
for (int i=0;i<cubeVisPoints.size();i++){
cubeVisPoints[0][0] = points(i, 0);
cubeVisPoints[0][1] = points(i, 1);
cubeVisPoints[0][2] = points(i, 2);
}
std::cout << "5" << std::endl;
// Align the trajectory and publish to see the visualization of obtained R and t.
Eigen::MatrixXd poses(curr_path.poses.size(), 4);
// extract
for (int i=0;i<curr_path.poses.size();i++){
poses(i, 0) = curr_path.poses[i].pose.position.x;
poses(i, 1) = curr_path.poses[i].pose.position.y;
poses(i, 2) = curr_path.poses[i].pose.position.z;
}
// align
auto aligned_poses = (T_align * poses.transpose()).transpose();
// fill again
for (int i=0;i<curr_path.poses.size();i++){
curr_path.poses[i].pose.position.x = poses(i, 0);
curr_path.poses[i].pose.position.y = poses(i, 1);
curr_path.poses[i].pose.position.z = poses(i, 2);
}
// publish
pub_aligned_traj.publish(curr_path);
}
//void callback_stereo_left_gps(sensor_msgs::ImagePtr msg){
// auto new_msg = sensor_msgs::ImagePtr();
// new_msg = msg;
//// new_msg->header.stamp = ros::Time(0.0);
// new_msg->header.frame_id = "cam0_gps";
// pub_sensor_left_gps.publish(new_msg);
//}
int main(int argc, char* argv[]){
ros::init(argc, argv, "render_cubes");
ros::NodeHandle nh;
fT_aligned.open("T_aligned_history.txt");
if (!fT_aligned.is_open()){
ROS_INFO("error opening file!");
}
else{
ROS_INFO("file open success!");
std::string curr_path = std::filesystem::current_path();
std::string output_str = "current path is: " + curr_path;
ROS_INFO(output_str.c_str());
}
// the idea: set the subscriber for IMU, and every time the IMU measurement comes, publish
// the cubes because the images are also published when the IMU measurement arrives.
sub_imu = nh.subscribe("/imu/data_raw", 1000, &callback_imu);
pub_cubes = nh.advertise<visualization_msgs::Marker>("/ov_msckf/vio_vis_markers", 0);
pub_cubes_gps = nh.advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
pub_aligned_traj = nh.advertise<nav_msgs::Path>("/ov_msckf/pathaligned", 2);
// a lame fix: create the subscirber here which sends the message from /stereo/left/image_raw to /global_fusion_node/sensor_left
// sub_stereo_left = nh.subscribe("/stereo/left/image_raw", 1000, &callback_stereo_left_gps);
// pub_sensor_left_gps = nh.advertise<sensor_msgs::Image>("/global_fusion_node/sensor_left", 2);
//// UNCOMMENT THIS TO RESTORE THE T_align_callback
// subs_T_align = nh.subscribe("/ov_msckf/T_align", 1000, T_align_callback);
Marker.header.frame_id = "gps_global";
Marker.header.stamp = ros::Time();
Marker.ns = "my_namespace";
Marker.id = 0;
Marker.type = visualization_msgs::Marker::CUBE_LIST;
Marker.action = visualization_msgs::Marker::ADD;
Marker.pose.orientation.x = 0.0;
Marker.pose.orientation.y = 0.0;
Marker.pose.orientation.z = 0.0;
Marker.pose.orientation.w = 1.0;
Marker.scale.x = 6.8f;
Marker.scale.y = 6.8f;
Marker.scale.z = 6.8f;
Marker.color.a = 1.0; // Don't forget to set the alpha!
Marker.color.r = 0.0;
Marker.color.g = 1.0;
Marker.color.b = 0.0;
//only if using a MESH_RESOURCE marker type:
Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
for (auto & cubeVisPoint : cubeVisPoints){
geometry_msgs::Point tempPoint;
tempPoint.x = cubeVisPoint[0];
tempPoint.y = cubeVisPoint[1];
tempPoint.z = cubeVisPoint[2];
Marker.points.push_back(tempPoint);
}
Marker_gps.header.frame_id = "global";
Marker_gps.header.stamp = ros::Time();
Marker_gps.ns = "gps_namespace";
Marker_gps.id = 1;
Marker_gps.type = visualization_msgs::Marker::CUBE_LIST;
Marker_gps.action = visualization_msgs::Marker::ADD;
Marker_gps.pose.orientation.x = 0.0;
Marker_gps.pose.orientation.y = 0.0;
Marker_gps.pose.orientation.z = 0.0;
Marker_gps.pose.orientation.w = 1.0;
Marker_gps.scale.x = 6.8f;
Marker_gps.scale.y = 6.8f;
Marker_gps.scale.z = 6.8f;
Marker_gps.color.a = 1.0; // Don't forget to set the alpha!
Marker_gps.color.r = 0.0;
Marker_gps.color.g = 1.0;
Marker_gps.color.b = 0.0;
//only if using a MESH_RESOURCE marker type:
Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
for (auto & cubeVisPoint : cubeVisPoints){
geometry_msgs::Point tempPoint;
tempPoint.x = cubeVisPoint[0];
tempPoint.y = cubeVisPoint[1];
tempPoint.z = cubeVisPoint[2];
Marker_gps.points.push_back(tempPoint);
}
ros::spin();
}

358
ov_msckf/src/ros/ROS1Visualizer.cpp
View File

@@ -39,7 +39,9 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
pub_poseimu = nh->advertise<geometry_msgs::PoseWithCovarianceStamped>("/ov_msckf/poseimu", 2); pub_poseimu = nh->advertise<geometry_msgs::PoseWithCovarianceStamped>("/ov_msckf/poseimu", 2);
PRINT_DEBUG("Publishing: %s\n", pub_poseimu.getTopic().c_str()); PRINT_DEBUG("Publishing: %s\n", pub_poseimu.getTopic().c_str());
pub_odomimu = nh->advertise<nav_msgs::Odometry>("/ov_msckf/odomimu", 2); pub_odomimu = nh->advertise<nav_msgs::Odometry>("/ov_msckf/odomimu", 2);
pub_odomimu_gps = nh->advertise<nav_msgs::Odometry>("/ov_msckf/odomimugps", 2);
PRINT_DEBUG("Publishing: %s\n", pub_odomimu.getTopic().c_str()); PRINT_DEBUG("Publishing: %s\n", pub_odomimu.getTopic().c_str());
ROS_INFO_STREAM("Publishing: " << pub_odomimu_gps.getTopic().c_str() << std::endl);
pub_pathimu = nh->advertise<nav_msgs::Path>("/ov_msckf/pathimu", 2); pub_pathimu = nh->advertise<nav_msgs::Path>("/ov_msckf/pathimu", 2);
PRINT_DEBUG("Publishing: %s\n", pub_pathimu.getTopic().c_str()); PRINT_DEBUG("Publishing: %s\n", pub_pathimu.getTopic().c_str());
pub_poserec = nh->advertise<geometry_msgs::PoseStamped>("/ov_msckf/poserec", 2); pub_poserec = nh->advertise<geometry_msgs::PoseStamped>("/ov_msckf/poserec", 2);
@@ -57,6 +59,7 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
// Our tracking image // Our tracking image
it_pub_tracks = it.advertise("/ov_msckf/trackhist", 2); it_pub_tracks = it.advertise("/ov_msckf/trackhist", 2);
sensor_left_pub = it.advertise("/ov_msckf/sensor_left", 2);
PRINT_DEBUG("Publishing: %s\n", it_pub_tracks.getTopic().c_str()); PRINT_DEBUG("Publishing: %s\n", it_pub_tracks.getTopic().c_str());
// Groundtruth publishers // Groundtruth publishers
@@ -75,7 +78,22 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
// Added by PI on 27.07.2022 // Added by PI on 27.07.2022
// В какой топик публикуется ? // В какой топик публикуется ?
subs_T_align = nh->subscribe("/ov_msckf/T_align", 1000, &ROS1Visualizer::T_align_callback, this); sensor_left_id = 0;
pub_aligned_traj = nh->advertise<nav_msgs::Path>("/ov_msckf/pathaligned", 2);
fT_aligned.open("T_aligned_history.txt");
if (!fT_aligned.is_open()){
ROS_INFO("error opening file!");
}
else{
ROS_INFO("file open success!");
std::string curr_path = std::filesystem::current_path();
std::string output_str = "current path is: " + curr_path;
ROS_INFO(output_str.c_str());
}
fT_aligned << "ts mT_aligned" << std::endl;
std::ifstream fileTraj; std::ifstream fileTraj;
fileTraj.open("/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt"); fileTraj.open("/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt");
@@ -116,30 +134,41 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
// } // }
pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", 2); pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", 2);
vis_pub = nh->advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0); pub_cam0_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/cam0_info", 2);
pub_gps_cam0_info = nh->advertise<sensor_msgs::CameraInfo>("/global_fusion_node/camera_info", 2);
sensor_left_gps_pub = it.advertise("/global_fusion_node/sensor_left", 2);
//// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING
// vis_pub = nh->advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
// pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_mskcf/camera_info", 2); // pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_mskcf/camera_info", 2);
// pub_camera_info_trackhist = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/trackhist", 2); // pub_camera_info_trackhist = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/trackhist", 2);
// camPub = nh->advertise<image_transport::CameraPublisher>("ov_msckf/camera_info", 2); // camPub = nh->advertise<image_transport::CameraPublisher>("ov_msckf/camera_info", 2);
//camPub = it.advertiseCamera("/ov_msckf/image_view", 2); //camPub = it.advertiseCamera("/ov_msckf/image_view", 2);
Marker.header.frame_id = "global";
Marker.header.stamp = ros::Time(); //// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING
Marker.ns = "my_namespace"; //
Marker.id = 0; // Marker.header.frame_id = "global";
Marker.type = visualization_msgs::Marker::CUBE_LIST; // Marker.header.stamp = ros::Time();
Marker.action = visualization_msgs::Marker::ADD; // Marker.ns = "my_namespace";
Marker.pose.orientation.x = 0.0; // Marker.id = 0;
Marker.pose.orientation.y = 0.0; // Marker.type = visualization_msgs::Marker::CUBE_LIST;
Marker.pose.orientation.z = 0.0; // Marker.action = visualization_msgs::Marker::ADD;
Marker.pose.orientation.w = 1.0; // Marker.pose.orientation.x = 0.0;
Marker.scale.x = 6.8f; // Marker.pose.orientation.y = 0.0;
Marker.scale.y = 6.8f; // Marker.pose.orientation.z = 0.0;
Marker.scale.z = 6.8f; // Marker.pose.orientation.w = 1.0;
Marker.color.a = 1.0; // Don't forget to set the alpha! // Marker.scale.x = 6.8f;
Marker.color.r = 0.0; // Marker.scale.y = 6.8f;
Marker.color.g = 1.0; // Marker.scale.z = 6.8f;
Marker.color.b = 0.0; // Marker.color.a = 1.0; // Don't forget to set the alpha!
//only if using a MESH_RESOURCE marker type: // Marker.color.r = 0.0;
Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"; // Marker.color.g = 1.0;
// Marker.color.b = 0.0;
// //only if using a MESH_RESOURCE marker type:
// Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
// cubeVisPoints = {{10.0, 10.0, 0.02}, {10, -10, 0.02}, // cubeVisPoints = {{10.0, 10.0, 0.02}, {10, -10, 0.02},
// {50, 10, 0.02}, {50, 30, 0.02}, // {50, 10, 0.02}, {50, 30, 0.02},
@@ -149,61 +178,39 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
// {240, 90, 0.02}, {80, 110, 0.02}, // {240, 90, 0.02}, {80, 110, 0.02},
// {280, 110, 0.02}, {100, 130, 0.02}, // {280, 110, 0.02}, {100, 130, 0.02},
// {320, 130, 0.02}, {320, 150, 0.02}}; // {320, 130, 0.02}, {320, 150, 0.02}};
float yCubesVisPos = -3.0;
float skew = 5.0;
cubeVisPoints = {
{-33.451, 231.074, yCubesVisPos},
{-74.510, 141.228, yCubesVisPos},
{-114.119, -45.468, yCubesVisPos},
{96.947, -52.157, yCubesVisPos},
{-183.654, 605.40, yCubesVisPos},
{228.287, 146.678, yCubesVisPos},
{363.666, 1769.109, yCubesVisPos},
{366.259, 1810.029, yCubesVisPos},
{241.346, 186.492, yCubesVisPos},
{424.114, 1789.749, yCubesVisPos},
{588.224, 322.999, yCubesVisPos},
{616.916, 1919.684, yCubesVisPos},
{845.480, 518.359, yCubesVisPos},
{1027.410, 713.207, yCubesVisPos},
{1027.410, 733.207, yCubesVisPos},
{1101.795, 667.008, yCubesVisPos},
{1100.890, 778.080, yCubesVisPos},
{1182.285, 788.335, yCubesVisPos},
{1593.721, 1111.198, yCubesVisPos},
{1635.912, 1542.791, yCubesVisPos},
{1936.860, 1898.863, yCubesVisPos},
{1865.827, 2190.633, yCubesVisPos},
{1603.437, 2262.750, yCubesVisPos},
// {307.369, 150.830, yCubesVisPos},
// {336.567, 223.124, yCubesVisPos},
// {406.839, 224.973, yCubesVisPos},
// {473.202, 296.579, yCubesVisPos},
// {663.727, 382.915, yCubesVisPos},
// {706.092, 446.382, yCubesVisPos},
// {773.465, 464.975, yCubesVisPos},
// {815.571, 524.754, yCubesVisPos},
// {862.600, 560.769, yCubesVisPos},
// {987.206, 590.868, yCubesVisPos},
// {1018.318, 661.753, yCubesVisPos}
{239.974, 72.267, yCubesVisPos}, //// UNCOMMENT THIS TO RESTORE THE CUBES PUBLISHING
{285.534, 147.012, yCubesVisPos}, // float yCubesVisPos = 3.0;
{379.105, 103.735, yCubesVisPos}, // float skew = 5.0;
{377.834, 190.505, yCubesVisPos}, // cubeVisPoints = {
{589.754, 207.650, yCubesVisPos}, // {71.215, 32.694, yCubesVisPos+3},
{583.577, 299.558, yCubesVisPos}, //// {81.668, 26.227, yCubesVisPos},
{695.422, 271.515, yCubesVisPos}, // {84.455, 41.731, yCubesVisPos+3},
{683.414, 342.212, yCubesVisPos}, //// {90.726, 32.825, yCubesVisPos},
{803.130, 295.558, yCubesVisPos}, // {109.541, 60.415, yCubesVisPos+3},
{901.909, 445.780, yCubesVisPos}, //// {149.264, 69.490, yCubesVisPos},
{104.461, 397.006, yCubesVisPos}, // {150.645, 89.474, yCubesVisPos+2},
{110.683, 534.233, yCubesVisPos}, //// {198.294, 97.113, yCubesVisPos},
{119.329, 502.730, yCubesVisPos}, // {251.391, 154.108, yCubesVisPos},
{238.588, 137.661, yCubesVisPos} //// {310.030, 190.208, yCubesVisPos},
}; // {330.554, 183.244, yCubesVisPos},
// End of added by PI // {746.086, 450.563, yCubesVisPos},
//// {792.608, 527.140, yCubesVisPos},
// {883.669, 517.444, yCubesVisPos},
// {957.811, 652.638, yCubesVisPos},
// {1086.045, 669.317, yCubesVisPos},
// {1125.310, 728.214, yCubesVisPos},
// {731.996, 425.021, yCubesVisPos},
//
// {1186.527, 700.189, yCubesVisPos},
//// {1205.432, 681.457, yCubesVisPos},
// {1184.375, 735.604, yCubesVisPos},
//// {1232.697, 715.647, yCubesVisPos},
// {1244.767, 784.257, yCubesVisPos}
// };
// cashedCubeVisPoints = cubeVisPoints;
// // End of added by PI
// option `to enable publishing of global to IMU transformation // option `to enable publishing of global to IMU transformation
nh->param<bool>("publish_global_to_imu_tf", publish_global2imu_tf, true); nh->param<bool>("publish_global_to_imu_tf", publish_global2imu_tf, true);
@@ -364,7 +371,7 @@ void ROS1Visualizer::visualize_odometry(double timestamp) {
return; return;
// Publish our odometry message if requested // Publish our odometry message if requested
if (pub_odomimu.getNumSubscribers() != 0) { if (pub_odomimu.getNumSubscribers() != 0 || pub_odomimu_gps.getNumSubscribers() != 0) {
nav_msgs::Odometry odomIinM; nav_msgs::Odometry odomIinM;
odomIinM.header.stamp = ros::Time(timestamp); odomIinM.header.stamp = ros::Time(timestamp);
@@ -405,6 +412,14 @@ void ROS1Visualizer::visualize_odometry(double timestamp) {
} }
} }
pub_odomimu.publish(odomIinM); pub_odomimu.publish(odomIinM);
auto gps_odom = nav_msgs::Odometry();
gps_odom.pose.pose.orientation = odomIinM.pose.pose.orientation;
gps_odom.pose.pose.position = odomIinM.pose.pose.position;
gps_odom.pose.covariance = odomIinM.pose.covariance;
gps_odom.header.frame_id = "gps_global";
gps_odom.header.stamp = ros::Time(original_img_ts);
pub_odomimu_gps.publish(gps_odom);
} }
// Publish our transform on TF // Publish our transform on TF
@@ -426,6 +441,14 @@ void ROS1Visualizer::visualize_odometry(double timestamp) {
trans_calib.child_frame_id_ = "cam" + std::to_string(calib.first); trans_calib.child_frame_id_ = "cam" + std::to_string(calib.first);
if (publish_calibration_tf) { if (publish_calibration_tf) {
mTfBr->sendTransform(trans_calib); mTfBr->sendTransform(trans_calib);
// added by PI
auto trans_calib_gps = tf::StampedTransform();
trans_calib_gps = trans_calib;
trans_calib_gps.stamp_ = ros::Time(original_img_ts);
trans_calib_gps.frame_id_ = "imu_gps";
trans_calib_gps.child_frame_id_ = "cam0_gps";
mTfBr->sendTransform(trans_calib_gps);
} }
} }
} }
@@ -588,6 +611,10 @@ void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, con
if (camera_last_timestamp.find(cam_id0) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_id0) + time_delta) { if (camera_last_timestamp.find(cam_id0) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_id0) + time_delta) {
return; return;
} }
// added by PI
auto gps_cv_ptr0 = cv_bridge::toCvCopy(msg0, sensor_msgs::image_encodings::RGB8);
gps_img_msg = gps_cv_ptr0->toImageMsg();
camera_last_timestamp[cam_id0] = timestamp; camera_last_timestamp[cam_id0] = timestamp;
// Get the image // Get the image
@@ -696,6 +723,9 @@ void ROS1Visualizer::publish_state() {
for (size_t i = 0; i < poses_imu.size(); i += std::floor((double)poses_imu.size() / 16384.0) + 1) { for (size_t i = 0; i < poses_imu.size(); i += std::floor((double)poses_imu.size() / 16384.0) + 1) {
arrIMU.poses.push_back(poses_imu.at(i)); arrIMU.poses.push_back(poses_imu.at(i));
} }
// Changed by PI
curr_path = arrIMU;
pub_pathimu.publish(arrIMU); pub_pathimu.publish(arrIMU);
// Move them forward in time // Move them forward in time
@@ -718,34 +748,94 @@ void ROS1Visualizer::publish_images() {
sensor_msgs::ImagePtr exl_msg = cv_bridge::CvImage(header, "bgr8", img_history).toImageMsg(); sensor_msgs::ImagePtr exl_msg = cv_bridge::CvImage(header, "bgr8", img_history).toImageMsg();
exl_msg_global = exl_msg; exl_msg_global = exl_msg;
// Get the original image in order to publish it to the topic for rendering with CameraPub.
// cv::Mat original_img;
// double original_img_ts;
// _app->get_active_image(original_img_ts, original_img);
// // usually the function returns -1. so, let's fill it with the ts from camera_last_timestamp.
// if (original_img_ts == -1){
// original_img_ts = camera_last_timestamp[0];
// }
std_msgs::Header orig_img_msg_header;
// as I remember it's the timestamp in kaist is already in seconds.
ROS_INFO_STREAM("org msg: 1");
ros::Time orig_stamp(original_img_ts);
ROS_INFO_STREAM("org msg: 2");
orig_img_msg_header.stamp = orig_stamp;
orig_img_msg_header.frame_id = "cam0";
// ROS_INFO_STREAM("The camera last timestamp for cam0 is: " << std::fixed << std::setprecision(21) << original_img_ts);
// ROS_INFO_STREAM("Original image width and height are: " << original_img.cols << " " << original_img.rows);
// Publish // Publish
it_pub_tracks.publish(exl_msg); it_pub_tracks.publish(exl_msg);
// publish the left image with the visualizations
// cv::Range rows(0, img_history.rows);
// cv::Range cols(0, img_history.cols / 2);
// cv::Mat cropped_image = img_history(rows, cols);
// sensor_msgs::ImagePtr sensor_left_msg = cv_bridge::CvImage(header, "bgr8", cropped_image).toImageMsg();
// sensor_left_msg->header.seq = sensor_left_id++;
// since this is a pointer, we need to initialize (heap-allocate) it in boost.
sensor_msgs::ImagePtr original_img_message = boost::make_shared<sensor_msgs::Image>();
original_img_message->header = orig_img_msg_header;
original_img_message->encoding = gps_img_msg->encoding;
original_img_message->height = gps_img_msg->height;
original_img_message->width = gps_img_msg->width;
original_img_message->is_bigendian = gps_img_msg->is_bigendian;
original_img_message->step = gps_img_msg->step;
original_img_message->data = gps_img_msg->data;
sensor_left_pub.publish(original_img_message);
original_img_message->header.frame_id = "cam0_gps";
sensor_left_gps_pub.publish(original_img_message);
ROS_INFO("Image publish success!");
bool is_fisheye = (std::dynamic_pointer_cast<ov_core::CamEqui>(_app->get_params().camera_intrinsics.at(0)) != nullptr); bool is_fisheye = (std::dynamic_pointer_cast<ov_core::CamEqui>(_app->get_params().camera_intrinsics.at(0)) != nullptr);
sensor_msgs::CameraInfo cameraparams; sensor_msgs::CameraInfo cameraparams;
cameraparams.header = header; cameraparams.header = header;
cameraparams.header.frame_id = "cam0"; cameraparams.header.frame_id = "cam0";
cameraparams.distortion_model = is_fisheye ? "equidistant" : "plumb_bob"; cameraparams.distortion_model = is_fisheye ? "equidistant" : "plumb_bob";
Eigen::VectorXd cparams = _app->get_state()->_cam_intrinsics.at(0)->value(); Eigen::VectorXd cparams = _app->get_state()->_cam_intrinsics.at(0)->value();
std::stringstream ss;
ss << cparams;
std::string str_to_output = "cparams are: "+ ss.str();
ROS_INFO(str_to_output.c_str());
cameraparams.D = {cparams(4), cparams(5), cparams(6), cparams(7)}; cameraparams.D = {cparams(4), cparams(5), cparams(6), cparams(7)};
cameraparams.K = {cparams(0), 0, cparams(2), 0, cparams(1), cparams(3), 0, 0, 1}; cameraparams.K = {cparams(0), 0, cparams(2), 0, cparams(1), cparams(3), 0, 0, 1};
cameraparams.P = {cparams(0), 0, cparams(2), 0, 0, cparams(1), cparams(3), 0, 0, 0, 1, 0}; cameraparams.P = {cparams(0), 0, cparams(2), 0, 0, cparams(1), cparams(3), 0, 0, 0, 1, 0};
PRINT_DEBUG("cameraparams.P: %d", cameraparams.P.data()); PRINT_DEBUG("cameraparams.P: %d", cameraparams.P.data());
ROS_INFO("cameraparams.P: %d", cameraparams.P.data());
camInfoGlob = sensor_msgs::CameraInfo(); camInfoGlob = sensor_msgs::CameraInfo();
camInfoGlob = cameraparams; camInfoGlob = cameraparams;
// Added by PI // Added by PI
pub_camera_info.publish(camInfoGlob); pub_camera_info.publish(camInfoGlob);
camInfoGlob.header.frame_id = "cam0";
camInfoGlob.header.stamp = orig_img_msg_header.stamp;
camInfoGlob.height = exl_msg->height;
camInfoGlob.width = exl_msg->width / 2;
pub_cam0_info.publish(camInfoGlob);
for (auto & cubeVisPoint : cubeVisPoints){ // publish caminfo for GPS
geometry_msgs::Point tempPoint; auto camInfoGPS = sensor_msgs::CameraInfo();
tempPoint.x = cubeVisPoint[0]; camInfoGPS = cameraparams;
tempPoint.y = cubeVisPoint[1]; camInfoGPS.header.frame_id = "cam0_gps";
tempPoint.z = cubeVisPoint[2]; // camInfoGPS.header.stamp = orig_img_msg_header.stamp;
Marker.points.push_back(tempPoint); camInfoGPS.height = exl_msg->height;
} camInfoGPS.width = exl_msg->width / 2;
vis_pub.publish( Marker ); pub_gps_cam0_info.publish(camInfoGPS);
//// UNCOMMENT THIS TO RESTORE THE CUBES PUBLISHING
// for (auto & cubeVisPoint : cubeVisPoints){
// geometry_msgs::Point tempPoint;
// tempPoint.x = cubeVisPoint[0];
// tempPoint.y = cubeVisPoint[1];
// tempPoint.z = cubeVisPoint[2];
// Marker.points.push_back(tempPoint);
// }
// vis_pub.publish( Marker );
// pub_camera_info.publish(cameraparams); // pub_camera_info.publish(cameraparams);
// pub_camera_info_trackhist.publish(cameraparams); // pub_camera_info_trackhist.publish(cameraparams);
@@ -945,6 +1035,9 @@ void ROS1Visualizer::publish_loopclosure_information() {
if (active_tracks_time1 != active_tracks_time2) if (active_tracks_time1 != active_tracks_time2)
return; return;
// PI: Fill the timestamp.
original_img_ts = active_tracks_time1;
// Default header // Default header
std_msgs::Header header; std_msgs::Header header;
header.stamp = ros::Time(active_tracks_time1); header.stamp = ros::Time(active_tracks_time1);
@@ -995,33 +1088,33 @@ void ROS1Visualizer::publish_loopclosure_information() {
pub_loop_intrinsics.publish(cameraparams); pub_loop_intrinsics.publish(cameraparams);
// Added by PI // Added by PI
int helllo = 0; // int helllo = 0;
// pub_camera_info.publish(cameraparams); //// pub_camera_info.publish(cameraparams);
// pub_camera_info_trackhist.publish(cameraparams); //// pub_camera_info_trackhist.publish(cameraparams);
// {cparams(0), 0, cparams(2), 0, 0, cparams(1), cparams(3), 0, 0, 0, 1} //// {cparams(0), 0, cparams(2), 0, 0, cparams(1), cparams(3), 0, 0, 0, 1}
cameraparams.P = {cparams(0), 0, cparams(2), 0, 0, cparams(1), cparams(3), 0, 0, 0, 1, 0}; // cameraparams.P = {cparams(0), 0, cparams(2), 0, 0, cparams(1), cparams(3), 0, 0, 0, 1, 0};
std::cout << "Testing message" << std::endl; // std::cout << "Testing message" << std::endl;
std::cout << "cameraparams.P: " << cameraparams.P.data() << std::endl; // std::cout << "cameraparams.P: " << cameraparams.P.data() << std::endl;
PRINT_DEBUG("cameraparams.P: %d", cameraparams.P.data()); // PRINT_DEBUG("cameraparams.P: %d", cameraparams.P.data());
//
sensor_msgs::CameraInfoPtr cameraParamsPtr; // sensor_msgs::CameraInfoPtr cameraParamsPtr;
cameraParamsPtr->header = cameraparams.header; // cameraParamsPtr->header = cameraparams.header;
cameraParamsPtr->D = cameraparams.D; // cameraParamsPtr->D = cameraparams.D;
cameraParamsPtr->K = cameraparams.K; // cameraParamsPtr->K = cameraparams.K;
cameraParamsPtr->P = cameraparams.P; // cameraParamsPtr->P = cameraparams.P;
cameraParamsPtr->R = cameraparams.R; // cameraParamsPtr->R = cameraparams.R;
cameraParamsPtr->distortion_model = cameraparams.distortion_model; // cameraParamsPtr->distortion_model = cameraparams.distortion_model;
cameraParamsPtr->binning_x = cameraparams.binning_x; // cameraParamsPtr->binning_x = cameraparams.binning_x;
cameraParamsPtr->binning_y = cameraparams.binning_y; // cameraParamsPtr->binning_y = cameraparams.binning_y;
cameraParamsPtr->height = cameraparams.height; // cameraParamsPtr->height = cameraparams.height;
cameraParamsPtr->roi = cameraparams.roi; // cameraParamsPtr->roi = cameraparams.roi;
cameraParamsPtr->width = cameraparams.width; // cameraParamsPtr->width = cameraparams.width;
//
camInfoGlob = sensor_msgs::CameraInfo(); // camInfoGlob = sensor_msgs::CameraInfo();
camInfoPtrGlob = cameraParamsPtr; // camInfoPtrGlob = cameraParamsPtr;
camInfoGlob = cameraparams; // camInfoGlob = cameraparams;
// camPub.publish(exl_msg_global, cameraparams); //// camPub.publish(exl_msg_global, cameraparams);
camPub.publish(exl_msg_global, cameraParamsPtr); // camPub.publish(exl_msg_global, cameraParamsPtr);
} }
//====================================================== //======================================================
@@ -1125,44 +1218,3 @@ void ROS1Visualizer::publish_loopclosure_information() {
} }
} }
void ROS1Visualizer::T_align_callback(const geometry_msgs::TransformStamped &msg){
// the main thing
const uint pointsLen = cubeVisPoints.size();
std::cout << "1" << std::endl;
Eigen::MatrixXd points(pointsLen, 4);
Eigen::Matrix4d T_align;
Eigen::Vector4d qRot;
qRot << msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w;
Eigen::Matrix3d mRot = ov_core::quat_2_Rot(qRot);
for (int i=0;i<3;i++){
for (int j=0;j<3;j++){
T_align(i,j) = mRot(i,j);
}
}
std::cout << "2" << std::endl;
T_align(0,3) = msg.transform.translation.x;
T_align(1,3) = msg.transform.translation.y;
T_align(2,3) = msg.transform.translation.z;
T_align(3,3) = 1;
std::cout << "3" << std::endl;
for (int i=0;i<cubeVisPoints.size();i++){
points(i, 0) = cubeVisPoints[0][0];
points(i, 1) = cubeVisPoints[0][1];
points(i, 2) = cubeVisPoints[0][2];
points(i, 3) = 1;
}
std::cout << "4" << std::endl;
points = (T_align * points.transpose()).transpose();
for (int i=0;i<cubeVisPoints.size();i++){
cubeVisPoints[0][0] = points(i, 0);
cubeVisPoints[0][1] = points(i, 1);
cubeVisPoints[0][2] = points(i, 2);
}
std::cout << "5" << std::endl;
}

18
ov_msckf/src/ros/ROS1Visualizer.h
View File

@@ -25,6 +25,7 @@
#include <fstream> #include <fstream>
#include <sstream> #include <sstream>
#include <string> #include <string>
#include <filesystem>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h> #include <geometry_msgs/PoseWithCovarianceStamped.h>
@@ -154,7 +155,7 @@ protected:
// Our publishers // Our publishers
image_transport::Publisher it_pub_tracks, it_pub_loop_img_depth, it_pub_loop_img_depth_color; image_transport::Publisher it_pub_tracks, it_pub_loop_img_depth, it_pub_loop_img_depth_color;
ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu; ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu, pub_odomimu_gps;
ros::Publisher pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim; ros::Publisher pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim;
ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics; ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics;
std::shared_ptr<tf::TransformBroadcaster> mTfBr; std::shared_ptr<tf::TransformBroadcaster> mTfBr;
@@ -162,18 +163,25 @@ protected:
// Added by PI on 27.07.2022 // Added by PI on 27.07.2022
// image_transport::CameraPublisher pub_camera_info, pub_camera_info_trackhist; // image_transport::CameraPublisher pub_camera_info, pub_camera_info_trackhist;
image_transport::CameraPublisher camPub; image_transport::CameraPublisher camPub;
image_transport::Publisher sensor_left_pub, sensor_left_gps_pub;
visualization_msgs::Marker Marker; visualization_msgs::Marker Marker;
std::vector<std::vector<double>> cubeVisPoints; std::vector<std::vector<double>> cubeVisPoints, cashedCubeVisPoints;
image_transport::CameraSubscriber camSub; image_transport::CameraSubscriber camSub;
sensor_msgs::CameraInfoPtr camInfoPtrGlob; sensor_msgs::CameraInfoPtr camInfoPtrGlob;
ros::Publisher pub_camera_info; ros::Publisher pub_camera_info, pub_cam0_info, pub_gps_cam0_info;
ros::Publisher vis_pub; ros::Publisher vis_pub;
sensor_msgs::CameraInfo camInfoGlob; sensor_msgs::CameraInfo camInfoGlob;
sensor_msgs::ImagePtr exl_msg_global; sensor_msgs::ImagePtr exl_msg_global;
sensor_msgs::ImageConstPtr gps_img_msg;
ros::Subscriber sub_camera_info, subs_T_align; ros::Subscriber sub_camera_info, subs_T_align;
ros::Publisher pub_poserec; ros::Publisher pub_poserec, pub_aligned_traj;
nav_msgs::Path curr_path;
std::ofstream fT_aligned;
std::vector<Eigen::Matrix<double, 7, 1>> loadedTrajectory; std::vector<Eigen::Matrix<double, 7, 1>> loadedTrajectory;
int sensor_left_id;
// End of adding by PI // End of adding by PI
// Our subscribers and camera synchronizers // Our subscribers and camera synchronizers
@@ -195,6 +203,8 @@ protected:
double summed_nees_pos = 0.0; double summed_nees_pos = 0.0;
size_t summed_number = 0; size_t summed_number = 0;
double original_img_ts;
// Start and end timestamps // Start and end timestamps
bool start_time_set = false; bool start_time_set = false;
boost::posix_time::ptime rT1, rT2; boost::posix_time::ptime rT1, rT2;