Added timing messages to map maker

Author: aharmat

CMakeLists.txt

@@ -28,7 +28,7 @@ link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${SUITESPARSE_LIBR
 
 add_message_files(
   DIRECTORY msg
-  FILES MapInfo.msg NetworkMapPoint.msg NetworkMeasurement.msg NetworkKeyFrame.msg NetworkMultiKeyFrame.msg NetworkOutlier.msg
+  FILES MapInfo.msg MapMakerTiming.msg NetworkMapPoint.msg NetworkMeasurement.msg NetworkKeyFrame.msg NetworkMultiKeyFrame.msg NetworkOutlier.msg
         SystemInfo.msg TrackerTiming.msg TrackerState.msg
 )
 

include/mcptam/MapMaker.h

@@ -137,6 +137,10 @@ class MapMaker : public MapMakerClientBase, public MapMakerServerBase, protected
   //testing 
   bool mbStopInit;  ///< End of initialization phase requested
 
+  ros::Publisher mCreationTimingPub;
+  ros::Publisher mLocalTimingPub;
+  ros::Publisher mGlobalTimingPub;
+  
 };
 
 #endif

msg/MapMakerTiming.msg

@@ -0,0 +1,4 @@
+Header header
+float32 elapsed
+int32 map_num_mkfs
+int32 map_num_points

msg/TrackerTiming.msg

@@ -12,3 +12,5 @@ float32 pose
 float32 depth
 float32 add
 float32 total
+int32 map_num_mkfs
+int32 map_num_points

src/MapMaker.cc

@@ -43,12 +43,17 @@
 #include <mcptam/SmallBlurryImage.h>
 #include <mcptam/Map.h>
 #include <mcptam/BundleAdjusterBase.h>
+#include <mcptam/MapMakerTiming.h>
 
 MapMaker::MapMaker(Map &map, TaylorCameraMap &cameras, BundleAdjusterBase &bundleAdjuster)
 : MapMakerBase(map, true)
 , MapMakerClientBase(map)
 , MapMakerServerBase(map, cameras, bundleAdjuster)
 {
+  mCreationTimingPub = mNodeHandlePriv.advertise<mcptam::MapMakerTiming>("timing_mkf_creation", 1, true);
+  mLocalTimingPub = mNodeHandlePriv.advertise<mcptam::MapMakerTiming>("timing_local_ba", 1,true);
+  mGlobalTimingPub = mNodeHandlePriv.advertise<mcptam::MapMakerTiming>("timing_global_ba", 1,true);
+  
   Reset();
   start();
 }
@@ -189,9 +194,19 @@ void MapMaker::run()
       mBundleAdjuster.UseTukey((mState == MM_RUNNING && mMap.mlpMultiKeyFrames.size() > 2));
       mBundleAdjuster.UseTwoStep((mState == MM_RUNNING && mMap.mlpMultiKeyFrames.size() > 2));
 
+      mcptam::MapMakerTiming timingMsg;
+      timingMsg.map_num_mkfs = mMap.mlpMultiKeyFrames.size();
+      timingMsg.map_num_points = mMap.mlpPoints.size();
+      
+      ros::Time start = ros::Time::now();
+      
       int nAccepted = mBundleAdjuster.BundleAdjustRecent(vOutliers);
       //mdMaxCov = mBundleAdjuster.GetMaxCov();
 
+      ros::Time nowTime = ros::Time::now();
+      timingMsg.elapsed = (nowTime - start).toSec();
+      timingMsg.header.stamp = nowTime;
+      
       ROS_DEBUG_STREAM("Accepted iterations: "<<nAccepted);
       ROS_DEBUG_STREAM("Number of outliers: "<<vOutliers.size());
       //ROS_DEBUG_STREAM("Max cov: "<<mdMaxCov);
@@ -210,6 +225,9 @@ void MapMaker::run()
         mnNumConsecutiveFailedBA = 0;
         HandleOutliers(vOutliers);
         PublishMapVisualization();
+        
+        timingMsg.elapsed /= nAccepted;
+        mLocalTimingPub.publish(timingMsg);
       }
     }
 
@@ -233,7 +251,18 @@ void MapMaker::run()
       mBundleAdjuster.UseTukey(mState == MM_RUNNING && mMap.mlpMultiKeyFrames.size() > 2);
       mBundleAdjuster.UseTwoStep((mState == MM_RUNNING && mMap.mlpMultiKeyFrames.size() > 2));
 
+      mcptam::MapMakerTiming timingMsg;
+      timingMsg.map_num_mkfs = mMap.mlpMultiKeyFrames.size();
+      timingMsg.map_num_points = mMap.mlpPoints.size();
+      
+      ros::Time start = ros::Time::now();
+      
       int nAccepted = mBundleAdjuster.BundleAdjustAll(vOutliers);
+      
+      ros::Time nowTime = ros::Time::now();
+      timingMsg.elapsed = (nowTime - start).toSec();
+      timingMsg.header.stamp = nowTime;
+      
       mdMaxCov = mBundleAdjuster.GetMaxCov();
 
       ROS_DEBUG_STREAM("Accepted iterations: "<<nAccepted);
@@ -255,6 +284,9 @@ void MapMaker::run()
         HandleOutliers(vOutliers);
         PublishMapVisualization();
 
+        timingMsg.elapsed /= nAccepted;
+        mGlobalTimingPub.publish(timingMsg);
+        
         if(mState == MM_INITIALIZING && (mdMaxCov < MapMakerServerBase::sdInitCovThresh || mbStopInit)) 
         {
           ROS_INFO_STREAM("INITIALIZING, Max cov "<<mdMaxCov<<" below threshold "<<MapMakerServerBase::sdInitCovThresh<<", switching to MM_RUNNING");
@@ -401,6 +433,12 @@ void MapMaker::AddMultiKeyFrameFromTopOfQueue()
   }
 */
 
+  mcptam::MapMakerTiming timingMsg;
+  timingMsg.map_num_mkfs = mMap.mlpMultiKeyFrames.size();
+  timingMsg.map_num_points = mMap.mlpPoints.size();
+  
+  ros::Time start = ros::Time::now();
+  
   if(mState == MM_RUNNING)
   {
     if(pMKF->NoImages()) // leftovers from Tracker, don't bother adding these
@@ -427,6 +465,11 @@ void MapMaker::AddMultiKeyFrameFromTopOfQueue()
     mMap.MoveDeletedMultiKeyFramesToTrash();
   }
 
+  ros::Time nowTime = ros::Time::now();
+  timingMsg.elapsed = (nowTime - start).toSec();
+  timingMsg.header.stamp = nowTime;
+  mCreationTimingPub.publish(timingMsg);
+  
   lock.lock();
   mqpMultiKeyFramesFromTracker.pop_front(); 
   lock.unlock();

src/MapMakerBase.cc

@@ -369,7 +369,7 @@ void MapMakerBase::PublishMapPoints()
   pointMsg->height = 1;
   pointMsg->is_dense = false;
 
-#if ROS_VERSION_MINIMUM(1, 9, 54)   // Hydro or above, uses new PCL library
+#if ROS_VERSION_MINIMUM(1, 9, 56)   // Hydro or above, uses new PCL library
   pointMsg->header.stamp = ros::Time::now().toNSec();
 #else
   pointMsg->header.stamp = ros::Time::now();

src/SystemFrontendBase.cc

@@ -331,7 +331,7 @@ void SystemFrontendBase::PublishSmallImage()
   pointMsg->height = 1;
   pointMsg->is_dense = false;
 
-#if ROS_VERSION_MINIMUM(1, 9, 54)   // Hydro or above, uses new PCL library
+#if ROS_VERSION_MINIMUM(1, 9, 56)   // Hydro or above, uses new PCL library
   pointMsg->header.stamp = timestamp.toNSec();
 #else
   pointMsg->header.stamp = timestamp;

src/Tracker.cc

@@ -114,7 +114,7 @@ Tracker::Tracker(Map &map, MapMakerClientBase &mapmaker, TaylorCameraMap &camera
   ROS_DEBUG("Tracker: In constructor, done");
 
   maskPub = mNodeHandlePrivate.advertise<sensor_msgs::Image>("mask", 1);
-  timingPub = mNodeHandlePrivate.advertise<mcptam::TrackerTiming>("timing", 1);
+  timingPub = mNodeHandlePrivate.advertise<mcptam::TrackerTiming>("timing_tracker", 1);
 }
 
 Tracker::~Tracker()
@@ -511,6 +511,8 @@ void Tracker::TrackFrame(ImageBWMap& imFrames, ros::Time timestamp, bool bDraw)
   }
 
   timingMsg.total = (ros::WallTime::now() - startTimeTotal).toSec();
+  timingMsg.map_num_points = mMap.mlpPoints.size();
+  timingMsg.map_num_mkfs = mMap.mlpMultiKeyFrames.size();
   timingMsg.header.stamp = ros::Time::now();
   timingPub.publish(timingMsg);
 }