Repository: Gaochao-hit/LIO-SAM_based_relocalization
Branch: master
Commit: b162b829660d
Files: 27
Total size: 264.3 KB
Directory structure:
gitextract_uwlmj5r8/
├── .github/
│ └── stale.yml
├── CMakeLists.txt
├── LICENSE
├── README.md
├── config/
│ ├── doc/
│ │ └── kitti2bag/
│ │ ├── README.md
│ │ └── kitti2bag.py
│ └── params.yaml
├── include/
│ └── utility.h
├── launch/
│ ├── include/
│ │ ├── config/
│ │ │ ├── robot.urdf.xacro
│ │ │ └── rviz.rviz
│ │ ├── module_loam.launch
│ │ ├── module_navsat.launch
│ │ ├── module_relocolize.launch
│ │ ├── module_robot_state_publisher.launch
│ │ ├── module_rviz.launch
│ │ └── rosconsole/
│ │ ├── rosconsole_error.conf
│ │ ├── rosconsole_info.conf
│ │ └── rosconsole_warn.conf
│ ├── run.launch
│ └── run_relocalize.launch
├── msg/
│ └── cloud_info.msg
├── package.xml
└── src/
├── featureExtraction.cpp
├── globalLocalize.cpp
├── imageProjection.cpp
├── imuPreintegration.cpp
└── mapOptmization.cpp
================================================
FILE CONTENTS
================================================
================================================
FILE: .github/stale.yml
================================================
# Number of days of inactivity before an issue becomes stale
daysUntilStale: 14
# Number of days of inactivity before a stale issue is closed
daysUntilClose: 1
# Issues with these labels will never be considered stale
exemptLabels:
- pinned
- security
# Label to use when marking an issue as stale
staleLabel: stale
# Comment to post when marking an issue as stale. Set to `false` to disable
markComment: >
This issue has been automatically marked as stale because it has not had
recent activity. It will be closed if no further activity occurs. Thank you
for your contributions.
# Comment to post when closing a stale issue. Set to `false` to disable
closeComment: false
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(lio_sam)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")
find_package(catkin REQUIRED COMPONENTS
tf
roscpp
rospy
cv_bridge
# pcl library
pcl_conversions
# msgs
std_msgs
sensor_msgs
geometry_msgs
nav_msgs
message_generation
)
find_package(OpenMP REQUIRED)
find_package(PCL REQUIRED QUIET)
find_package(OpenCV REQUIRED QUIET)
find_package(GTSAM REQUIRED QUIET)
add_message_files(
DIRECTORY msg
FILES
cloud_info.msg
)
generate_messages(
DEPENDENCIES
geometry_msgs
std_msgs
nav_msgs
sensor_msgs
)
catkin_package(
INCLUDE_DIRS include
DEPENDS PCL GTSAM
CATKIN_DEPENDS
std_msgs
nav_msgs
geometry_msgs
sensor_msgs
message_runtime
message_generation
)
# include directories
include_directories(
include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${GTSAM_INCLUDE_DIR}
)
# link directories
link_directories(
include
${PCL_LIBRARY_DIRS}
${OpenCV_LIBRARY_DIRS}
${GTSAM_LIBRARY_DIRS}
)
###########
## Build ##
###########
# Range Image Projection
add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp)
add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
# Feature Association
add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp)
add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
# Mapping Optimization
add_executable(${PROJECT_NAME}_mapOptmization src/mapOptmization.cpp)
add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS})
target_link_libraries(${PROJECT_NAME}_mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam)
# IMU Preintegration
add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp)
target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)
#Global Localization
add_executable(${PROJECT_NAME}_globalLocalize src/globalLocalize.cpp)
add_dependencies(${PROJECT_NAME}_globalLocalize ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_compile_options(${PROJECT_NAME}_globalLocalize PRIVATE ${OpenMP_CXX_FLAGS})
target_link_libraries(${PROJECT_NAME}_globalLocalize ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam)
================================================
FILE: LICENSE
================================================
BSD 3-Clause License
Copyright (c) 2020, Tixiao Shan
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
================================================
FILE: README.md
================================================
A simple version of system that can relocalize in a built map is developed in this repository. The sysytem is bsed on LIO_SAM.
The repository is developed based on the origional version of LIO-SAM in which the GPS is not fused.
## Run the package
1. Make sure the map should be saved in the right folder:
```
Firstly, you need to run LIO-SAM, and then save the map in the default folder
```
2. Run the launch file:
```
roslaunch lio_sam run_relocalize.launch
```
3. Play existing bag files:
```
rosbag play your-bag.bag
```
-A video of the demonstration of the method can be found on [YouTube](https://youtu.be/PRsH8SpuSIc)
## Notes
- **Initialization:** During the initialization stage, had better keep the robot still. Or if you play bags, fistly play the bag for about 0.5s, and then pause the bag until the initialization succeed. The initialization method requres you to give it initial guesses by hand on the the Rviz.
******************************************************************************************************************************************************************
# LIO-SAM
**A real-time lidar-inertial odometry package. We strongly recommend the users read this document thoroughly and test the package with the provided dataset first. A video of the demonstration of the method can be found on [YouTube](https://www.youtube.com/watch?v=A0H8CoORZJU).**
We design a system that maintains two graphs and runs up to 10x faster than real-time.
- The factor graph in "mapOptimization.cpp" optimizes lidar odometry factor and GPS factor. This factor graph is maintained consistently throughout the whole test.
- The factor graph in "imuPreintegration.cpp" optimizes IMU and lidar odometry factor and estimates IMU bias. This factor graph is reset periodically and guarantees real-time odometry estimation at IMU frequency.
## Dependency
- [ROS](http://wiki.ros.org/ROS/Installation) (tested with Kinetic and Melodic)
```
sudo apt-get install -y ros-kinetic-navigation
sudo apt-get install -y ros-kinetic-robot-localization
sudo apt-get install -y ros-kinetic-robot-state-publisher
```
- [gtsam](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library)
```
wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.2.zip
cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/
cd ~/Downloads/gtsam-4.0.2/
mkdir build && cd build
cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..
sudo make install -j8
```
## Install
Use the following commands to download and compile the package.
```
cd ~/catkin_ws/src
git clone https://github.com/TixiaoShan/LIO-SAM.git
cd ..
catkin_make
```
## Prepare lidar data
The user needs to prepare the point cloud data in the correct format for cloud deskewing, which is mainly done in "imageProjection.cpp". The two requirements are:
- **Provide point time stamp**. LIO-SAM uses IMU data to perform point cloud deskew. Thus, the relative point time in a scan needs to be known. The up-to-date Velodyne ROS driver should output this information directly. Here, we assume the point time channel is called "time." The definition of the point type is located at the top of the "imageProjection.cpp." "deskewPoint()" function utilizes this relative time to obtain the transformation of this point relative to the beginning of the scan. When the lidar rotates at 10Hz, the timestamp of a point should vary between 0 and 0.1 seconds. If you are using other lidar sensors, you may need to change the name of this time channel and make sure that it is the relative time in a scan.
- **Provide point ring number**. LIO-SAM uses this information to organize the point correctly in a matrix. The ring number indicates which channel of the sensor that this point belongs to. The definition of the point type is located at the top of "imageProjection.cpp." The up-to-date Velodyne ROS driver should output this information directly. Again, if you are using other lidar sensors, you may need to rename this information. Note that only mechanical lidars are supported by the package currently.
## Prepare IMU data
- **IMU requirement**. Like the original LOAM implementation, LIO-SAM only works with a 9-axis IMU, which gives roll, pitch, and yaw estimation. The roll and pitch estimation is mainly used to initialize the system at the correct attitude. The yaw estimation initializes the system at the right heading when using GPS data. Theoretically, an initialization procedure like VINS-Mono will enable LIO-SAM to work with a 6-axis IMU. The performance of the system largely depends on the quality of the IMU measurements. The higher the IMU data rate, the better the system accuracy. We use Microstrain 3DM-GX5-25, which outputs data at 500Hz. We recommend using an IMU that gives at least a 200Hz output rate. Note that the internal IMU of Ouster lidar is not usable due to high vibration.
- **IMU alignment**. LIO-SAM transforms IMU raw data from the IMU frame to the Lidar frame, which follows the ROS REP-105 convention (x - forward, y - left, z - upward). To make the system function properly, the correct extrinsic transformation needs to be provided in "params.yaml" file. Using our setup as an example:
- we need to set the readings of x-z acceleration and gyro negative to transform the IMU data in the lidar frame, which is indicated by "extrinsicRot" in "params.yaml."
- The transformation of attitude readings is slightly different. We rotate the attitude measurements by -90 degrees around "lidar-z" axis and get the corresponding roll, pitch, and yaw readings in the lidar frame. This transformation is indicated by "extrinsicRPY" in "params.yaml."
- **IMU debug**. It's strongly recommended that the user uncomment the debug lines in "imuHandler()" of "imageProjection.cpp" and test the output of the transformed IMU data. The user can rotate the sensor suite to check whether the readings correspond to the sensor's movement.
## Sample datasets
* Download some sample datasets to test the functionality of the package. The datasets below is configured to run using the default settings:
- [**Walking dataset**](https://drive.google.com/file/d/1HN5fYPXEHbDq0E5JtbQPkCHIHUoTFFnN/view?usp=sharing)
- [**Garden dataset**](https://drive.google.com/file/d/1q6yuVhyJbkUBhut9yhfox2WdV4VZ9BZX/view?usp=sharing)
- [**Park dataset**](https://drive.google.com/file/d/19PZieaJaVkXDs2ZromaHTxYoq0zkiHae/view?usp=sharing)
* The datasets below need the parameters to be configured. In these datasets, the point cloud topic is "points_raw." The IMU topic is "imu_correct," which gives the IMU data in ROS REP105 standard. Because no IMU transformation is needed for this dataset, the following configurations need to be changed to run this dataset successfully:
- The "imuTopic" parameter in "config/params.yaml" needs to be set to "imu_correct".
- The "extrinsicRot" and "extrinsicRPY" in "config/params.yaml" needs to be set as identity matrices.
- [**Rotation dataset**](https://drive.google.com/file/d/1V4ijY4PgLdjKmdzcQ18Xu7VdcHo2UaWI/view?usp=sharing)
- [**Campus dataset (large)**](https://drive.google.com/file/d/1q4Sf7s2veVc7bs08Qeha3stOiwsytopL/view?usp=sharing)
- [**Campus dataset (small)**](https://drive.google.com/file/d/1_V-cFMTQ4RO-_16mU9YPUE8ozsPeddCv/view?usp=sharing)
* Ouster (OS1-128) dataset. No extrinsics need to be changed for this dataset if you are using the default settings. Please follow the Ouster notes below to configure the package to run with Ouster data. A video of the dataset can be found on [YouTube](https://youtu.be/O7fKgZQzkEo):
- [**Rooftop dataset**](https://drive.google.com/file/d/1Qy2rZdPudFhDbATPpblioBb8fRtjDGQj/view?usp=sharing)
* KITTI dataset. The extrinsics can be found in the Notes KITTI section below. To generate more bags using other KITTI raw data, you can use the python script provided in "config/doc/kitti2bag".
- [**2011_09_30_drive_0028**](https://drive.google.com/file/d/12h3ooRAZVTjoMrf3uv1_KriEXm33kHc7/view?usp=sharing)
## Run the package
1. Run the launch file:
```
roslaunch lio_sam run.launch
```
2. Play existing bag files:
```
rosbag play your-bag.bag -r 3
```
## Other notes
- **Loop closure:** The loop function here gives an example of proof of concept. It is directly adapted from LeGO-LOAM loop closure. For more advanced loop closure implementation, please refer to [ScanContext](https://github.com/irapkaist/SC-LeGO-LOAM). Set the "loopClosureEnableFlag" in "params.yaml" to "true" to test the loop closure function. In Rviz, uncheck "Map (cloud)" and check "Map (global)". This is because the visualized map - "Map (cloud)" - is simply a stack of point clouds in Rviz. Their postion will not be updated after pose correction. The loop closure function here is simply adapted from LeGO-LOAM, which is an ICP-based method. Because ICP runs pretty slow, it is suggested that the playback speed is set to be "-r 1". You can try the Campus dataset (large) for testing. The loop closure happens when the sensor returns back to the original starting location.
- **Using GPS:** The park dataset is provided for testing LIO-SAM with GPS data. This dataset is gathered by [Yewei Huang](https://robustfieldautonomylab.github.io/people.html). To enable the GPS function, change "gpsTopic" in "params.yaml" to "odometry/gps". In Rviz, uncheck "Map (cloud)" and check "Map (global)". Also check "Odom GPS", which visualizes the GPS odometry. "gpsCovThreshold" can be adjusted to filter bad GPS readings. "poseCovThreshold" can be used to adjust the frequency of adding GPS factor to the graph. For example, you will notice the trajectory is constantly corrected by GPS whey you set "poseCovThreshold" to 1.0. Because of the heavy iSAM optimization, it's recommended that the playback speed is "-r 1".
- **KITTI:** Since LIO-SAM needs a high-frequency IMU for function properly, we need to use KITTI raw data for testing. One problem remains unsolved is that the intrinsics of the IMU are unknown, which has a big impact on the accuracy of LIO-SAM. Download the provided sample data and make the following changes in "params.yaml":
- extrinsicTrans: [-8.086759e-01, 3.195559e-01, -7.997231e-01]
- extrinsicRot: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]
- extrinsicRPY: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]
- N_SCAN: 64
- downsampleRate: 2 or 4
- loopClosureEnableFlag: true or false
- **Ouster lidar:** To make LIO-SAM work with Ouster lidar, some preparations needs to be done on hardware and software level.
- Hardware:
- Use an external IMU. LIO-SAM does not work with the internal 6-axis IMU of Ouster lidar. You need to attach a 9-axis IMU to the lidar and perform data-gathering.
- Configure the driver. Change "timestamp_mode" in your Ouster launch file to "TIME_FROM_PTP_1588" so you can have ROS format timestamp for the point clouds.
- Software:
- Change "timeField" in "params.yaml" to "t". "t" is the point timestamp in a scan for Ouster lidar.
- Change "N_SCAN" and "Horizon_SCAN" in "params.yaml" according to your lidar, i.e., N_SCAN=128, Horizon_SCAN=1024.
- Comment the point definition for Velodyne on top of "imageProjection.cpp".
- Uncomment the point definition for Ouster on top of "imageProjection.cpp".
- Comment line "deskewPoint(&thisPoint, laserCloudIn->points[i].time)" in "imageProjection.cpp".
- Uncomment line "deskewPoint(&thisPoint, (float)laserCloudIn->points[i].t / 1000000000.0" in "imageProjection.cpp".
- Run "catkin_make" to re-compile the package.
## Paper
Thank you for citing [LIO-SAM (IROS-2020)](./config/doc/paper.pdf) if you use any of this code.
```
@inproceedings{liosam2020shan,
title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping},
author={Shan, Tixiao and Englot, Brendan and Meyers, Drew and Wang, Wei and Ratti, Carlo and Rus Daniela},
journal={arXiv preprint arXiv:2007.00258}
year={2020}
}
```
Part of the code is adapted from [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM).
```
@inproceedings{legoloam2018shan,
title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain},
author={Shan, Tixiao and Englot, Brendan},
booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
pages={4758-4765},
year={2018},
organization={IEEE}
}
```
## TODO
- [ ] Add loop closure visualization and fix potential bug
## Acknowledgement
- LIO-SAM is based on LOAM (J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time).
================================================
FILE: config/doc/kitti2bag/README.md
================================================
# kitti2bag
## How to run it?
```bash
wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0084/2011_09_26_drive_0084_sync.zip
wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0084/2011_09_26_drive_0084_extract.zip
wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_calib.zip
unzip 2011_09_26_drive_0084_sync.zip
unzip 2011_09_26_drive_0084_extract.zip
unzip 2011_09_26_calib.zip
python kitti2bag.py -t 2011_09_26 -r 0084 raw_synced .
```
That's it. You have a bag that contains your data.
Other source files can be found at [KITTI raw data](http://www.cvlibs.net/datasets/kitti/raw_data.php) page.
================================================
FILE: config/doc/kitti2bag/kitti2bag.py
================================================
#!env python
# -*- coding: utf-8 -*-
import sys
try:
import pykitti
except ImportError as e:
print('Could not load module \'pykitti\'. Please run `pip install pykitti`')
sys.exit(1)
import tf
import os
import cv2
import rospy
import rosbag
import progressbar
from tf2_msgs.msg import TFMessage
from datetime import datetime
from std_msgs.msg import Header
from sensor_msgs.msg import CameraInfo, Imu, PointField, NavSatFix
import sensor_msgs.point_cloud2 as pcl2
from geometry_msgs.msg import TransformStamped, TwistStamped, Transform
from cv_bridge import CvBridge
import numpy as np
import argparse
def save_imu_data(bag, kitti, imu_frame_id, topic):
print("Exporting IMU")
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
q = tf.transformations.quaternion_from_euler(oxts.packet.roll, oxts.packet.pitch, oxts.packet.yaw)
imu = Imu()
imu.header.frame_id = imu_frame_id
imu.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]
imu.linear_acceleration.x = oxts.packet.af
imu.linear_acceleration.y = oxts.packet.al
imu.linear_acceleration.z = oxts.packet.au
imu.angular_velocity.x = oxts.packet.wf
imu.angular_velocity.y = oxts.packet.wl
imu.angular_velocity.z = oxts.packet.wu
bag.write(topic, imu, t=imu.header.stamp)
def save_imu_data_raw(bag, kitti, imu_frame_id, topic):
print("Exporting IMU Raw")
synced_path = kitti.data_path
unsynced_path = synced_path.replace('sync', 'extract')
imu_path = os.path.join(unsynced_path, 'oxts')
# read time stamp (convert to ros seconds format)
with open(os.path.join(imu_path, 'timestamps.txt')) as f:
lines = f.readlines()
imu_datetimes = []
for line in lines:
if len(line) == 1:
continue
timestamp = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f')
imu_datetimes.append(float(timestamp.strftime("%s.%f")))
# fix imu time using a linear model (may not be ideal, ^_^)
imu_index = np.asarray(range(len(imu_datetimes)), dtype=np.float64)
z = np.polyfit(imu_index, imu_datetimes, 1)
imu_datetimes_new = z[0] * imu_index + z[1]
imu_datetimes = imu_datetimes_new.tolist()
# get all imu data
imu_data_dir = os.path.join(imu_path, 'data')
imu_filenames = sorted(os.listdir(imu_data_dir))
imu_data = [None] * len(imu_filenames)
for i, imu_file in enumerate(imu_filenames):
imu_data_file = open(os.path.join(imu_data_dir, imu_file), "r")
for line in imu_data_file:
if len(line) == 1:
continue
stripped_line = line.strip()
line_list = stripped_line.split()
imu_data[i] = line_list
assert len(imu_datetimes) == len(imu_data)
for timestamp, data in zip(imu_datetimes, imu_data):
roll, pitch, yaw = float(data[3]), float(data[4]), float(data[5]),
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
imu = Imu()
imu.header.frame_id = imu_frame_id
imu.header.stamp = rospy.Time.from_sec(timestamp)
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]
imu.linear_acceleration.x = float(data[11])
imu.linear_acceleration.y = float(data[12])
imu.linear_acceleration.z = float(data[13])
imu.angular_velocity.x = float(data[17])
imu.angular_velocity.y = float(data[18])
imu.angular_velocity.z = float(data[19])
bag.write(topic, imu, t=imu.header.stamp)
imu.header.frame_id = 'imu_enu_link'
bag.write('/imu_correct', imu, t=imu.header.stamp) # for LIO-SAM GPS
def save_dynamic_tf(bag, kitti, kitti_type, initial_time):
print("Exporting time dependent transformations")
if kitti_type.find("raw") != -1:
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
tf_oxts_msg = TFMessage()
tf_oxts_transform = TransformStamped()
tf_oxts_transform.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
tf_oxts_transform.header.frame_id = 'world'
tf_oxts_transform.child_frame_id = 'base_link'
transform = (oxts.T_w_imu)
t = transform[0:3, 3]
q = tf.transformations.quaternion_from_matrix(transform)
oxts_tf = Transform()
oxts_tf.translation.x = t[0]
oxts_tf.translation.y = t[1]
oxts_tf.translation.z = t[2]
oxts_tf.rotation.x = q[0]
oxts_tf.rotation.y = q[1]
oxts_tf.rotation.z = q[2]
oxts_tf.rotation.w = q[3]
tf_oxts_transform.transform = oxts_tf
tf_oxts_msg.transforms.append(tf_oxts_transform)
bag.write('/tf', tf_oxts_msg, tf_oxts_msg.transforms[0].header.stamp)
elif kitti_type.find("odom") != -1:
timestamps = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
for timestamp, tf_matrix in zip(timestamps, kitti.T_w_cam0):
tf_msg = TFMessage()
tf_stamped = TransformStamped()
tf_stamped.header.stamp = rospy.Time.from_sec(timestamp)
tf_stamped.header.frame_id = 'world'
tf_stamped.child_frame_id = 'camera_left'
t = tf_matrix[0:3, 3]
q = tf.transformations.quaternion_from_matrix(tf_matrix)
transform = Transform()
transform.translation.x = t[0]
transform.translation.y = t[1]
transform.translation.z = t[2]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
tf_stamped.transform = transform
tf_msg.transforms.append(tf_stamped)
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic, initial_time):
print("Exporting camera {}".format(camera))
if kitti_type.find("raw") != -1:
camera_pad = '{0:02d}'.format(camera)
image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad))
image_path = os.path.join(image_dir, 'data')
image_filenames = sorted(os.listdir(image_path))
with open(os.path.join(image_dir, 'timestamps.txt')) as f:
image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines())
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.width, calib.height = tuple(util['S_rect_{}'.format(camera_pad)].tolist())
calib.distortion_model = 'plumb_bob'
calib.K = util['K_{}'.format(camera_pad)]
calib.R = util['R_rect_{}'.format(camera_pad)]
calib.D = util['D_{}'.format(camera_pad)]
calib.P = util['P_rect_{}'.format(camera_pad)]
elif kitti_type.find("odom") != -1:
camera_pad = '{0:01d}'.format(camera)
image_path = os.path.join(kitti.sequence_path, 'image_{}'.format(camera_pad))
image_filenames = sorted(os.listdir(image_path))
image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.P = util['P{}'.format(camera_pad)]
iterable = zip(image_datetimes, image_filenames)
bar = progressbar.ProgressBar()
for dt, filename in bar(iterable):
image_filename = os.path.join(image_path, filename)
cv_image = cv2.imread(image_filename)
calib.height, calib.width = cv_image.shape[:2]
if camera in (0, 1):
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
encoding = "mono8" if camera in (0, 1) else "bgr8"
image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
image_message.header.frame_id = camera_frame_id
if kitti_type.find("raw") != -1:
image_message.header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f")))
topic_ext = "/image_raw"
elif kitti_type.find("odom") != -1:
image_message.header.stamp = rospy.Time.from_sec(dt)
topic_ext = "/image_rect"
calib.header.stamp = image_message.header.stamp
bag.write(topic + topic_ext, image_message, t = image_message.header.stamp)
bag.write(topic + '/camera_info', calib, t = calib.header.stamp)
def save_velo_data(bag, kitti, velo_frame_id, topic):
print("Exporting velodyne data")
velo_path = os.path.join(kitti.data_path, 'velodyne_points')
velo_data_dir = os.path.join(velo_path, 'data')
velo_filenames = sorted(os.listdir(velo_data_dir))
with open(os.path.join(velo_path, 'timestamps.txt')) as f:
lines = f.readlines()
velo_datetimes = []
for line in lines:
if len(line) == 1:
continue
dt = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f')
velo_datetimes.append(dt)
iterable = zip(velo_datetimes, velo_filenames)
bar = progressbar.ProgressBar()
count = 0
for dt, filename in bar(iterable):
if dt is None:
continue
velo_filename = os.path.join(velo_data_dir, filename)
# read binary data
scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4)
# get ring channel
depth = np.linalg.norm(scan, 2, axis=1)
pitch = np.arcsin(scan[:, 2] / depth) # arcsin(z, depth)
fov_down = -24.8 / 180.0 * np.pi
fov = (abs(-24.8) + abs(2.0)) / 180.0 * np.pi
proj_y = (pitch + abs(fov_down)) / fov # in [0.0, 1.0]
proj_y *= 64 # in [0.0, H]
proj_y = np.floor(proj_y)
proj_y = np.minimum(64 - 1, proj_y)
proj_y = np.maximum(0, proj_y).astype(np.int32) # in [0,H-1]
proj_y = proj_y.reshape(-1, 1)
scan = np.concatenate((scan,proj_y), axis=1)
# create header
header = Header()
header.frame_id = velo_frame_id
header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f")))
# fill pcl msg
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1),
PointField('ring', 16, PointField.UINT16, 1)]
pcl_msg = pcl2.create_cloud(header, fields, scan)
pcl_msg.is_dense = True
# print(pcl_msg)
bag.write(topic, pcl_msg, t=pcl_msg.header.stamp)
# count += 1
# if count > 200:
# break
def get_static_transform(from_frame_id, to_frame_id, transform):
t = transform[0:3, 3]
q = tf.transformations.quaternion_from_matrix(transform)
tf_msg = TransformStamped()
tf_msg.header.frame_id = from_frame_id
tf_msg.child_frame_id = to_frame_id
tf_msg.transform.translation.x = float(t[0])
tf_msg.transform.translation.y = float(t[1])
tf_msg.transform.translation.z = float(t[2])
tf_msg.transform.rotation.x = float(q[0])
tf_msg.transform.rotation.y = float(q[1])
tf_msg.transform.rotation.z = float(q[2])
tf_msg.transform.rotation.w = float(q[3])
return tf_msg
def inv(transform):
"Invert rigid body transformation matrix"
R = transform[0:3, 0:3]
t = transform[0:3, 3]
t_inv = -1 * R.T.dot(t)
transform_inv = np.eye(4)
transform_inv[0:3, 0:3] = R.T
transform_inv[0:3, 3] = t_inv
return transform_inv
def save_static_transforms(bag, transforms, timestamps):
print("Exporting static transformations")
tfm = TFMessage()
for transform in transforms:
t = get_static_transform(from_frame_id=transform[0], to_frame_id=transform[1], transform=transform[2])
tfm.transforms.append(t)
for timestamp in timestamps:
time = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
for i in range(len(tfm.transforms)):
tfm.transforms[i].header.stamp = time
bag.write('/tf_static', tfm, t=time)
def save_gps_fix_data(bag, kitti, gps_frame_id, topic):
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
navsatfix_msg = NavSatFix()
navsatfix_msg.header.frame_id = gps_frame_id
navsatfix_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
navsatfix_msg.latitude = oxts.packet.lat
navsatfix_msg.longitude = oxts.packet.lon
navsatfix_msg.altitude = oxts.packet.alt
navsatfix_msg.status.service = 1
bag.write(topic, navsatfix_msg, t=navsatfix_msg.header.stamp)
def save_gps_vel_data(bag, kitti, gps_frame_id, topic):
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
twist_msg = TwistStamped()
twist_msg.header.frame_id = gps_frame_id
twist_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
twist_msg.twist.linear.x = oxts.packet.vf
twist_msg.twist.linear.y = oxts.packet.vl
twist_msg.twist.linear.z = oxts.packet.vu
twist_msg.twist.angular.x = oxts.packet.wf
twist_msg.twist.angular.y = oxts.packet.wl
twist_msg.twist.angular.z = oxts.packet.wu
bag.write(topic, twist_msg, t=twist_msg.header.stamp)
if __name__ == "__main__":
parser = argparse.ArgumentParser(description = "Convert KITTI dataset to ROS bag file the easy way!")
# Accepted argument values
kitti_types = ["raw_synced", "odom_color", "odom_gray"]
odometry_sequences = []
for s in range(22):
odometry_sequences.append(str(s).zfill(2))
parser.add_argument("kitti_type", choices = kitti_types, help = "KITTI dataset type")
parser.add_argument("dir", nargs = "?", default = os.getcwd(), help = "base directory of the dataset, if no directory passed the deafult is current working directory")
parser.add_argument("-t", "--date", help = "date of the raw dataset (i.e. 2011_09_26), option is only for RAW datasets.")
parser.add_argument("-r", "--drive", help = "drive number of the raw dataset (i.e. 0001), option is only for RAW datasets.")
parser.add_argument("-s", "--sequence", choices = odometry_sequences,help = "sequence of the odometry dataset (between 00 - 21), option is only for ODOMETRY datasets.")
args = parser.parse_args()
bridge = CvBridge()
compression = rosbag.Compression.NONE
# compression = rosbag.Compression.BZ2
# compression = rosbag.Compression.LZ4
# CAMERAS
cameras = [
(0, 'camera_gray_left', '/kitti/camera_gray_left'),
(1, 'camera_gray_right', '/kitti/camera_gray_right'),
(2, 'camera_color_left', '/kitti/camera_color_left'),
(3, 'camera_color_right', '/kitti/camera_color_right')
]
if args.kitti_type.find("raw") != -1:
if args.date == None:
print("Date option is not given. It is mandatory for raw dataset.")
print("Usage for raw dataset: kitti2bag raw_synced [dir] -t -r ")
sys.exit(1)
elif args.drive == None:
print("Drive option is not given. It is mandatory for raw dataset.")
print("Usage for raw dataset: kitti2bag raw_synced [dir] -t -r ")
sys.exit(1)
bag = rosbag.Bag("kitti_{}_drive_{}_{}.bag".format(args.date, args.drive, args.kitti_type[4:]), 'w', compression=compression)
kitti = pykitti.raw(args.dir, args.date, args.drive)
if not os.path.exists(kitti.data_path):
print('Path {} does not exists. Exiting.'.format(kitti.data_path))
sys.exit(1)
if len(kitti.timestamps) == 0:
print('Dataset is empty? Exiting.')
sys.exit(1)
try:
# IMU
imu_frame_id = 'imu_link'
imu_topic = '/kitti/oxts/imu'
imu_raw_topic = '/imu_raw'
gps_fix_topic = '/gps/fix'
gps_vel_topic = '/gps/vel'
velo_frame_id = 'velodyne'
velo_topic = '/points_raw'
T_base_link_to_imu = np.eye(4, 4)
T_base_link_to_imu[0:3, 3] = [-2.71/2.0-0.05, 0.32, 0.93]
# tf_static
transforms = [
('base_link', imu_frame_id, T_base_link_to_imu),
(imu_frame_id, velo_frame_id, inv(kitti.calib.T_velo_imu)),
(imu_frame_id, cameras[0][1], inv(kitti.calib.T_cam0_imu)),
(imu_frame_id, cameras[1][1], inv(kitti.calib.T_cam1_imu)),
(imu_frame_id, cameras[2][1], inv(kitti.calib.T_cam2_imu)),
(imu_frame_id, cameras[3][1], inv(kitti.calib.T_cam3_imu))
]
util = pykitti.utils.read_calib_file(os.path.join(kitti.calib_path, 'calib_cam_to_cam.txt'))
# Export
# save_static_transforms(bag, transforms, kitti.timestamps)
# save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=None)
# save_imu_data(bag, kitti, imu_frame_id, imu_topic)
save_imu_data_raw(bag, kitti, imu_frame_id, imu_raw_topic)
save_gps_fix_data(bag, kitti, imu_frame_id, gps_fix_topic)
save_gps_vel_data(bag, kitti, imu_frame_id, gps_vel_topic)
for camera in cameras:
save_camera_data(bag, args.kitti_type, kitti, util, bridge, camera=camera[0], camera_frame_id=camera[1], topic=camera[2], initial_time=None)
break
save_velo_data(bag, kitti, velo_frame_id, velo_topic)
finally:
print("## OVERVIEW ##")
print(bag)
bag.close()
elif args.kitti_type.find("odom") != -1:
if args.sequence == None:
print("Sequence option is not given. It is mandatory for odometry dataset.")
print("Usage for odometry dataset: kitti2bag {odom_color, odom_gray} [dir] -s ")
sys.exit(1)
bag = rosbag.Bag("kitti_data_odometry_{}_sequence_{}.bag".format(args.kitti_type[5:],args.sequence), 'w', compression=compression)
kitti = pykitti.odometry(args.dir, args.sequence)
if not os.path.exists(kitti.sequence_path):
print('Path {} does not exists. Exiting.'.format(kitti.sequence_path))
sys.exit(1)
kitti.load_calib()
kitti.load_timestamps()
if len(kitti.timestamps) == 0:
print('Dataset is empty? Exiting.')
sys.exit(1)
if args.sequence in odometry_sequences[:11]:
print("Odometry dataset sequence {} has ground truth information (poses).".format(args.sequence))
kitti.load_poses()
try:
util = pykitti.utils.read_calib_file(os.path.join(args.dir,'sequences',args.sequence, 'calib.txt'))
current_epoch = (datetime.utcnow() - datetime(1970, 1, 1)).total_seconds()
# Export
if args.kitti_type.find("gray") != -1:
used_cameras = cameras[:2]
elif args.kitti_type.find("color") != -1:
used_cameras = cameras[-2:]
save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=current_epoch)
for camera in used_cameras:
save_camera_data(bag, args.kitti_type, kitti, util, bridge, camera=camera[0], camera_frame_id=camera[1], topic=camera[2], initial_time=current_epoch)
finally:
print("## OVERVIEW ##")
print(bag)
bag.close()
================================================
FILE: config/params.yaml
================================================
lio_sam:
# Topics
pointCloudTopic: "points_raw" # Point cloud data
imuTopic: "imu_correct" # IMU data
odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
# GPS Settings
useImuHeadingInitialization: true # if using GPS data, set to "true"
useGpsElevation: false # if GPS elevation is bad, set to "false"
gpsCovThreshold: 2.0 # m^2, threshold for using GPS data
poseCovThreshold: 25.0 # m^2, threshold for using GPS data
# Export settings
savePCD: false # https://github.com/TixiaoShan/LIO-SAM/issues/3
savePCDDirectory: "/Downloads/LOAM/" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
# Sensor Settings
N_SCAN: 16 # number of lidar channel (i.e., 16, 32, 64, 128)
Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
timeField: "time" # point timestamp field, Velodyne - "time", Ouster - "t"
downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
# IMU Settings
imuAccNoise: 3.9939570888238808e-03
imuGyrNoise: 1.5636343949698187e-03
imuAccBiasN: 6.4356659353532566e-05
imuGyrBiasN: 3.5640318696367613e-05
imuGravity: 9.80511
# Extrinsics (lidar -> IMU)
extrinsicTrans: [0.0, 0.0, 0.0]
extrinsicRot: [1, 0, 0,
0, 1, 0,
0, 0, 1]
extrinsicRPY: [1, 0, 0,
0, 1, 0,
0, 0, 1]
# extrinsicRot: [1, 0, 0,
# 0, 1, 0,
# 0, 0, 1]
# extrinsicRPY: [1, 0, 0,
# 0, 1, 0,
# 0, 0, 1]
# LOAM feature threshold
edgeThreshold: 1.0
surfThreshold: 0.1
edgeFeatureMinValidNum: 10
surfFeatureMinValidNum: 100
# voxel filter paprams
odometrySurfLeafSize: 0.4 # default: 0.4
mappingCornerLeafSize: 0.2 # default: 0.2
mappingSurfLeafSize: 0.4 # default: 0.4
# robot motion constraint (in case you are using a 2D robot)
z_tollerance: 1000 # meters
rotation_tollerance: 1000 # radians
# CPU Params
numberOfCores: 4 # number of cores for mapping optimization
mappingProcessInterval: 0.15 # seconds, regulate mapping frequency
# Surrounding map
surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold
surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold
surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses
surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled)
# Loop closure
loopClosureEnableFlag: false
surroundingKeyframeSize: 25 # submap size (when loop closure enabled)
historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure
historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure
historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure
historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment
# Visualization
globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius
globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density
globalMapVisualizationLeafSize: 1.0 # meters, global map visualization cloud density
# Navsat (convert GPS coordinates to Cartesian)
navsat:
frequency: 50
wait_for_datum: false
delay: 0.0
magnetic_declination_radians: 0
yaw_offset: 0
zero_altitude: true
broadcast_utm_transform: false
broadcast_utm_transform_as_parent_frame: false
publish_filtered_gps: false
# EKF for Navsat
ekf_gps:
publish_tf: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
frequency: 50
two_d_mode: false
sensor_timeout: 0.01
# -------------------------------------
# External IMU:
# -------------------------------------
imu0: imu_correct
# make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
imu0_config: [false, false, false,
true, true, true,
false, false, false,
false, false, true,
true, true, true]
imu0_differential: false
imu0_queue_size: 50
imu0_remove_gravitational_acceleration: true
# -------------------------------------
# Odometry (From Navsat):
# -------------------------------------
odom0: odometry/gps
odom0_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom0_differential: false
odom0_queue_size: 10
# x y z r p y x_dot y_dot z_dot r_dot p_dot y_dot x_ddot y_ddot z_ddot
process_noise_covariance: [ 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 10.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
================================================
FILE: include/utility.h
================================================
#pragma once
#ifndef _UTILITY_LIDAR_ODOMETRY_H_
#define _UTILITY_LIDAR_ODOMETRY_H_
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
typedef pcl::PointXYZI PointType;
class ParamServer
{
public:
ros::NodeHandle nh;
std::string robot_id;
string pointCloudTopic;
string imuTopic;
string odomTopic;
string gpsTopic;
// GPS Settings
bool useImuHeadingInitialization;
bool useGpsElevation;
float gpsCovThreshold;
float poseCovThreshold;
// Save pcd
bool savePCD;
string savePCDDirectory;
// Velodyne Sensor Configuration: Velodyne
int N_SCAN;
int Horizon_SCAN;
string timeField;
int downsampleRate;
// IMU
float imuAccNoise;
float imuGyrNoise;
float imuAccBiasN;
float imuGyrBiasN;
float imuGravity;
vector extRotV;
vector extRPYV;
vector extTransV;
Eigen::Matrix3d extRot;
Eigen::Matrix3d extRPY;
Eigen::Vector3d extTrans;
Eigen::Quaterniond extQRPY;
// LOAM
float edgeThreshold;
float surfThreshold;
int edgeFeatureMinValidNum;
int surfFeatureMinValidNum;
// voxel filter paprams
float odometrySurfLeafSize;
float mappingCornerLeafSize;
float mappingSurfLeafSize ;
float z_tollerance;
float rotation_tollerance;
// CPU Params
int numberOfCores;
double mappingProcessInterval;
// Surrounding map
float surroundingkeyframeAddingDistThreshold;
float surroundingkeyframeAddingAngleThreshold;
float surroundingKeyframeDensity;
float surroundingKeyframeSearchRadius;
// Loop closure
bool loopClosureEnableFlag;
int surroundingKeyframeSize;
float historyKeyframeSearchRadius;
float historyKeyframeSearchTimeDiff;
int historyKeyframeSearchNum;
float historyKeyframeFitnessScore;
// global map visualization radius
float globalMapVisualizationSearchRadius;
float globalMapVisualizationPoseDensity;
float globalMapVisualizationLeafSize;
ParamServer()
{
nh.param("/robot_id", robot_id, "roboat");
nh.param("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
nh.param("lio_sam/imuTopic", imuTopic, "imu_correct");
nh.param("lio_sam/odomTopic", odomTopic, "odometry/imu");
nh.param("lio_sam/gpsTopic", gpsTopic, "odometry/gps");
nh.param("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false);
nh.param("lio_sam/useGpsElevation", useGpsElevation, false);
nh.param("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0);
nh.param("lio_sam/poseCovThreshold", poseCovThreshold, 25.0);
nh.param("lio_sam/savePCD", savePCD, false);
nh.param("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/");
nh.param("lio_sam/N_SCAN", N_SCAN, 16);
nh.param("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800);
nh.param("lio_sam/timeField", timeField, "time");
nh.param("lio_sam/downsampleRate", downsampleRate, 1);
nh.param("lio_sam/imuAccNoise", imuAccNoise, 0.01);
nh.param("lio_sam/imuGyrNoise", imuGyrNoise, 0.001);
nh.param("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002);
nh.param("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003);
nh.param("lio_sam/imuGravity", imuGravity, 9.80511);
nh.param>("lio_sam/extrinsicRot", extRotV, vector());
nh.param>("lio_sam/extrinsicRPY", extRPYV, vector());
nh.param>("lio_sam/extrinsicTrans", extTransV, vector());
extRot = Eigen::Map>(extRotV.data(), 3, 3);
extRPY = Eigen::Map>(extRPYV.data(), 3, 3);
extTrans = Eigen::Map>(extTransV.data(), 3, 1);
extQRPY = Eigen::Quaterniond(extRPY);
nh.param("lio_sam/edgeThreshold", edgeThreshold, 0.1);
nh.param("lio_sam/surfThreshold", surfThreshold, 0.1);
nh.param("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
nh.param("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);
nh.param("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2);
nh.param("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2);
nh.param("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2);
nh.param("lio_sam/z_tollerance", z_tollerance, FLT_MAX);
nh.param("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX);
nh.param("lio_sam/numberOfCores", numberOfCores, 2);
nh.param("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15);
nh.param("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
nh.param("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
nh.param("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);
nh.param("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);
nh.param("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false);
nh.param("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50);
nh.param("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);
nh.param("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);
nh.param("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);
nh.param("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);
nh.param("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
nh.param("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
nh.param("lio_sam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);
usleep(100);
}
//gc: tranform to lidar frame
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{
sensor_msgs::Imu imu_out = imu_in;
// rotate acceleration
Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
acc = extRot * acc;
imu_out.linear_acceleration.x = acc.x();
imu_out.linear_acceleration.y = acc.y();
imu_out.linear_acceleration.z = acc.z();
// rotate gyroscope
Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
gyr = extRot * gyr;
imu_out.angular_velocity.x = gyr.x();
imu_out.angular_velocity.y = gyr.y();
imu_out.angular_velocity.z = gyr.z();
// rotate roll pitch yaw
Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
Eigen::Quaterniond q_final = q_from * extQRPY;
imu_out.orientation.x = q_final.x();
imu_out.orientation.y = q_final.y();
imu_out.orientation.z = q_final.z();
imu_out.orientation.w = q_final.w();
if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
{
ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
ros::shutdown();
}
return imu_out;
}
};
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
{
sensor_msgs::PointCloud2 tempCloud;
pcl::toROSMsg(*thisCloud, tempCloud);
tempCloud.header.stamp = thisStamp;
tempCloud.header.frame_id = thisFrame;
if (thisPub->getNumSubscribers() != 0)
thisPub->publish(tempCloud);
return tempCloud;
}
template
double ROS_TIME(T msg)
{
return msg->header.stamp.toSec();
}
template
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{
*angular_x = thisImuMsg->angular_velocity.x;
*angular_y = thisImuMsg->angular_velocity.y;
*angular_z = thisImuMsg->angular_velocity.z;
}
template
void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
{
*acc_x = thisImuMsg->linear_acceleration.x;
*acc_y = thisImuMsg->linear_acceleration.y;
*acc_z = thisImuMsg->linear_acceleration.z;
}
template
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
double imuRoll, imuPitch, imuYaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
*rosRoll = imuRoll;
*rosPitch = imuPitch;
*rosYaw = imuYaw;
}
float pointDistance(PointType p)
{
return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}
float pointDistance(PointType p1, PointType p2)
{
return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
}
#endif
================================================
FILE: launch/include/config/robot.urdf.xacro
================================================
================================================
FILE: launch/include/config/rviz.rviz
================================================
Panels:
- Class: rviz/Displays
Help Height: 70
Name: Displays
Property Tree Widget:
Expanded:
- /links1
- /Odometry1
- /Point Cloud1
- /Point Cloud1/Velodyne1
- /Feature Cloud1
- /Mapping1
- /Grid1
- /PointCloud21
- /PointCloud22
- /PointCloud23
Splitter Ratio: 0.5600000023841858
Tree Height: 292
- Class: rviz/Selection
Name: Selection
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
Name: Tool Properties
Splitter Ratio: 0.5
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Class: rviz/Group
Displays:
- Class: rviz/TF
Enabled: false
Frame Timeout: 999
Frames:
All Enabled: false
Marker Scale: 3
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Class: rviz/Axes
Enabled: true
Length: 2
Name: map
Radius: 0.5
Reference Frame: map
Value: true
- Class: rviz/Axes
Enabled: true
Length: 1
Name: base_link
Radius: 0.30000001192092896
Reference Frame: base_link
Value: true
Enabled: true
Name: links
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: false
Enabled: false
Keep: 300
Name: Odo IMU
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 0.25
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /odometry/imu
Unreliable: false
Value: false
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: false
Enabled: true
Keep: 300
Name: Odom GPS
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.30000001192092896
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /odometry/gps
Unreliable: false
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: false
Enabled: false
Keep: 300
Name: Odom lidar
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 0.25
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /lio_sam/mapping/odometry
Unreliable: false
Value: false
Enabled: false
Name: Odometry
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 18.805150985717773
Min Value: -3.3611395359039307
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 255
Min Color: 0; 0; 0
Min Intensity: 0
Name: Velodyne
Position Transformer: XYZ
Queue Size: 10
Selectable: false
Size (Pixels): 2
Size (m): 0.009999999776482582
Style: Points
Topic: /lio_sam/deskew/cloud_deskewed
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0.7900000214576721
Min Color: 0; 0; 0
Min Intensity: 0
Name: Reg Cloud
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /lio_sam/mapping/cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Name: Point Cloud
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 0; 255; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 15.077729225158691
Min Color: 0; 0; 0
Min Intensity: 0.019985778257250786
Name: Edge Feature
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 5
Size (m): 0.009999999776482582
Style: Points
Topic: /lio_sam/feature/cloud_corner
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 85; 255
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 15.100607872009277
Min Color: 0; 0; 0
Min Intensity: 0.0007188677554950118
Name: Plannar Feature
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /lio_sam/feature/cloud_surface
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: false
Name: Feature Cloud
- Class: rviz/Group
Displays:
- Alpha: 0.30000001192092896
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 7.4858574867248535
Min Value: -1.2309398651123047
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 500
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 101.66666412353516
Min Color: 0; 0; 0
Min Intensity: 1
Name: Map (cloud)
Position Transformer: XYZ
Queue Size: 10
Selectable: false
Size (Pixels): 2
Size (m): 0.009999999776482582
Style: Points
Topic: /lio_sam/mapping/cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 23.98253059387207
Min Value: -2.447427749633789
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 15.100604057312012
Min Color: 0; 0; 0
Min Intensity: 0.014545874670147896
Name: Map (local)
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 2
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /lio_sam/mapping/map_local
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 15.100096702575684
Min Color: 0; 0; 0
Min Intensity: 0.007923072203993797
Name: Map (global)
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 2
Size (m): 0.009999999776482582
Style: Points
Topic: /lio_sam/mapping/map_global
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 85; 255; 255
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.10000000149011612
Name: Path (global)
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /lio_sam/mapping/path
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 0; 127
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.10000000149011612
Name: Path (local)
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /lio_sam/imu/path
Unreliable: false
Value: true
Enabled: true
Name: Mapping
- 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: 200
Reference Frame:
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: x
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 182.80648803710938
Min Color: 0; 0; 0
Min Intensity: -118.49858856201172
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.20000000298023224
Style: Flat Squares
Topic: /lio_sam/mapping/cloud_map_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 120
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.5
Style: Flat Squares
Topic: /lio_sam/mapping/lasercloud_in_world
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 19.07742691040039
Min Value: -7.043781757354736
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: -999999
Min Color: 0; 0; 0
Min Intensity: 999999
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.30000001192092896
Style: Flat Squares
Topic: /lio_sam/mapping/laserclouinmapframe
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- 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
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 133.46669006347656
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5.347261428833008
Y: 2.5245871543884277
Z: -9.126422882080078
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.1797966957092285
Target Frame: base_link
Value: Orbit (rviz)
Yaw: 4.47352409362793
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 713
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000300000000000001a30000026ffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000002e000001340000000000000000fb000000100044006900730070006c006100790073010000003d000001a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000286000000a70000005c00fffffffb0000000a0056006900650077007301000001ea000000c2000000a400ffffff000000010000011100000435fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002e00000435000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000003000004b000000040fc0100000002fb0000000800540069006d00650000000000000004b00000000000000000fb0000000800540069006d006501000000000000045000000000000000000000036a0000026f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1299
X: 67
Y: 27
================================================
FILE: launch/include/module_loam.launch
================================================
================================================
FILE: launch/include/module_navsat.launch
================================================
================================================
FILE: launch/include/module_relocolize.launch
================================================
================================================
FILE: launch/include/module_robot_state_publisher.launch
================================================
================================================
FILE: launch/include/module_rviz.launch
================================================
================================================
FILE: launch/include/rosconsole/rosconsole_error.conf
================================================
# Set the default ros output to warning and higher
log4j.logger.ros = ERROR
================================================
FILE: launch/include/rosconsole/rosconsole_info.conf
================================================
# Set the default ros output to warning and higher
log4j.logger.ros = INFO
================================================
FILE: launch/include/rosconsole/rosconsole_warn.conf
================================================
# Set the default ros output to warning and higher
log4j.logger.ros = WARN
================================================
FILE: launch/run.launch
================================================
================================================
FILE: launch/run_relocalize.launch
================================================
================================================
FILE: msg/cloud_info.msg
================================================
# Cloud Info
Header header
int32[] startRingIndex
int32[] endRingIndex
int32[] pointColInd # point column index in range image
float32[] pointRange # point range
int64 imuAvailable
int64 odomAvailable
# Attitude for LOAM initialization
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit
# Initial guess from imu pre-integration
float32 initialGuessX
float32 initialGuessY
float32 initialGuessZ
float32 initialGuessRoll
float32 initialGuessPitch
float32 initialGuessYaw
# Preintegration reset ID
int64 imuPreintegrationResetId
# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed
sensor_msgs/PointCloud2 cloud_corner # extracted corner feature
sensor_msgs/PointCloud2 cloud_surface # extracted surface feature
================================================
FILE: package.xml
================================================
lio_sam1.0.0Lidar OdometryTixiao ShanTODOTixiao Shancatkinroscpproscpprospyrospytftfcv_bridgecv_bridgepcl_conversionspcl_conversionsstd_msgsstd_msgssensor_msgssensor_msgsgeometry_msgsgeometry_msgsnav_msgsnav_msgsmessage_generationmessage_generationmessage_runtimemessage_runtimeGTSAMGTSAM
================================================
FILE: src/featureExtraction.cpp
================================================
#include "utility.h"
#include "lio_sam/cloud_info.h"
struct smoothness_t{
float value;
size_t ind;
};
struct by_value{
bool operator()(smoothness_t const &left, smoothness_t const &right) {
return left.value < right.value;
}
};
class FeatureExtraction : public ParamServer
{
public:
ros::Subscriber subLaserCloudInfo;
ros::Publisher pubLaserCloudInfo;
ros::Publisher pubCornerPoints;
ros::Publisher pubSurfacePoints;
pcl::PointCloud::Ptr extractedCloud;
pcl::PointCloud::Ptr cornerCloud;
pcl::PointCloud::Ptr surfaceCloud;
pcl::VoxelGrid downSizeFilter;
lio_sam::cloud_info cloudInfo;
std_msgs::Header cloudHeader;
std::vector cloudSmoothness;
float *cloudCurvature;
int *cloudNeighborPicked;
int *cloudLabel;
FeatureExtraction()
{
subLaserCloudInfo = nh.subscribe("lio_sam/deskew/cloud_info", 10, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
pubLaserCloudInfo = nh.advertise ("lio_sam/feature/cloud_info", 10);
pubCornerPoints = nh.advertise("lio_sam/feature/cloud_corner", 1);
pubSurfacePoints = nh.advertise("lio_sam/feature/cloud_surface", 1);
initializationValue();
}
void initializationValue()
{
cloudSmoothness.resize(N_SCAN*Horizon_SCAN);
downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);
extractedCloud.reset(new pcl::PointCloud());
cornerCloud.reset(new pcl::PointCloud());
surfaceCloud.reset(new pcl::PointCloud());
cloudCurvature = new float[N_SCAN*Horizon_SCAN];
cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];
cloudLabel = new int[N_SCAN*Horizon_SCAN];
}
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
cloudInfo = *msgIn; // new cloud info
cloudHeader = msgIn->header; // new cloud header
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
calculateSmoothness();//gc: calculate curvature
markOccludedPoints();//gc: check unstable points
extractFeatures();//gc: extract features
publishFeatureCloud();
}
//gc:
void calculateSmoothness()
{
int cloudSize = extractedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++)
{
float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
+ cloudInfo.pointRange[i+5];
cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
// cloudSmoothness for sorting
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// mark occluded points and parallel beam points
for (int i = 5; i < cloudSize - 6; ++i)
{
// occluded points
float depth1 = cloudInfo.pointRange[i];
float depth2 = cloudInfo.pointRange[i+1];
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
if (columnDiff < 10){
// 10 pixel diff in range image
if (depth1 - depth2 > 0.3){
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}else if (depth2 - depth1 > 0.3){
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
// parallel beam
//gc:points on local planar surfaces that are roughly parallel to laser beam
float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
cloudNeighborPicked[i] = 1;
}
}
void extractFeatures()
{
cornerCloud->clear();
surfaceCloud->clear();
pcl::PointCloud::Ptr surfaceCloudScan(new pcl::PointCloud());
pcl::PointCloud::Ptr surfaceCloudScanDS(new pcl::PointCloud());
//gc: for every ring
for (int i = 0; i < N_SCAN; i++)
{
surfaceCloudScan->clear();
//gc: for every section
for (int j = 0; j < 6; j++)
{
int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)
continue;
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--)
{
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
{
largestPickedNum++;
if (largestPickedNum <= 20){
cloudLabel[ind] = 1;
cornerCloud->push_back(extractedCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
for (int k = sp; k <= ep; k++)
{
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
{
cloudLabel[ind] = -1;
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0){
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
}
surfaceCloudScanDS->clear();
downSizeFilter.setInputCloud(surfaceCloudScan);//gc: only surface points should be down sample
downSizeFilter.filter(*surfaceCloudScanDS);
*surfaceCloud += *surfaceCloudScanDS;
}
}
void freeCloudInfoMemory()
{
cloudInfo.startRingIndex.clear();
cloudInfo.endRingIndex.clear();
cloudInfo.pointColInd.clear();
cloudInfo.pointRange.clear();
}
void publishFeatureCloud()
{
// free cloud info memory
freeCloudInfoMemory();
// save newly extracted features
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, "base_link");
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, "base_link");
// publish to mapOptimization
pubLaserCloudInfo.publish(cloudInfo);
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
FeatureExtraction FE;
ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");
ros::spin();
return 0;
}
================================================
FILE: src/globalLocalize.cpp
================================================
//
// Created by gc2 on 20-7-20.
//
#include "utility.h"
#include "lio_sam/cloud_info.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace gtsam;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using symbol_shorthand::G; // GPS pose
/*
* A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
*/
struct PointXYZIRPYT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
(double, time, time))
typedef PointXYZIRPYT PointTypePose;
class mapOptimization : public ParamServer
{
public:
// gtsam
NonlinearFactorGraph gtSAMgraph;
Values initialEstimate;
Values optimizedEstimate;
ISAM2 *isam;
Values isamCurrentEstimate;
Eigen::MatrixXd poseCovariance;
ros::Publisher pubLaserCloudSurround;
ros::Publisher pubOdomAftMappedROS;
ros::Publisher pubKeyPoses;
ros::Publisher pubPath;
ros::Publisher pubHistoryKeyFrames;
ros::Publisher pubIcpKeyFrames;
ros::Publisher pubRecentKeyFrames;
ros::Publisher pubRecentKeyFrame;
ros::Publisher pubCloudRegisteredRaw;
ros::Subscriber subLaserCloudInfo;
ros::Subscriber subGPS;
std::deque gpsQueue;
lio_sam::cloud_info cloudInfo;
vector::Ptr> cornerCloudKeyFrames;
vector::Ptr> surfCloudKeyFrames;
pcl::PointCloud::Ptr cloudKeyPoses3D;//gc: can be used to illustrate the path of odometry // keep
pcl::PointCloud::Ptr cloudKeyPoses6D;//gc: can be used to illustrate the path of odometry //keep
//addded**********************************by gc
std::mutex mtxWin;
std::vector win_cloudKeyPoses3D;
std::vector win_cloudKeyPoses6D;
std::vector::Ptr> win_cornerCloudKeyFrames;
std::vector::Ptr> win_surfCloudKeyFrames;
//added***********************************by gc
pcl::PointCloud::Ptr laserCloudCornerLast; // corner feature set from odoOptimization
pcl::PointCloud::Ptr laserCloudSurfLast; // surf feature set from odoOptimization
pcl::PointCloud::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
pcl::PointCloud::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization
pcl::PointCloud::Ptr laserCloudOri;
pcl::PointCloud::Ptr coeffSel;
std::vector laserCloudOriCornerVec; // corner point holder for parallel computation
std::vector coeffSelCornerVec;
std::vector laserCloudOriCornerFlag;
std::vector laserCloudOriSurfVec; // surf point holder for parallel computation
std::vector coeffSelSurfVec;
std::vector laserCloudOriSurfFlag;
pcl::PointCloud::Ptr laserCloudCornerFromMap;
pcl::PointCloud::Ptr laserCloudSurfFromMap;
pcl::PointCloud::Ptr laserCloudCornerFromMapDS;
pcl::PointCloud::Ptr laserCloudSurfFromMapDS;
pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap;
pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap;
//pcl::KdTreeFLANN::Ptr kdtreeSurroundingKeyPoses;
//pcl::KdTreeFLANN::Ptr kdtreeHistoryKeyPoses;
pcl::PointCloud::Ptr latestKeyFrameCloud;
pcl::PointCloud::Ptr nearHistoryKeyFrameCloud;
pcl::VoxelGrid downSizeFilterCorner;
pcl::VoxelGrid downSizeFilterSurf;
pcl::VoxelGrid downSizeFilterICP;
pcl::VoxelGrid downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
ros::Time timeLaserInfoStamp;
double timeLaserCloudInfoLast;
float transformTobeMapped[6];
std::mutex mtx;
double timeLastProcessing = -1;
bool isDegenerate = false;
Eigen::Matrix matP;
int winSize = 30;
int laserCloudCornerFromMapDSNum = 0;
int laserCloudSurfFromMapDSNum = 0;
int laserCloudCornerLastDSNum = 0;
int laserCloudSurfLastDSNum = 0;
bool aLoopIsClosed = false;
int imuPreintegrationResetId = 0;
nav_msgs::Path globalPath;
Eigen::Affine3f transPointAssociateToMap;
/*************added by gc*****************/
pcl::PointCloud::Ptr cloudGlobalMap;
pcl::PointCloud::Ptr cloudGlobalMapDS;
pcl::PointCloud::Ptr cloudScanForInitialize;
ros::Subscriber subIniPoseFromRviz;
ros::Publisher pubLaserCloudInWorld;
ros::Publisher pubMapWorld;
//ros::Publisher fortest_publasercloudINWorld;
float transformInTheWorld[6];// the pose in the world, i.e. the prebuilt map
float tranformOdomToWorld[6];
int globalLocaSkipFrames = 3;
int frameNum = 1;
tf::TransformBroadcaster tfOdom2Map;
std::mutex mtxtranformOdomToWorld;
std::mutex mtx_general;
bool globalLocalizeInitialiized = false;
ros::Subscriber subImu;
enum InitializedFlag
{
NonInitialized,
Initializing,
Initialized
};
InitializedFlag initializedFlag;
geometry_msgs::PoseStamped poseOdomToMap;
ros::Publisher pubOdomToMapPose;
/*************added by gc******************/
mapOptimization()
{
//std::cout << "come in" << std::endl;
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.1;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);
pubKeyPoses = nh.advertise("lio_sam/mapping/trajectory", 1);
pubLaserCloudSurround = nh.advertise("lio_sam/mapping/map_global", 1);
pubOdomAftMappedROS = nh.advertise ("lio_sam/mapping/odometry", 1);
pubPath = nh.advertise("lio_sam/mapping/path", 1);
subLaserCloudInfo = nh.subscribe("lio_sam/feature/cloud_info", 10, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
subGPS = nh.subscribe (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
//std::cout << "come in2" << std::endl;
//added ******************by gc
subIniPoseFromRviz = nh.subscribe("/initialpose", 8, &mapOptimization::initialpose_callback, this);
pubMapWorld = nh.advertise("lio_sam/mapping/cloud_map_map",1);//
//fortest_publasercloudINWorld = nh.advertise("lio_sam/mapping/laserclouinmapframe",1);
pubLaserCloudInWorld = nh.advertise("lio_sam/mapping/lasercloud_in_world", 1);//added
pubOdomToMapPose = nh.advertise("lio_sam/mapping/pose_odomTo_map", 1);
//subImu = nh.subscribe (imuTopic, 200, &mapOptimization::imuHandler, this, ros::TransportHints().tcpNoDelay());
//added ******************by gc
pubHistoryKeyFrames = nh.advertise("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
pubIcpKeyFrames = nh.advertise("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);
pubRecentKeyFrames = nh.advertise("lio_sam/mapping/map_local", 1);
pubRecentKeyFrame = nh.advertise("lio_sam/mapping/cloud_registered", 1);
pubCloudRegisteredRaw = nh.advertise("lio_sam/mapping/cloud_registered_raw", 1);
downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization
//std::cout << "come in3" << std::endl;
allocateMemory();
}
void allocateMemory()
{
cloudGlobalMap.reset(new pcl::PointCloud());//addded by gc
cloudGlobalMapDS.reset(new pcl::PointCloud());//added
cloudScanForInitialize.reset(new pcl::PointCloud());
resetLIO();
//added by gc
for (int i = 0; i < 6; ++i){
transformInTheWorld[i] = 0;
}
for (int i = 0; i < 6; ++i){
tranformOdomToWorld[i] = 0;
}
initializedFlag = NonInitialized;
cloudGlobalLoad();//added by gc
//added by gc
}
void resetLIO()
{
cloudKeyPoses3D.reset(new pcl::PointCloud());
cloudKeyPoses6D.reset(new pcl::PointCloud());
//kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN());
//kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN());
laserCloudCornerLast.reset(new pcl::PointCloud()); // corner feature set from odoOptimization
laserCloudSurfLast.reset(new pcl::PointCloud()); // surf feature set from odoOptimization
laserCloudCornerLastDS.reset(new pcl::PointCloud()); // downsampled corner featuer set from odoOptimization
laserCloudSurfLastDS.reset(new pcl::PointCloud()); // downsampled surf featuer set from odoOptimization
laserCloudOri.reset(new pcl::PointCloud());
coeffSel.reset(new pcl::PointCloud());
laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
laserCloudCornerFromMap.reset(new pcl::PointCloud());
laserCloudSurfFromMap.reset(new pcl::PointCloud());
laserCloudCornerFromMapDS.reset(new pcl::PointCloud());
laserCloudSurfFromMapDS.reset(new pcl::PointCloud());
kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN());
kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN());
latestKeyFrameCloud.reset(new pcl::PointCloud());
nearHistoryKeyFrameCloud.reset(new pcl::PointCloud());
for (int i = 0; i < 6; ++i){
transformTobeMapped[i] = 0;
}
matP.setZero();
}
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
// extract time stamp
//added
timeLaserInfoStamp = msgIn->header.stamp;
timeLaserCloudInfoLast = msgIn->header.stamp.toSec();
// extract info and feature cloud
cloudInfo = *msgIn;
pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
/************************************added by gc*****************************/
//if the sysytem is not initialized ffer the first scan for the system to initialize
//the LIO system stsrt working only when the localization initializing is finished
if(initializedFlag == NonInitialized || initializedFlag == Initializing)
{
if(cloudScanForInitialize->points.size() == 0)
{
downsampleCurrentScan();
mtx_general.lock();
*cloudScanForInitialize += *laserCloudCornerLastDS;
*cloudScanForInitialize += *laserCloudSurfLastDS;
mtx_general.unlock();
laserCloudCornerLastDS->clear();
laserCloudSurfLastDS->clear();
laserCloudCornerLastDSNum = 0;
laserCloudSurfLastDSNum = 0;
transformTobeMapped[0] = cloudInfo.imuRollInit;
transformTobeMapped[1] = cloudInfo.imuPitchInit;
transformTobeMapped[2] = cloudInfo.imuYawInit;
if (!useImuHeadingInitialization)//gc: if not use the heading of init_IMU as Initialization
transformTobeMapped[2] = 0;
}
return;
}
frameNum++;
/************************************added by gc*****************************/
std::lock_guard lock(mtx);
if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval) {//gc:control the rate of mapping process
timeLastProcessing = timeLaserCloudInfoLast;
updateInitialGuess();//gc: update initial value for states
extractSurroundingKeyFrames();//gc:
downsampleCurrentScan();//gc:down sample the current corner points and surface points
scan2MapOptimization();//gc: calculate the tranformtion using lidar measurement with the Imu preintegration as initial values
//and then interpolate roll and pitch angle using IMU measurement and above measurement
saveKeyFramesAndFactor();//gc: save corner cloud and surface cloud of this scan, and add odom and GPS factors
//correctPoses();
publishOdometry();
publishFrames();
}
}
void gpsHandler(const nav_msgs::Odometry::ConstPtr& gpsMsg)
{
gpsQueue.push_back(*gpsMsg);
}
void pointAssociateToMap(PointType const * const pi, PointType * const po)
{
po->x = transPointAssociateToMap(0,0) * pi->x + transPointAssociateToMap(0,1) * pi->y + transPointAssociateToMap(0,2) * pi->z + transPointAssociateToMap(0,3);
po->y = transPointAssociateToMap(1,0) * pi->x + transPointAssociateToMap(1,1) * pi->y + transPointAssociateToMap(1,2) * pi->z + transPointAssociateToMap(1,3);
po->z = transPointAssociateToMap(2,0) * pi->x + transPointAssociateToMap(2,1) * pi->y + transPointAssociateToMap(2,2) * pi->z + transPointAssociateToMap(2,3);
po->intensity = pi->intensity;
}
pcl::PointCloud::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn, PointTypePose* transformIn)
{
pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud());
PointType *pointFrom;
int cloudSize = cloudIn->size();
cloudOut->resize(cloudSize);
Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
for (int i = 0; i < cloudSize; ++i){
pointFrom = &cloudIn->points[i];
cloudOut->points[i].x = transCur(0,0) * pointFrom->x + transCur(0,1) * pointFrom->y + transCur(0,2) * pointFrom->z + transCur(0,3);
cloudOut->points[i].y = transCur(1,0) * pointFrom->x + transCur(1,1) * pointFrom->y + transCur(1,2) * pointFrom->z + transCur(1,3);
cloudOut->points[i].z = transCur(2,0) * pointFrom->x + transCur(2,1) * pointFrom->y + transCur(2,2) * pointFrom->z + transCur(2,3);
cloudOut->points[i].intensity = pointFrom->intensity;
}
return cloudOut;
}
gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint)
{
return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
}
gtsam::Pose3 trans2gtsamPose(float transformIn[])
{
return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
}
Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint)
{
return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
}
Eigen::Affine3f trans2Affine3f(float transformIn[])
{
return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
}
PointTypePose trans2PointTypePose(float transformIn[])
{
PointTypePose thisPose6D;
thisPose6D.x = transformIn[3];
thisPose6D.y = transformIn[4];
thisPose6D.z = transformIn[5];
thisPose6D.roll = transformIn[0];
thisPose6D.pitch = transformIn[1];
thisPose6D.yaw = transformIn[2];
return thisPose6D;
}
void updateInitialGuess()
{
static Eigen::Affine3f lastImuTransformation;//gc: note that this is static type
// initialization
if (cloudKeyPoses3D->points.empty())//gc: there is no key pose 初始化
{
transformTobeMapped[0] = cloudInfo.imuRollInit;
transformTobeMapped[1] = cloudInfo.imuPitchInit;
transformTobeMapped[2] = cloudInfo.imuYawInit;
if (!useImuHeadingInitialization)//gc: if not use the heading of init_IMU as Initialization
transformTobeMapped[2] = 0;
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
return;
}
// use imu pre-integration estimation for pose guess
if (cloudInfo.odomAvailable == true && cloudInfo.imuPreintegrationResetId == imuPreintegrationResetId)
{
transformTobeMapped[0] = cloudInfo.initialGuessRoll;
transformTobeMapped[1] = cloudInfo.initialGuessPitch;
transformTobeMapped[2] = cloudInfo.initialGuessYaw;
transformTobeMapped[3] = cloudInfo.initialGuessX;
transformTobeMapped[4] = cloudInfo.initialGuessY;
transformTobeMapped[5] = cloudInfo.initialGuessZ;
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
return;
}
// }
// use imu incremental estimation for pose guess (only rotation)
if (cloudInfo.imuAvailable == true)
{
Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;//gc: the transform of IMU between two scans
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
Eigen::Affine3f transFinal = transTobe * transIncre;
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
return;
}
}
void extractForLoopClosure()
{
//change-1
/**************gc added**************/
//{
//in this place the maximum of numPoses is winSize
pcl::PointCloud::Ptr cloudToExtract(new pcl::PointCloud());
int numPoses = win_cloudKeyPoses3D.size();
for (int i = numPoses-1; i >=0; --i)
{
cloudToExtract->push_back(win_cloudKeyPoses3D[i]);
}
extractCloud(cloudToExtract);
// }
/**************gc added**************/
// {
// pcl::PointCloud::Ptr cloudToExtract(new pcl::PointCloud());
// int numPoses = cloudKeyPoses3D->size();
// for (int i = numPoses-1; i >= 0; --i)
// {
// if ((int)cloudToExtract->size() <= surroundingKeyframeSize)
// cloudToExtract->push_back(cloudKeyPoses3D->points[i]);
// else
// break;
// }
//
// extractCloud(cloudToExtract);
// }
}
//gc:search nearby key poses and downsample them and then extract the local map points
// void extractNearby()
// {
//
// }
//gc: extract the nearby Map points
void extractCloud(pcl::PointCloud::Ptr cloudToExtract)
{
//change-2
/**************gc added**************/
//std::cout << "cloudToExtract size: " << cloudToExtract->size() << std::endl;
std::vector> laserCloudCornerSurroundingVec;
std::vector> laserCloudSurfSurroundingVec;
laserCloudCornerSurroundingVec.resize(cloudToExtract->size());
laserCloudSurfSurroundingVec.resize(cloudToExtract->size());
// extract surrounding map
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < (int)cloudToExtract->size(); ++i)
{
PointTypePose thisPose6D;
thisPose6D = win_cloudKeyPoses6D[i];
laserCloudCornerSurroundingVec[i] = *transformPointCloud(win_cornerCloudKeyFrames[i], &thisPose6D);
laserCloudSurfSurroundingVec[i] = *transformPointCloud(win_surfCloudKeyFrames[i], &thisPose6D);
}
// fuse the map
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
for (int i = 0; i < (int)cloudToExtract->size(); ++i)
{
*laserCloudCornerFromMap += laserCloudCornerSurroundingVec[i];
*laserCloudSurfFromMap += laserCloudSurfSurroundingVec[i];
}
// Downsample the surrounding corner key frames (or map)
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
//std::cout << "the size of laserCloudCornerFromMapDS: " << laserCloudCornerFromMapDSNum << std::endl;
// Downsample the surrounding surf key frames (or map)
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
/**************gc added**************/
// {
// std::vector> laserCloudCornerSurroundingVec;
// std::vector> laserCloudSurfSurroundingVec;
//
// laserCloudCornerSurroundingVec.resize(cloudToExtract->size());
// laserCloudSurfSurroundingVec.resize(cloudToExtract->size());
//
// // extract surrounding map
//#pragma omp parallel for num_threads(numberOfCores)
// for (int i = 0; i < (int)cloudToExtract->size(); ++i)
// {
// if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
// continue;
// int thisKeyInd = (int)cloudToExtract->points[i].intensity;//gc: the index of this key frame
// //gc: tranform the corner points and surfpoints of the nearby keyFrames into the world frame
// laserCloudCornerSurroundingVec[i] = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
// laserCloudSurfSurroundingVec[i] = *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
// }
//
// // fuse the map
// laserCloudCornerFromMap->clear();
// laserCloudSurfFromMap->clear();
// for (int i = 0; i < (int)cloudToExtract->size(); ++i)
// {
// *laserCloudCornerFromMap += laserCloudCornerSurroundingVec[i];
// *laserCloudSurfFromMap += laserCloudSurfSurroundingVec[i];
// }
//
// // Downsample the surrounding corner key frames (or map)
// downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
// downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
// laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
// // Downsample the surrounding surf key frames (or map)
// downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
// downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
// laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
// }
}
void extractSurroundingKeyFrames()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
//if (loopClosureEnableFlag == true)//gc:TODO: a little weired: Loop closure should search the whole map while
//{
//std::cout << "the size of cloudKeyPoses3D: " << cloudKeyPoses3D->points.size() << std::endl;
extractForLoopClosure(); //gc: the name is misleading
//} else {
// extractNearby();
//}
}
void downsampleCurrentScan()
{
// Downsample cloud from current scan
laserCloudCornerLastDS->clear();
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerLastDS);
laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
laserCloudSurfLastDS->clear();
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfLastDS);
laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
}
void updatePointAssociateToMap()
{
transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
}
void cornerOptimization()
{
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
//gc: for every corner point
for (int i = 0; i < laserCloudCornerLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector pointSearchInd;
std::vector pointSearchSqDis;
pointOri = laserCloudCornerLastDS->points[i];
//gc: calculate its location in the map using the prediction pose
pointAssociateToMap(&pointOri, &pointSel);
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
if (pointSearchSqDis[4] < 1.0) {
float cx = 0, cy = 0, cz = 0;
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
//gc: the average coordinate of the most nearest points
cx /= 5; cy /= 5; cz /= 5;
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
a22 += ay * ay; a23 += ay * az;
a33 += az * az;
}
a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
matA1.at(0, 0) = a11; matA1.at(0, 1) = a12; matA1.at(0, 2) = a13;
matA1.at(1, 0) = a12; matA1.at(1, 1) = a22; matA1.at(1, 2) = a23;
matA1.at(2, 0) = a13; matA1.at(2, 1) = a23; matA1.at(2, 2) = a33;
cv::eigen(matA1, matD1, matV1);
if (matD1.at(0, 0) > 3 * matD1.at(0, 1)) {
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = cx + 0.1 * matV1.at(0, 0);
float y1 = cy + 0.1 * matV1.at(0, 1);
float z1 = cz + 0.1 * matV1.at(0, 2);
float x2 = cx - 0.1 * matV1.at(0, 0);
float y2 = cy - 0.1 * matV1.at(0, 1);
float z2 = cz - 0.1 * matV1.at(0, 2);
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float ld2 = a012 / l12;
float s = 1 - 0.9 * fabs(ld2);
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
if (s > 0.1) {
laserCloudOriCornerVec[i] = pointOri;
coeffSelCornerVec[i] = coeff;
laserCloudOriCornerFlag[i] = true;
}
}
}
}
}
void surfOptimization()
{
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudSurfLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector pointSearchInd;
std::vector pointSearchSqDis;
pointOri = laserCloudSurfLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix matA0;
Eigen::Matrix matB0;
Eigen::Vector3f matX0;
matA0.setZero();
matB0.fill(-1);
matX0.setZero();
if (pointSearchSqDis[4] < 1.0) {
for (int j = 0; j < 5; j++) {
matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
matX0 = matA0.colPivHouseholderQr().solve(matB0);
float pa = matX0(0, 0);
float pb = matX0(1, 0);
float pc = matX0(2, 0);
float pd = 1;
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps; pb /= ps; pc /= ps; pd /= ps;
bool planeValid = true;
for (int j = 0; j < 5; j++) {
if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
planeValid = false;
break;
}
}
if (planeValid) {
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
if (s > 0.1) {
laserCloudOriSurfVec[i] = pointOri;
coeffSelSurfVec[i] = coeff;
laserCloudOriSurfFlag[i] = true;
}
}
}
}
}
void combineOptimizationCoeffs()
{
// combine corner coeffs
for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
if (laserCloudOriCornerFlag[i] == true){
laserCloudOri->push_back(laserCloudOriCornerVec[i]);
coeffSel->push_back(coeffSelCornerVec[i]);
}
}
// combine surf coeffs
for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
if (laserCloudOriSurfFlag[i] == true){
laserCloudOri->push_back(laserCloudOriSurfVec[i]);
coeffSel->push_back(coeffSelSurfVec[i]);
}
}
// reset flag for next iteration
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
}
bool LMOptimization(int iterCount)
{
// This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
// lidar <- camera --- camera <- lidar
// x = z --- x = y
// y = x --- y = z
// z = y --- z = x
// roll = yaw --- roll = pitch
// pitch = roll --- pitch = yaw
// yaw = pitch --- yaw = roll
// lidar -> camera
float srx = sin(transformTobeMapped[1]);
float crx = cos(transformTobeMapped[1]);
float sry = sin(transformTobeMapped[2]);
float cry = cos(transformTobeMapped[2]);
float srz = sin(transformTobeMapped[0]);
float crz = cos(transformTobeMapped[0]);
int laserCloudSelNum = laserCloudOri->size();
if (laserCloudSelNum < 50) {
return false;
}
cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));
PointType pointOri, coeff;
for (int i = 0; i < laserCloudSelNum; i++) {
// lidar -> camera
pointOri.x = laserCloudOri->points[i].y;
pointOri.y = laserCloudOri->points[i].z;
pointOri.z = laserCloudOri->points[i].x;
// lidar -> camera
coeff.x = coeffSel->points[i].y;
coeff.y = coeffSel->points[i].z;
coeff.z = coeffSel->points[i].x;
coeff.intensity = coeffSel->points[i].intensity;
// in camera
float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
float ary = ((cry*srx*srz - crz*sry)*pointOri.x
+ (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
+ ((-cry*crz - srx*sry*srz)*pointOri.x
+ (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
// lidar -> camera
matA.at(i, 0) = arz;
matA.at
