From 28a48deda7664f4bc53da68b405b76af937367e2 Mon Sep 17 00:00:00 2001 From: Adam Harmat Date: Tue, 16 Dec 2014 17:23:47 -0500 Subject: [PATCH] Added timing messages to map maker --- CMakeLists.txt | 2 +- include/mcptam/MapMaker.h | 4 ++++ msg/MapMakerTiming.msg | 4 ++++ msg/TrackerTiming.msg | 2 ++ src/MapMaker.cc | 43 +++++++++++++++++++++++++++++++++++++++ src/MapMakerBase.cc | 2 +- src/SystemFrontendBase.cc | 2 +- src/Tracker.cc | 4 +++- 8 files changed, 59 insertions(+), 4 deletions(-) create mode 100644 msg/MapMakerTiming.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index a1e6566..da959a8 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/include/mcptam/MapMaker.h b/include/mcptam/MapMaker.h index 3d92f12..f52d8e7 100644 --- a/include/mcptam/MapMaker.h +++ b/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 diff --git a/msg/MapMakerTiming.msg b/msg/MapMakerTiming.msg new file mode 100644 index 0000000..ad3fc37 --- /dev/null +++ b/msg/MapMakerTiming.msg @@ -0,0 +1,4 @@ +Header header +float32 elapsed +int32 map_num_mkfs +int32 map_num_points diff --git a/msg/TrackerTiming.msg b/msg/TrackerTiming.msg index fdc758f..866c60f 100644 --- a/msg/TrackerTiming.msg +++ b/msg/TrackerTiming.msg @@ -12,3 +12,5 @@ float32 pose float32 depth float32 add float32 total +int32 map_num_mkfs +int32 map_num_points diff --git a/src/MapMaker.cc b/src/MapMaker.cc index 154e719..37a9124 100644 --- a/src/MapMaker.cc +++ b/src/MapMaker.cc @@ -43,12 +43,17 @@ #include #include #include +#include MapMaker::MapMaker(Map &map, TaylorCameraMap &cameras, BundleAdjusterBase &bundleAdjuster) : MapMakerBase(map, true) , MapMakerClientBase(map) , MapMakerServerBase(map, cameras, bundleAdjuster) { + mCreationTimingPub = mNodeHandlePriv.advertise("timing_mkf_creation", 1, true); + mLocalTimingPub = mNodeHandlePriv.advertise("timing_local_ba", 1,true); + mGlobalTimingPub = mNodeHandlePriv.advertise("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: "< 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: "<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(); diff --git a/src/MapMakerBase.cc b/src/MapMakerBase.cc index 339dfd0..5dfcd3d 100644 --- a/src/MapMakerBase.cc +++ b/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(); diff --git a/src/SystemFrontendBase.cc b/src/SystemFrontendBase.cc index 3262e6a..8d83e23 100644 --- a/src/SystemFrontendBase.cc +++ b/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; diff --git a/src/Tracker.cc b/src/Tracker.cc index c4fc31f..ffaba22 100644 --- a/src/Tracker.cc +++ b/src/Tracker.cc @@ -114,7 +114,7 @@ Tracker::Tracker(Map &map, MapMakerClientBase &mapmaker, TaylorCameraMap &camera ROS_DEBUG("Tracker: In constructor, done"); maskPub = mNodeHandlePrivate.advertise("mask", 1); - timingPub = mNodeHandlePrivate.advertise("timing", 1); + timingPub = mNodeHandlePrivate.advertise("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); }