diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 3e78796c928c38aed9c59238eaa9d5b76b0491ca..023a64a5cdab1ca95da0ae91785b777a930a064e 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -5,8 +5,6 @@ namespace wolf {
 
 TrajectoryBase::TrajectoryBase() :
     NodeBase("TRAJECTORY", "TrajectoryBase")
-    // last_key_frame_ptr_(nullptr),
-    // last_key_or_aux_frame_ptr_(nullptr)
 {
 //    WOLF_DEBUG("constructed T");
     frame_list_ = FrameList();
@@ -20,7 +18,9 @@ TrajectoryBase::~TrajectoryBase()
 FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
-    frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
+    assert(frame_list_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
+
+    frame_list_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
 
     return _frame_ptr;
 }