diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f1f99f4c4bfb71389afc7fe1695355ad801f3c0..765441670adcd64066b6015507c734a5de11e1f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +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) 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 5cf7d1d48b0ae036decf7f33a05e0fb1359738b3..7e17ac4f9e1b44337923c8f19230b11fe2f76ea3 100644 --- a/include/core/common/time_stamp.h +++ b/include/core/common/time_stamp.h @@ -23,6 +23,7 @@ class TimeStamp { protected: unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970. + bool is_valid_; // time stamp has a valid value public: /** \brief Constructor @@ -60,6 +61,16 @@ class TimeStamp */ ~TimeStamp(); + /** \brief Value of time stamp is valid + * + */ + static TimeStamp Invalid ( ); + bool ok ( ) const; + void setOk ( ); + void setNOk ( ); + + static TimeStamp Now(); + /** \brief Time stamp to now */ void setToNow(); @@ -174,19 +185,42 @@ class TimeStamp }; +inline wolf::TimeStamp TimeStamp::Invalid ( ) +{ + return TimeStamp(-1.0); +} + +inline bool TimeStamp::ok ( ) const +{ + return is_valid_; +} + +inline void TimeStamp::setOk ( ) +{ + is_valid_ = true; +} + +inline void TimeStamp::setNOk ( ) +{ + is_valid_ = false; +} + inline void TimeStamp::set(const double& ts) { - time_stamp_nano_ = (ts > 0 ? (unsigned long int)(ts*NANOSECS) : 0); + time_stamp_nano_ = (ts >= 0 ? (unsigned long int)(ts*NANOSECS) : 0); + is_valid_ = (ts >= 0); } inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec) { time_stamp_nano_ = sec*NANOSECS+nanosec; + is_valid_ = true; } inline void TimeStamp::set(const timeval& ts) { time_stamp_nano_ = (unsigned long int)(ts.tv_sec*NANOSECS) + (unsigned long int)(ts.tv_usec*1000); + is_valid_ = (ts.tv_sec >= 0 and ts.tv_usec >= 0); } inline double TimeStamp::get() const @@ -207,11 +241,13 @@ inline unsigned long int TimeStamp::getNanoSeconds() const inline void TimeStamp::operator =(const TimeStamp& ts) { time_stamp_nano_ = ts.time_stamp_nano_; + is_valid_ = ts.is_valid_; } inline void TimeStamp::operator =(const double& ts) { time_stamp_nano_ = (unsigned long int)(ts*NANOSECS); + is_valid_ = (ts >= 0); } inline bool TimeStamp::operator ==(const TimeStamp& ts) const @@ -254,7 +290,7 @@ 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 InvalidStamp(-1,-1); +//static const TimeStamp TimeStampInvalid() {return TimeStamp(-1.0);} } // namespace wolf diff --git a/include/core/processor/processor_logging.h b/include/core/processor/processor_logging.h index de1d5ead89e38e28f15aa6b1c789350c3a0b0384..bc1f8399aadc6ac344973baa377b8ea8bdb85db7 100644 --- a/include/core/processor/processor_logging.h +++ b/include/core/processor/processor_logging.h @@ -9,7 +9,7 @@ #define _WOLF_PROCESSOR_LOGGING_H_ /// @brief un-comment for IDE highlights. -//#include "core/utils/logging.h" + #define __INTERNAL_WOLF_ASSERT_PROCESSOR \ static_assert(std::is_base_of<ProcessorBase, \ diff --git a/src/common/time_stamp.cpp b/src/common/time_stamp.cpp index 528d0ad54872bc077ba74d72be0cb2cb79099d13..595023484187d36e0bb5c8a6e361beda1ff2f089 100644 --- a/src/common/time_stamp.cpp +++ b/src/common/time_stamp.cpp @@ -3,8 +3,17 @@ namespace wolf { +TimeStamp TimeStamp::Now ( ) +{ + TimeStamp t(0); + t.setToNow(); + return t; +} + std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts) { + if (!_ts.ok()) + os << "TimeStamp is invalid! "; os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream os << std::setfill(' '); return os; @@ -13,24 +22,29 @@ std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts) TimeStamp::TimeStamp() : //time_stamp_(0) time_stamp_nano_(0) + , + is_valid_(false) { - setToNow(); +// setToNow(); } TimeStamp::TimeStamp(const TimeStamp& _ts) : - time_stamp_nano_(_ts.time_stamp_nano_) + time_stamp_nano_(_ts.time_stamp_nano_), + is_valid_(_ts.is_valid_) { // } TimeStamp::TimeStamp(const double& _ts) : - time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0) + time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0), + is_valid_(_ts > 0) { // } TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) : - time_stamp_nano_(_sec*NANOSECS+_nsec) + time_stamp_nano_(_sec*NANOSECS+_nsec), + is_valid_(true) { // } @@ -45,6 +59,7 @@ void TimeStamp::setToNow() timeval ts; gettimeofday(&ts, NULL); set(ts); + setOk(); } TimeStamp TimeStamp::operator +(const double& dt) const @@ -64,6 +79,7 @@ TimeStamp TimeStamp::operator -(const double& dt) const void TimeStamp::operator -=(const double& dt) { unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS); + is_valid_ = (time_stamp_nano_ >= dt_nano); time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano); } diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 4d2462f72d4e9bbf2fc6cb2c453389ea02028309..ee3884ba30cdd6f434956ebde18b86b6c424839a 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -18,7 +18,7 @@ #include "core/state_block/state_angle.h" #include "core/tree_manager/factory_tree_manager.h" #include "core/tree_manager/tree_manager_base.h" -#include "core/utils/logging.h" + #include "core/utils/params_server.h" #include "core/utils/loader.h" #include "core/utils/check_log.h" @@ -419,56 +419,63 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // TimeStamp Problem::getTimeStamp ( ) const { - if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state - { - auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); + TimeStamp ts = TimeStamp::Invalid(); - assert(last_kf_or_aux != nullptr && "Problem has no Keyframe so no timestamp can be obtained!"); + for (const auto& prc : processor_is_motion_list_) + if (prc->getTimeStamp().ok()) + if ( (not ts.ok() ) or prc->getTimeStamp() > ts) + ts = prc->getTimeStamp(); - return last_kf_or_aux->getTimeStamp(); - } - else + + if (not ts.ok()) { - TimeStamp ts(0); - for (const auto& prc : processor_is_motion_list_) - if (prc->getTimeStamp() > ts) - ts = prc->getTimeStamp(); - return ts; + const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); + + if (last_kf_or_aux) + ts = last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state + } + + if (not ts.ok()) + WOLF_WARN( "Problem has nowhere to find a valid timestamp!"); + + return ts; } VectorComposite Problem::getState(const StateStructure& _structure) const { StateStructure structure = (_structure == "" ? getFrameStructure() : _structure); + VectorComposite state; - if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state - { - auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); - if (last_kf_or_aux) - state = last_kf_or_aux->getState(structure); - else - state = stateZero(structure); - } - else // Compose from different processor motion + // compose the states of all processor motions into one only state + for (const auto& prc : processor_is_motion_list_) { - // compose the states of all processor motions into one only state - for (const auto& prc : processor_is_motion_list_) + const auto& prc_state = prc->getState(); + for (const auto& pair_key_vec : prc_state) { - const auto& prc_state = prc->getState(); - for (const auto& pair_key_vec : prc_state) - { - if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet - state.insert(pair_key_vec); - } + if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet + state.insert(pair_key_vec); } + } + + // check for empty blocks and fill them with the last KF, 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); - // check for empty blocks and fill them with zeros - for (const auto& ckey : frame_structure_) + for (const auto& ckey : structure) + { + const auto& key = string(1,ckey); + if (state.count(key) == 0) { - const auto& key = string(1,ckey); - if (state.count(key) == 0) + auto state_last_it = state_last.find(key); + + if (state_last_it != state_last.end()) + state.emplace(key, state_last_it->second); + + else state.emplace(key, stateZero(key).at(key)); } } @@ -480,20 +487,8 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ { StateStructure structure = (_structure == "" ? getFrameStructure() : _structure); - if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state - { - const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); - if (last_kf_or_aux) - { - return last_kf_or_aux->getState(structure); - } - else - { - return stateZero(structure); - } - } - VectorComposite state; + for (const auto& prc : processor_is_motion_list_) { const auto& prc_state = prc->getState(_ts); @@ -508,12 +503,24 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ } } - // check for empty blocks and fill them with zeros - for (const auto& ckey : frame_structure_) + // check for empty blocks and fill them with the closest KF to ts, 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); + + for (const auto& ckey : structure) { const auto& key = string(1,ckey); if (state.count(key) == 0) - state.emplace(key, stateZero(key).at(key)); + { + auto state_last_it = state_last.find(key); + + if (state_last_it != state_last.end()) + state.emplace(key, state_last_it->second); + + else + state.emplace(key, stateZero(key).at(key)); + } } return state; diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp index 454ff6dfb13d7db63818eafebc10ebec6630cf66..5586332e6eb0470e8ffef1c9b0eb2d9d2fd1b313 100644 --- a/test/gtest_ceres_manager.cpp +++ b/test/gtest_ceres_manager.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/problem/problem.h" #include "core/sensor/sensor_base.h" diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 457d41fe43f1adb01345065130f416f8ec785185..2c28c2bd442bc8675bf5e71fb82d97141cac8390 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/problem/problem.h" #include "core/sensor/sensor_base.h" diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp index 57235013f2c82a115a4fcafd389ac8421195651e..1a3f492384290e2bb14e8f01af153aa61abc1193 100644 --- a/test/gtest_factor_autodiff_distance_3d.cpp +++ b/test/gtest_factor_autodiff_distance_3d.cpp @@ -7,7 +7,7 @@ #include "core/factor/factor_autodiff_distance_3d.h" #include "core/problem/problem.h" -#include "core/utils/logging.h" + #include "core/ceres_wrapper/ceres_manager.h" #include "core/math/rotations.h" diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp index 05c9f8c7980df3bb47aeb80a0b28408302ba0b4c..e1e035bf0ae084d309b51db84b4c49ac62440924 100644 --- a/test/gtest_factor_base.cpp +++ b/test/gtest_factor_base.cpp @@ -7,7 +7,7 @@ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/factor/factor_base.h" diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp index 726fa7e47566cae2c1071b7c3511ace30f03308e..7edff3ed17f1a5b35e32cd44898ab4d77b48b001 100644 --- a/test/gtest_factory_state_block.cpp +++ b/test/gtest_factory_state_block.cpp @@ -10,7 +10,7 @@ #include "core/sensor/factory_sensor.h" #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + using namespace wolf; diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 9d1368d3b673bb1d00baf11c8100273cbc813d1d..abdefc83d7d4a3e1f041f5355b3139c1276775cb 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/frame/frame_base.h" #include "core/sensor/sensor_odom_2d.h" diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp index 39684eaf17c71ab8fcd4637683fe37f05139b5e2..8b6ed4fc1e2ca38cc5618eaf0ee587ff408f5d4d 100644 --- a/test/gtest_local_param.cpp +++ b/test/gtest_local_param.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/state_block/local_parametrization_quaternion.h" #include "core/state_block/local_parametrization_homogeneous.h" diff --git a/test/gtest_logging.cpp b/test/gtest_logging.cpp index ec6df4414d1934fd4c1f4c3dd989639848dbad82..58480003d32db34aec7b643bfa39c4b8259a2d3b 100644 --- a/test/gtest_logging.cpp +++ b/test/gtest_logging.cpp @@ -7,7 +7,7 @@ #include "core/common/wolf.h" #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + TEST(logging, info) { diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp index 3c74ede52ce7ec57a153cdcf416b9c84597cadd9..f47e9bd12826c323be8f61c882ed638ba35a5745 100644 --- a/test/gtest_motion_buffer.cpp +++ b/test/gtest_motion_buffer.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/processor/motion_buffer.h" diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp index 6faf7e424fe24d64bdad85c64c05e0df490da426..a512601e2b75ed7035f3b70745294ccffe1a9596 100644 --- a/test/gtest_param_prior.cpp +++ b/test/gtest_param_prior.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/problem/problem.h" #include "core/ceres_wrapper/ceres_manager.h" diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 20e272cfa698f904e3bb63c6d7730b11e0fe4568..0e13554d43eb92416f04321d6039f930b8d5a6e5 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -//#include "core/utils/logging.h" + #include "core/problem/problem.h" #include "core/sensor/sensor_base.h" diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 509c20d6ee2f9b809f4067ed03128ef7bca059a9..bcf69745cea695fef25bbb2238b0c4fddf334799 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -8,7 +8,7 @@ #include "core/utils/utils_gtest.h" #include "core/common/wolf.h" -#include "core/utils/logging.h" + #include "core/sensor/sensor_odom_2d.h" #include "core/processor/processor_odom_2d.h" diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp index 4a94d3e1b88d1afc89ef80a39b9fad1baa67a1f7..dc24beddd0643d7d64d21deeb59c2068752d50ec 100644 --- a/test/gtest_processor_odom_3d.cpp +++ b/test/gtest_processor_odom_3d.cpp @@ -8,7 +8,7 @@ #include "core/utils/utils_gtest.h" #include "core/common/wolf.h" -#include "core/utils/logging.h" + #include "core/processor/processor_odom_3d.h" diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index e8afad2af2283b3a0aaa2fdb7f9290fc9e633981..7e212b9971b5bf8fb1a46f94141ce61a1a5aab08 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/problem/problem.h" #include "core/sensor/sensor_base.h" diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp index 19d9e3cf63623ef275ee5f8fc8111f4e88ece4d1..dbcc4b6745e1c4ed99c1b1843c04fcbd7c1b546d 100644 --- a/test/gtest_state_block.cpp +++ b/test/gtest_state_block.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp index 169532a42076310bb819b85098e1b063902219f7..f8f70272b239a06f30ce5fdf6eed32308ae95689 100644 --- a/test/gtest_time_stamp.cpp +++ b/test/gtest_time_stamp.cpp @@ -3,9 +3,30 @@ #include <thread> +using namespace wolf; + +TEST(WolfTestTimeStamp, TimeStampInvalid) +{ + auto t = TimeStamp::Invalid(); + WOLF_DEBUG("t = ", t); + ASSERT_FALSE(t.ok()); + + t = -1; + WOLF_DEBUG("t = ", t); + ASSERT_FALSE(t.ok()); + + t = 0; + WOLF_DEBUG("t = ", t); + ASSERT_TRUE(t.ok()); + + t = 1; + WOLF_DEBUG("t = ", t); + ASSERT_TRUE(t.ok()); +} + TEST(WolfTestTimeStamp, TimeStampInitNow) { - wolf::TimeStamp start; + wolf::TimeStamp start = wolf::TimeStamp::Now(); // If we don't sleep, start == time_stamp sometimes. // And sometimes start <= time_stamp ... @@ -13,7 +34,7 @@ TEST(WolfTestTimeStamp, TimeStampInitNow) ASSERT_NE(start.get(), 0); - wolf::TimeStamp time_stamp; + wolf::TimeStamp time_stamp = wolf::TimeStamp::Now(); // std::cout << std::fixed; // std::cout << std::setprecision(15); @@ -137,11 +158,11 @@ TEST(WolfTestTimeStamp, TimeStampEquality) TEST(WolfTestTimeStamp, TimeStampInequality) { - wolf::TimeStamp start; + wolf::TimeStamp start = wolf::TimeStamp::Now(); std::this_thread::sleep_for(std::chrono::microseconds(1)); - wolf::TimeStamp time_stamp; + wolf::TimeStamp time_stamp = wolf::TimeStamp::Now(); // error: no match for ‘operator!=’ //ASSERT_NE(time_stamp, start); diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index 9be1e82fafac833ca90d000e487072d5488ff240..00f7d83fff99ff39b16fc8a67c2eee9b767cfbe1 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -6,7 +6,7 @@ */ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/problem/problem.h" #include "core/trajectory/trajectory_base.h" diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp index acd3da51ab0ddd7ccabe960f06e9dbbc8abe54e9..1d97e2925487f9e363e1292b6ef2c2a82c29fc96 100644 --- a/test/gtest_tree_manager.cpp +++ b/test/gtest_tree_manager.cpp @@ -1,5 +1,5 @@ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/problem/problem.h" #include "dummy/tree_manager_dummy.h" diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index db8a2e87d386116c6b1c468b457471aae98e4404..5cbc9550cc9a751a57046807f236486f4d5cbb92 100644 --- a/test/gtest_tree_manager_sliding_window.cpp +++ b/test/gtest_tree_manager_sliding_window.cpp @@ -1,5 +1,5 @@ #include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" + #include "core/problem/problem.h" #include "core/tree_manager/tree_manager_sliding_window.h" diff --git a/wolf_scripts/templates/gtest_template.cpp b/wolf_scripts/templates/gtest_template.cpp index ccfec27db00cba6d92ac7f5d6b91718db1896be2..2acdaa25f334608ea1e9b9ce2cdec7e4642f6b84 100644 --- a/wolf_scripts/templates/gtest_template.cpp +++ b/wolf_scripts/templates/gtest_template.cpp @@ -1,7 +1,7 @@ #include "core/utils/utils_gtest.h" #include "wolf.h" -#include "logging.h" +//#include "logging.h" #include "header_file"