diff --git a/CMakeLists.txt b/CMakeLists.txt index 765441670adcd64066b6015507c734a5de11e1f3..9112af5f16d8a84404e203361c568563ccb88dee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,7 @@ PROJECT(wolf) SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin) SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) SET(CMAKE_INSTALL_PREFIX /usr/local) -SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY TRUE) +SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE) IF (NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE "DEBUG") diff --git a/include/core/common/time_stamp.h b/include/core/common/time_stamp.h index 7e17ac4f9e1b44337923c8f19230b11fe2f76ea3..2643f13c8f9629dce921726d6dfacf3171c3417f 100644 --- a/include/core/common/time_stamp.h +++ b/include/core/common/time_stamp.h @@ -290,8 +290,6 @@ inline double TimeStamp::operator -(const TimeStamp& ts) const return double((long int)(time_stamp_nano_ - ts.time_stamp_nano_))*1e-9; // long int cast fix overflow in case of negative substraction result } -//static const TimeStamp TimeStampInvalid() {return TimeStamp(-1.0);} - } // namespace wolf #endif diff --git a/src/common/time_stamp.cpp b/src/common/time_stamp.cpp index 595023484187d36e0bb5c8a6e361beda1ff2f089..0421efa9b06aaf3f2f75796910485aa687217bba 100644 --- a/src/common/time_stamp.cpp +++ b/src/common/time_stamp.cpp @@ -37,7 +37,7 @@ TimeStamp::TimeStamp(const TimeStamp& _ts) : TimeStamp::TimeStamp(const double& _ts) : time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0), - is_valid_(_ts > 0) + is_valid_(_ts >= 0) { // } diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index ee3884ba30cdd6f434956ebde18b86b6c424839a..caf9e1115582963c64980a02bf10f15a8b0c108e 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -460,10 +460,17 @@ VectorComposite Problem::getState(const StateStructure& _structure) const } } - // check for empty blocks and fill them with the last KF, or with zeros in the worst case + // check for empty blocks and fill them with the last KF, with the prior, or with zeros in the worst case + VectorComposite state_last; + const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); - if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure); + + if (last_kf_or_aux) + state_last = last_kf_or_aux->getState(structure); + + else if (prior_options_ and prior_options_->mode != "nothing") + state_last = prior_options_->state; for (const auto& ckey : structure) { @@ -503,10 +510,17 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ } } - // check for empty blocks and fill them with the closest KF to ts, or with zeros in the worst case + // check for empty blocks and fill them with the closest KF to ts, with the prior, or with zeros in the worst case + VectorComposite state_last; + const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); - if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure); + + if (last_kf_or_aux) + state_last = last_kf_or_aux->getState(structure); + + else if (prior_options_ and prior_options_->mode != "nothing") + state_last = prior_options_->state; for (const auto& ckey : structure) { diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 9aea50f6cb7dd123bcc8728bda1b9a46fcc0335e..7fbec4d4acbd7612467f0177c41c6d4641fa063c 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -58,6 +58,24 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source, const FrameBasePtr& _keyframe_target, const wolf::CaptureMotionPtr& _capture_target) { + /** we are doing this: + * + * Before split: + * + * ts_split + * KF | F or KF + * * -----------------------* + * origin source + * + * + * After split: + * + * KF KF F or KF + * * ----------* -----------* + * origin target source + * + */ + // split the buffer // and give the part of the buffer before the new keyframe to the capture for the KF callback _capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());