reconfigure imuPreintegration

config/params.yaml

@@ -76,7 +76,7 @@ lio_sam:
 
   # Loop closure
   loopClosureEnableFlag: false
-  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+  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

launch/include/config/rviz.rviz

@@ -78,13 +78,13 @@ Visualization Manager:
               Scale: 1
               Value: true
             Value: false
-          Enabled: false
+          Enabled: true
           Keep: 300
-          Name: Odo IMU
+          Name: Odom IMU
           Position Tolerance: 0.100000001
           Shape:
             Alpha: 1
-            Axes Length: 0.25
+            Axes Length: 0.5
             Axes Radius: 0.100000001
             Color: 255; 25; 0
             Head Length: 0.300000012
@@ -94,7 +94,7 @@ Visualization Manager:
             Value: Axes
           Topic: /odometry/imu
           Unreliable: false
-          Value: false
+          Value: true
         - Angle Tolerance: 0.100000001
           Class: rviz/Odometry
           Covariance:
@@ -182,7 +182,7 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 100
+          Max Intensity: 245
           Min Color: 0; 0; 0
           Min Intensity: 0
           Name: Velodyne
@@ -310,9 +310,9 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 100
+          Max Intensity: 130
           Min Color: 0; 0; 0
-          Min Intensity: 1
+          Min Intensity: 0
           Name: Map (cloud)
           Position Transformer: XYZ
           Queue Size: 10
@@ -395,7 +395,7 @@ Visualization Manager:
           Length: 0.300000012
           Line Style: Billboards
           Line Width: 0.100000001
-          Name: Path (global)
+          Name: Path
           Offset:
             X: 0
             Y: 0
@@ -408,29 +408,6 @@ Visualization Manager:
           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.300000012
-          Head Length: 0.200000003
-          Length: 0.300000012
-          Line Style: Billboards
-          Line Width: 0.100000001
-          Name: Path (local)
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: None
-          Radius: 0.0299999993
-          Shaft Diameter: 0.100000001
-          Shaft Length: 0.100000001
-          Topic: /lio_sam/imu/path
-          Unreliable: false
-          Value: true
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -487,7 +464,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 65.799942
+      Distance: 44.0755539
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
@@ -502,10 +479,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 1.00039816
+      Pitch: 1.04039812
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 0.443523765
+      Yaw: 4.05668163
     Saved: ~
 Window Geometry:
   Displays:

msg/cloud_info.msg

@@ -23,9 +23,6 @@ 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

src/imageProjection.cpp

@@ -377,7 +377,6 @@ class ImageProjection : public ParamServer
         cloudInfo.initialGuessRoll  = roll;
         cloudInfo.initialGuessPitch = pitch;
         cloudInfo.initialGuessYaw   = yaw;
-        cloudInfo.imuPreintegrationResetId = round(startOdomMsg.pose.covariance[0]); // not used if it's incremental
 
         cloudInfo.odomAvailable = true;
 

src/imuPreintegration.cpp

@@ -20,6 +20,97 @@ using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
 using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
 using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
 
+class TransformFusion : public ParamServer
+{
+public:
+    std::mutex mtx;
+
+    ros::Subscriber subImuOdometry;
+    ros::Subscriber subLaserOdometry;
+
+    ros::Publisher pubImuOdometry;
+
+    Eigen::Affine3f lidarOdomAffine;
+    Eigen::Affine3f imuOdomAffineFront;
+    Eigen::Affine3f imuOdomAffineBack;
+
+    double lidarOdomTime = -1;
+    deque<nav_msgs::Odometry> imuOdomQueue;
+
+    TransformFusion()
+    {
+        subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
+        subImuOdometry   = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental",   2000, &TransformFusion::imuOdometryHandler,   this, ros::TransportHints().tcpNoDelay());
+
+        pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
+    }
+
+    Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
+    {
+        double x, y, z, roll, pitch, yaw;
+        x = odom.pose.pose.position.x;
+        y = odom.pose.pose.position.y;
+        z = odom.pose.pose.position.z;
+        tf::Quaternion orientation;
+        tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
+        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
+        return pcl::getTransformation(x, y, z, roll, pitch, yaw);
+    }
+
+    void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
+    {
+        std::lock_guard<std::mutex> lock(mtx);
+
+        lidarOdomAffine = odom2affine(*odomMsg);
+
+        lidarOdomTime = odomMsg->header.stamp.toSec();
+    }
+
+    void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
+    {
+        // static tf
+        static tf::TransformBroadcaster tfMap2Odom;
+        static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
+        tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
+
+        std::lock_guard<std::mutex> lock(mtx);
+
+        imuOdomQueue.push_back(*odomMsg);
+
+        // front imu odometry
+        if (lidarOdomTime == -1)
+            return;
+        while (!imuOdomQueue.empty())
+        {
+            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
+                imuOdomQueue.pop_front();
+            else
+                break;
+        }
+        Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
+        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
+        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
+        Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
+        float x, y, z, roll, pitch, yaw;
+        pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
+
+        // publish latest odometry
+        nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
+        laserOdometry.pose.pose.position.x = x;
+        laserOdometry.pose.pose.position.y = y;
+        laserOdometry.pose.pose.position.z = z;
+        laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        pubImuOdometry.publish(laserOdometry);
+
+        // publish tf
+        static tf::TransformBroadcaster tfOdom2BaseLink;
+        tf::Transform tCur;
+        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
+        tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, lidarFrame);
+        tfOdom2BaseLink.sendTransform(odom_2_baselink);
+    }
+};
+
 class IMUPreintegration : public ParamServer
 {
 public:
@@ -71,19 +162,16 @@ class IMUPreintegration : public ParamServer
     const double delta_t = 0;
 
     int key = 1;
-    int imuPreintegrationResetId = 0;
 
     gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
     gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
 
-    bool pubTFFlag = true;
-
     IMUPreintegration()
     {
         subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic,                   2000, &IMUPreintegration::imuHandler,      this, ros::TransportHints().tcpNoDelay());
-        subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
+        subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
 
-        pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic, 2000);
+        pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
         pubImuPath     = nh.advertise<nav_msgs::Path>     ("lio_sam/imu/path", 1);
 
         map_to_odom    = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
@@ -104,14 +192,6 @@ class IMUPreintegration : public ParamServer
         imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization        
     }
 
-    void incrementalSetting()
-    {
-        // configure this class to publish incremental odometry (no pose correction)
-        subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
-        pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
-        pubTFFlag = false;
-    }
-
     void resetOptimization()
     {
         gtsam::ISAM2Params optParameters;
@@ -150,17 +230,8 @@ class IMUPreintegration : public ParamServer
         float r_y = odomMsg->pose.pose.orientation.y;
         float r_z = odomMsg->pose.pose.orientation.z;
         float r_w = odomMsg->pose.pose.orientation.w;
-        int currentResetId = round(odomMsg->pose.covariance[0]);
         gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
 
-        // correction pose jumped, reset imu pre-integration
-        if (currentResetId != imuPreintegrationResetId)
-        {
-            resetParams();
-            imuPreintegrationResetId = currentResetId;
-            return;
-        }
-
 
         // 0. initialize system
         if (systemInitialized == false)
@@ -352,9 +423,6 @@ class IMUPreintegration : public ParamServer
         std::lock_guard<std::mutex> lock(mtx);
 
         sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
-        // publish static tf
-        if (pubTFFlag)
-            tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, thisImu.header.stamp, mapFrame, odometryFrame));
 
         imuQueOpt.push_back(thisImu);
         imuQueImu.push_back(thisImu);
@@ -397,38 +465,7 @@ class IMUPreintegration : public ParamServer
         odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
         odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
         odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
-        odometry.pose.covariance[0] = double(imuPreintegrationResetId);
         pubImuOdometry.publish(odometry);
-
-        // publish imu path
-        static nav_msgs::Path imuPath;
-        static double last_path_time = -1;
-        if (imuTime - last_path_time > 0.1 && pubTFFlag)
-        {
-            last_path_time = imuTime;
-            geometry_msgs::PoseStamped pose_stamped;
-            pose_stamped.header.stamp = thisImu.header.stamp;
-            pose_stamped.header.frame_id = odometryFrame;
-            pose_stamped.pose = odometry.pose.pose;
-            imuPath.poses.push_back(pose_stamped);
-            while(!imuPath.poses.empty() && abs(imuPath.poses.front().header.stamp.toSec() - imuPath.poses.back().header.stamp.toSec()) > 3.0)
-                imuPath.poses.erase(imuPath.poses.begin());
-            if (pubImuPath.getNumSubscribers() != 0)
-            {
-                imuPath.header.stamp = thisImu.header.stamp;
-                imuPath.header.frame_id = odometryFrame;
-                pubImuPath.publish(imuPath);
-            }
-        }
-
-        // publish transformation
-        if (pubTFFlag)
-        {
-            tf::Transform tCur;
-            tf::poseMsgToTF(odometry.pose.pose, tCur);
-            tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, thisImu.header.stamp, odometryFrame, lidarFrame);
-            tfOdom2BaseLink.sendTransform(odom_2_baselink);
-        }
     }
 };
 
@@ -437,10 +474,9 @@ int main(int argc, char** argv)
 {
     ros::init(argc, argv, "roboat_loam");
 
-    IMUPreintegration ImuP_global;
+    IMUPreintegration ImuP;
 
-    IMUPreintegration ImuP_incremental;
-    ImuP_incremental.incrementalSetting();
+    TransformFusion TF;
 
     ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
 

src/mapOptmization.cpp

@@ -136,7 +136,6 @@ class mapOptimization : public ParamServer
     int laserCloudSurfLastDSNum = 0;
 
     bool aLoopIsClosed = false;
-    int imuPreintegrationResetId = 0;
     map<int, int> loopIndexContainer; // from new to old
     vector<pair<int, int>> loopIndexQueue;
     vector<gtsam::Pose3> loopPoseQueue;
@@ -679,7 +678,7 @@ class mapOptimization : public ParamServer
         // use imu pre-integration estimation for pose guess
         static bool lastImuPreTransAvailable = false;
         static Eigen::Affine3f lastImuPreTransformation;
-        if (cloudInfo.odomAvailable == true)// && cloudInfo.imuPreintegrationResetId == imuPreintegrationResetId)
+        if (cloudInfo.odomAvailable == true)
         {
             Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX,    cloudInfo.initialGuessY,     cloudInfo.initialGuessZ, 
                                                                cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
@@ -1465,8 +1464,6 @@ class mapOptimization : public ParamServer
             }
 
             aLoopIsClosed = false;
-            // ID for reseting IMU pre-integration
-            ++imuPreintegrationResetId;
         }
     }
 
@@ -1498,7 +1495,6 @@ class mapOptimization : public ParamServer
         laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
         laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
         laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
-        laserOdometryROS.pose.covariance[0] = double(imuPreintegrationResetId);
         pubLaserOdometryGlobal.publish(laserOdometryROS);
 
         // Publish odometry for ROS (incremental)
@@ -1519,7 +1515,7 @@ class mapOptimization : public ParamServer
             {
                 if (std::abs(cloudInfo.imuPitchInit) < 1.4)
                 {
-                    double imuWeight = 0.05;
+                    double imuWeight = 0.01;
                     tf::Quaternion imuQuaternion;
                     tf::Quaternion transformQuaternion;
                     double rollMid, pitchMid, yawMid;