From 8215327f0e95e365a5349c1e66b7fc00f33f71b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Tue, 5 Apr 2022 18:39:59 +0200 Subject: [PATCH] tests passing --- include/core/trajectory/trajectory_base.h | 8 ++++++++ src/problem/problem.cpp | 1 + 2 files changed, 9 insertions(+) diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index e13e4f2cb..4fa2ec43e 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 ec734700d..847e49434 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>()) { -- GitLab