mirrors/FAST_LIO_LOCALIZATION
1
0
Fork 0
mirror of https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git synced 2023-05-28 12:51:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

support for Scan context loop closure

Browse Source
This commit is contained in:
xw
2021-07-14 06:51:30 -04:00
parent 96774ef3e9
commit c8aea0dd95
7 changed files with 89 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
src/laserMapping.cpp
View File

@@ -92,7 +92,7 @@ int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count =
int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0;
bool point_selected_surf[100000] = {0};
bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited;
bool scan_pub_en = false, dense_pub_en = false;
bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
vector<vector<int>> pointSearchInd_surf;
vector<BoxPointType> cub_needrm;
@@ -206,6 +206,17 @@ void RGBpointBodyToWorld(PointType const * const pi, PointType * const po)
po->intensity = pi->intensity;
}
void RGBpointBodyLidarToIMU(PointType const * const pi, PointType * const po)
{
V3D p_body_lidar(pi->x, pi->y, pi->z);
V3D p_body_imu(state_point.offset_R_L_I*p_body_lidar + state_point.offset_T_L_I);
po->x = p_body_imu(0);
po->y = p_body_imu(1);
po->z = p_body_imu(2);
po->intensity = pi->intensity;
}
void points_cache_collect()
{
PointVector points_history;
@@ -443,9 +454,8 @@ void map_incremental()
PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
{
// *pcl_wait_pub += *laserCloudWorld;
if(scan_pub_en)
{
PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
@@ -461,11 +471,10 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time::now();
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "camera_init";
pubLaserCloudFullRes.publish(laserCloudmsg);
pubLaserCloudFull.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
// pcl_wait_pub->clear();
}
/**************** save map ****************/
@@ -486,6 +495,25 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
}
}
void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
{
int size = feats_undistort->points.size();
PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
for (int i = 0; i < size; i++)
{
RGBpointBodyLidarToIMU(&feats_undistort->points[i], \
&laserCloudIMUBody->points[i]);
}
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "body";
pubLaserCloudFull_body.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
}
void publish_effect_world(const ros::Publisher & pubLaserCloudEffect)
{
PointCloudXYZI::Ptr laserCloudWorld( \
@@ -497,7 +525,7 @@ void publish_effect_world(const ros::Publisher & pubLaserCloudEffect)
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time::now();
laserCloudFullRes3.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudFullRes3.header.frame_id = "camera_init";
pubLaserCloudEffect.publish(laserCloudFullRes3);
}
@@ -506,7 +534,7 @@ void publish_map(const ros::Publisher & pubLaserCloudMap)
{
sensor_msgs::PointCloud2 laserCloudMap;
pcl::toROSMsg(*featsFromMap, laserCloudMap);
laserCloudMap.header.stamp = ros::Time::now();
laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudMap.header.frame_id = "camera_init";
pubLaserCloudMap.publish(laserCloudMap);
}
@@ -527,8 +555,8 @@ void set_posestamp(T & out)
void publish_odometry(const ros::Publisher & pubOdomAftMapped)
{
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "aft_mapped";
odomAftMapped.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar);
odomAftMapped.child_frame_id = "body";
odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);// ros::Time().fromSec(lidar_end_time);
set_posestamp(odomAftMapped.pose);
pubOdomAftMapped.publish(odomAftMapped);
auto P = kf.get_P();
@@ -554,13 +582,13 @@ void publish_odometry(const ros::Publisher & pubOdomAftMapped)
q.setY(odomAftMapped.pose.pose.orientation.y);
q.setZ(odomAftMapped.pose.pose.orientation.z);
transform.setRotation( q );
br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped" ) );
br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "body" ) );
}
void publish_path(const ros::Publisher pubPath)
{
set_posestamp(msg_body_pose);
msg_body_pose.header.stamp = ros::Time::now();
msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);
msg_body_pose.header.frame_id = "camera_init";
/*** if path is too large, the rvis will crash ***/
@@ -682,8 +710,9 @@ int main(int argc, char** argv)
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
nh.param<bool>("scan_publish_enable",scan_pub_en,1);
nh.param<bool>("dense_publish_enable",dense_pub_en,0);
nh.param<bool>("publish/scan_publish_en",scan_pub_en,1);
nh.param<bool>("publish/dense_publish_en",dense_pub_en,0);
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en,1);
nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4);
nh.param<string>("map_file_path",map_file_path,"");
nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");
@@ -761,14 +790,16 @@ int main(int argc, char** argv)
nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered_body", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_effected", 100000);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
("/Laser_map", 100000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/aft_mapped_to_init", 100000);
("/Odometry", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
("/path", 100000);
//------------------------------------------------------------------------------------------------------
@@ -888,8 +919,8 @@ int main(int argc, char** argv)
/******* Publish points *******/
publish_path(pubPath);
if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes);
if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull);
if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body);
// publish_effect_world(pubLaserCloudEffect);
// publish_map(pubLaserCloudMap);