Skip to content

Commit

Permalink
add loop closure visualization
Browse files Browse the repository at this point in the history
  • Loading branch information
@TixiaoShan
TixiaoShan committed Jul 27, 2020
1 parent b4a1385 commit b67ba7d
Show file tree
Hide file tree
Showing 3 changed files with 80 additions and 11 deletions.
2 changes: 2 additions & 0 deletions include/utility.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <opencv/cv.h>

Expand Down
14 changes: 11 additions & 3 deletions launch/include/config/rviz.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -182,7 +182,7 @@ Visualization Manager:
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 102
Max Intensity: 100
Min Color: 0; 0; 0
Min Intensity: 0
Name: Velodyne
Expand Down Expand Up @@ -461,6 +461,14 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /lio_sam/mapping/loop_closure_constraints
Name: Loop constraint
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: true
Name: Mapping
Enabled: true
Expand Down Expand Up @@ -494,10 +502,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.995398104
Pitch: 1.00039816
Target Frame: base_link
Value: Orbit (rviz)
Yaw: 1.79352248
Yaw: 1.79852247
Saved: ~
Window Geometry:
Displays:
Expand Down
75 changes: 67 additions & 8 deletions src/mapOptmization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@ class mapOptimization : public ParamServer
ros::Publisher pubRecentKeyFrames;
ros::Publisher pubRecentKeyFrame;
ros::Publisher pubCloudRegisteredRaw;
ros::Publisher pubLoopConstraintEdge;

ros::Subscriber subLaserCloudInfo;
ros::Subscriber subGPS;
Expand Down Expand Up @@ -126,8 +127,6 @@ class mapOptimization : public ParamServer

std::mutex mtx;

double timeLastProcessing = -1;

bool isDegenerate = false;
Eigen::Matrix<float, 6, 6> matP;

Expand All @@ -138,6 +137,7 @@ class mapOptimization : public ParamServer

bool aLoopIsClosed = false;
int imuPreintegrationResetId = 0;
map<int, int> loopIndexContainer; // from new to old

nav_msgs::Path globalPath;

Expand All @@ -163,6 +163,7 @@ class mapOptimization : public ParamServer

pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);
pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/lio_sam/mapping/loop_closure_constraints", 1);

pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);
pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
Expand Down Expand Up @@ -235,8 +236,9 @@ class mapOptimization : public ParamServer

std::lock_guard<std::mutex> lock(mtx);

if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval) {

static double timeLastProcessing = -1;
if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval)
{
timeLastProcessing = timeLaserCloudInfoLast;

updateInitialGuess();
Expand Down Expand Up @@ -495,6 +497,11 @@ class mapOptimization : public ParamServer
*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 @@ -584,6 +591,57 @@ class mapOptimization : public ParamServer
gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));

aLoopIsClosed = true;

// visualize loop constriant
loopIndexContainer[latestFrameIDLoopCloure] = closestHistoryFrameID;
{
visualization_msgs::MarkerArray markerArray;
// loop nodes
visualization_msgs::Marker markerNode;
markerNode.header.frame_id = "odom";
markerNode.header.stamp = timeLaserInfoStamp;
markerNode.action = visualization_msgs::Marker::ADD;
markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
markerNode.ns = "loop_nodes";
markerNode.id = 0;
markerNode.pose.orientation.w = 1;
markerNode.scale.x = 0.3; markerNode.scale.y = 0.3; markerNode.scale.z = 0.3;
markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1;
markerNode.color.a = 1;
// loop edges
visualization_msgs::Marker markerEdge;
markerEdge.header.frame_id = "odom";
markerEdge.header.stamp = timeLaserInfoStamp;
markerEdge.action = visualization_msgs::Marker::ADD;
markerEdge.type = visualization_msgs::Marker::LINE_LIST;
markerEdge.ns = "loop_edges";
markerEdge.id = 1;
markerEdge.pose.orientation.w = 1;
markerEdge.scale.x = 0.1; markerEdge.scale.y = 0.1; markerEdge.scale.z = 0.1;
markerEdge.color.r = 0.9; markerEdge.color.g = 0.9; markerEdge.color.b = 0;
markerEdge.color.a = 1;

for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it)
{
int key_cur = it->first;
int key_pre = it->second;
geometry_msgs::Point p;
p.x = copy_cloudKeyPoses6D->points[key_cur].x;
p.y = copy_cloudKeyPoses6D->points[key_cur].y;
p.z = copy_cloudKeyPoses6D->points[key_cur].z;
markerNode.points.push_back(p);
markerEdge.points.push_back(p);
p.x = copy_cloudKeyPoses6D->points[key_pre].x;
p.y = copy_cloudKeyPoses6D->points[key_pre].y;
p.z = copy_cloudKeyPoses6D->points[key_pre].z;
markerNode.points.push_back(p);
markerEdge.points.push_back(p);
}

markerArray.markers.push_back(markerNode);
markerArray.markers.push_back(markerEdge);
pubLoopConstraintEdge.publish(markerArray);
}
}


Expand Down Expand Up @@ -617,16 +675,16 @@ class mapOptimization : public ParamServer
}

// use imu pre-integration estimation for pose guess
static bool lastImuTransAvailable = false;
static bool lastImuPreTransAvailable = false;
static Eigen::Affine3f lastImuPreTransformation;
if (cloudInfo.odomAvailable == true && cloudInfo.imuPreintegrationResetId == imuPreintegrationResetId)
{
Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
if (lastImuTransAvailable == false)
if (lastImuPreTransAvailable == false)
{
lastImuPreTransformation = transBack;
lastImuTransAvailable = true;
lastImuPreTransAvailable = true;
} else {
Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
Expand Down Expand Up @@ -1107,7 +1165,6 @@ class mapOptimization : public ParamServer
}

transformUpdate();
incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
} else {
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
}
Expand Down Expand Up @@ -1141,6 +1198,8 @@ class mapOptimization : public ParamServer
transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);

incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
}

float constraintTransformation(float value, float limit)
Expand Down

0 comments on commit b67ba7d

Please sign in to comment.