diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index e13e4f2cbd6c845e4a138e613a700f1b812ebd56..4fa2ec43e5b5fd593561886420ccd2b26d646264 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/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;
 }
 
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index ec734700de21982cfe907a947b94566ef1413747..847e49434e6cf23c01781646c6358e5dc5bfe446 100644
--- a/src/problem/problem.cpp
+++ b/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>())
 {