reduce gif size

ameysutavani · Jul 27, 2020 · 8b7d935 · 8b7d935
1 parent 30ac917
commit 8b7d935
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 9 deletions.
diff --git a/config/doc/demo.gif b/config/doc/demo.gif
diff --git a/config/doc/kitti-demo.gif b/config/doc/kitti-demo.gif
diff --git a/config/doc/ouster-demo.gif b/config/doc/ouster-demo.gif
diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp
@@ -121,7 +121,7 @@ class mapOptimization : public ParamServer
     pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
 
     ros::Time timeLaserInfoStamp;
-    double timeLaserCloudInfoLast;
+    double timeLaserInfoCur;
 
     float transformTobeMapped[6];
 
@@ -229,7 +229,7 @@ class mapOptimization : public ParamServer
     {
         // extract time stamp
         timeLaserInfoStamp = msgIn->header.stamp;
-        timeLaserCloudInfoLast = msgIn->header.stamp.toSec();
+        timeLaserInfoCur = msgIn->header.stamp.toSec();
 
         // extract info and feature cloud
         cloudInfo = *msgIn;
@@ -239,9 +239,9 @@ class mapOptimization : public ParamServer
         std::lock_guard<std::mutex> lock(mtx);
 
         static double timeLastProcessing = -1;
-        if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval)
+        if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
         {
-            timeLastProcessing = timeLaserCloudInfoLast;
+            timeLastProcessing = timeLaserInfoCur;
 
             updateInitialGuess();
 
@@ -486,7 +486,7 @@ class mapOptimization : public ParamServer
         for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
         {
             int id = pointSearchIndLoop[i];
-            if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserCloudInfoLast) > historyKeyframeSearchTimeDiff)
+            if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
             {
                 closestHistoryFrameID = id;
                 break;
@@ -757,7 +757,7 @@ class mapOptimization : public ParamServer
         int numPoses = cloudKeyPoses3D->size();
         for (int i = numPoses-1; i >= 0; --i)
         {
-            if (timeLaserCloudInfoLast - cloudKeyPoses6D->points[i].time < 10.0)
+            if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
                 surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
             else
                 break;
@@ -1276,12 +1276,12 @@ class mapOptimization : public ParamServer
 
         while (!gpsQueue.empty())
         {
-            if (gpsQueue.front().header.stamp.toSec() < timeLaserCloudInfoLast - 0.2)
+            if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
             {
                 // message too old
                 gpsQueue.pop_front();
             }
-            else if (gpsQueue.front().header.stamp.toSec() > timeLaserCloudInfoLast + 0.2)
+            else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2)
             {
                 // message too new
                 break;
@@ -1409,7 +1409,7 @@ class mapOptimization : public ParamServer
         thisPose6D.roll  = latestEstimate.rotation().roll();
         thisPose6D.pitch = latestEstimate.rotation().pitch();
         thisPose6D.yaw   = latestEstimate.rotation().yaw();
-        thisPose6D.time = timeLaserCloudInfoLast;
+        thisPose6D.time = timeLaserInfoCur;
         cloudKeyPoses6D->push_back(thisPose6D);
 
         // cout << "****************************************************" << endl;
@@ -1499,6 +1499,12 @@ class mapOptimization : public ParamServer
         laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
         laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
         pubLaserOdometryGlobal.publish(laserOdometryROS);
+        // Publish TF
+        static tf::TransformBroadcaster br;
+        tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
+                                                      tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
+        tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, "odom", "lidar_link");
+        br.sendTransform(trans_odom_to_lidar);
 
         // Publish odometry for ROS (incremental)
         static bool lastIncreOdomPubFlag = false;