Skip to content

Commit

Permalink
organize loop closure code
Browse files Browse the repository at this point in the history
  • Loading branch information
@TixiaoShan
TixiaoShan committed Jul 27, 2020
1 parent c06c011 commit ba9fde6
Show file tree
Hide file tree
Showing 5 changed files with 57 additions and 32 deletions.
2 changes: 1 addition & 1 deletion config/params.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ lio_sam:

# Loop closure
loopClosureEnableFlag: false
surroundingKeyframeSize: 25 # submap size (when loop closure enabled)
surroundingKeyframeSize: 50 # 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
Expand Down
8 changes: 4 additions & 4 deletions launch/include/config/rviz.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -306,13 +306,13 @@ Visualization Manager:
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 500
Decay Time: 300
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 100
Min Color: 0; 0; 0
Min Intensity: 0
Min Intensity: 1
Name: Map (cloud)
Position Transformer: XYZ
Queue Size: 10
Expand Down Expand Up @@ -487,7 +487,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 57.5358086
Distance: 65.799942
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Expand All @@ -505,7 +505,7 @@ Visualization Manager:
Pitch: 1.00039816
Target Frame: base_link
Value: Orbit (rviz)
Yaw: 1.79852247
Yaw: 0.443523765
Saved: ~
Window Geometry:
Displays:
Expand Down
4 changes: 2 additions & 2 deletions src/featureExtraction.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,9 @@ class FeatureExtraction : public ParamServer

FeatureExtraction()
{
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 10, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());

pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 10);
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);
pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);
pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);

Expand Down
2 changes: 1 addition & 1 deletion src/imageProjection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ class ImageProjection : public ParamServer
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());

pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 10);
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);

allocateMemory();
resetParameters();
Expand Down
73 changes: 49 additions & 24 deletions src/mapOptmization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,6 +138,9 @@ class mapOptimization : public ParamServer
bool aLoopIsClosed = false;
int imuPreintegrationResetId = 0;
map<int, int> loopIndexContainer; // from new to old
vector<pair<int, int>> loopIndexQueue;
vector<gtsam::Pose3> loopPoseQueue;
vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;

nav_msgs::Path globalPath;

Expand All @@ -158,7 +161,7 @@ class mapOptimization : public ParamServer
pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry_incremental", 1);
pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);

subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 10, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
subGPS = nh.subscribe<nav_msgs::Odometry> (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());

pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
Expand Down Expand Up @@ -463,8 +466,13 @@ class mapOptimization : public ParamServer

bool detectLoopClosure(int *latestID, int *closestID)
{
int latestFrameIDLoopCloure;
int closestHistoryFrameID;
int latestFrameIDLoopCloure = copy_cloudKeyPoses3D->size() - 1;
int closestHistoryFrameID = -1;

// check loop constraint added before
auto it = loopIndexContainer.find(latestFrameIDLoopCloure);
if (it != loopIndexContainer.end())
return false;

latestKeyFrameCloud->clear();
nearHistoryKeyFrameCloud->clear();
Expand All @@ -475,7 +483,6 @@ class mapOptimization : public ParamServer
kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);

closestHistoryFrameID = -1;
for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
{
int id = pointSearchIndLoop[i];
Expand All @@ -489,19 +496,13 @@ class mapOptimization : public ParamServer
if (closestHistoryFrameID == -1)
return false;

if ((int)copy_cloudKeyPoses3D->size() - 1 == closestHistoryFrameID)
if (latestFrameIDLoopCloure == closestHistoryFrameID)
return false;

// save latest key frames
latestFrameIDLoopCloure = copy_cloudKeyPoses3D->size() - 1;
*latestKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &copy_cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
*latestKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure], &copy_cloudKeyPoses6D->points[latestFrameIDLoopCloure]);

// check loop constraint added before
auto it = loopIndexContainer.find(latestFrameIDLoopCloure);
if (it != loopIndexContainer.end())
return false;

// save history near key frames
bool nearFrameAvailable = false;
for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j)
Expand Down Expand Up @@ -538,7 +539,7 @@ class mapOptimization : public ParamServer
return;

// ICP Settings
pcl::IterativeClosestPoint<PointType, PointType> icp;
static pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
Expand Down Expand Up @@ -587,10 +588,11 @@ class mapOptimization : public ParamServer
noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);

// Add pose constraint
std::lock_guard<std::mutex> lock(mtx);
gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));

aLoopIsClosed = true;
mtx.lock();
loopIndexQueue.push_back(make_pair(latestFrameIDLoopCloure, closestHistoryFrameID));
loopPoseQueue.push_back(poseFrom.between(poseTo));
loopNoiseQueue.push_back(constraintNoise);
mtx.unlock();

// visualize loop constriant
loopIndexContainer[latestFrameIDLoopCloure] = closestHistoryFrameID;
Expand Down Expand Up @@ -1329,6 +1331,26 @@ class mapOptimization : public ParamServer
}
}

void addLoopFactor()
{
if (loopIndexQueue.empty())
return;

for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
{
int indexFrom = loopIndexQueue[i].first;
int indexTo = loopIndexQueue[i].second;
gtsam::Pose3 poseBetween = loopPoseQueue[i];
gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
}

loopIndexQueue.clear();
loopPoseQueue.clear();
loopNoiseQueue.clear();
aLoopIsClosed = true;
}

void saveKeyFramesAndFactor()
{
if (saveFrame() == false)
Expand All @@ -1340,21 +1362,24 @@ class mapOptimization : public ParamServer
// gps factor
addGPSFactor();

// loop factor
addLoopFactor();

// cout << "****************************************************" << endl;
// gtSAMgraph.print("GTSAM Graph:\n");

// update iSAM
isam->update(gtSAMgraph, initialEstimate);
isam->update();

if (aLoopIsClosed == true)
{
isam->update();
isam->update();
isam->update();
isam->update();
isam->update();
}
// if (aLoopIsClosed == true)
// {
// isam->update();
// isam->update();
// isam->update();
// isam->update();
// isam->update();
// }

gtSAMgraph.resize(0);
initialEstimate.clear();
Expand Down

0 comments on commit ba9fde6

Please sign in to comment.