Resolve "IsMotion used in Problem::getState()"

Closes #399 (closed)

include/core/problem/problem.h

@@ -58,7 +58,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         HardwareBasePtr     hardware_ptr_;
         TrajectoryBasePtr   trajectory_ptr_;
         MapBasePtr          map_ptr_;
-        std::list<IsMotionPtr>  processor_is_motion_list_;
+        std::map<int, IsMotionPtr>  processor_is_motion_map_;
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
         SizeEigen state_size_, state_cov_size_, dim_;
         std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -178,7 +178,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
     public:
         IsMotionPtr getProcessorIsMotion();
-        std::list<IsMotionPtr> getProcessorIsMotionList();
+        std::map<int,IsMotionPtr> getProcessorIsMotionMap();
         
 
 
@@ -431,15 +431,15 @@ inline bool Problem::isPriorSet() const
 
 inline IsMotionPtr Problem::getProcessorIsMotion()
 {
-    if (!processor_is_motion_list_.empty())
-        return processor_is_motion_list_.front();
+    if (not processor_is_motion_map_.empty())
+        return processor_is_motion_map_.begin()->second;
     return nullptr;
 }
 
 
-inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
+inline std::map<int,IsMotionPtr> Problem::getProcessorIsMotionMap()
 {
-    return processor_is_motion_list_;
+    return processor_is_motion_map_;
 }
 
 inline SizeStd Problem::getStateBlockNotificationMapSize() const