Add asser for repeated timestamp in trajectory

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;
 }