tests passing

include/core/trajectory/trajectory_base.h

@@ -170,21 +170,29 @@ inline const FramePtrMap& TrajectoryBase::getFrameMap()
 
 inline FrameBaseConstPtr TrajectoryBase::getFirstFrame() const
 {
+    if (frame_const_map_.empty())
+        return nullptr;
     return frame_const_map_.begin()->second;
 }
 
 inline FrameBasePtr TrajectoryBase::getFirstFrame()
 {
+    if (frame_map_.empty())
+        return nullptr;
     return frame_map_.begin()->second;
 }
 
 inline FrameBaseConstPtr TrajectoryBase::getLastFrame() const
 {
+    if (frame_const_map_.empty())
+        return nullptr;
     return frame_const_map_.rbegin()->second;
 }
 
 inline FrameBasePtr TrajectoryBase::getLastFrame()
 {
+    if (frame_map_.empty())
+        return nullptr;
     return frame_map_.rbegin()->second;
 }
 

src/problem/problem.cpp

@@ -68,6 +68,7 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
         map_ptr_(_map),
         motion_provider_map_(),
+        motion_provider_const_map_(),
         frame_structure_(_frame_structure),
         prior_options_(std::make_shared<PriorOptions>())
 {