diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 9d306d5983883edea43f8712f07484d2086df065..8e5c10205a488143a7f5fc44dbf603658768cf5c 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -46,7 +46,7 @@ class Problem : public std::enable_shared_from_this<Problem> HardwareBasePtr hardware_ptr_; TrajectoryBasePtr trajectory_ptr_; MapBasePtr map_ptr_; - IsMotionPtr processor_motion_ptr_; + std::vector<IsMotionPtr> processor_is_motion_vec_; // IsMotionPtr is_motion_ptr_; std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_; SizeEigen state_size_, state_cov_size_, dim_; @@ -149,8 +149,8 @@ class Problem : public std::enable_shared_from_this<Problem> * * Set the processor motion. */ - void setProcessorMotion(IsMotionPtr _processor_motion_ptr); - IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name); + void addProcessorMotion(IsMotionPtr _processor_motion_ptr); + IsMotionPtr addProcessorMotion(const std::string& _unique_processor_name); void clearProcessorMotion(); public: @@ -364,7 +364,14 @@ inline bool Problem::priorIsSet() const inline IsMotionPtr& Problem::getProcessorMotion() { - return processor_motion_ptr_; + if (!processor_is_motion_vec_.empty()) + return processor_is_motion_vec_[0]; + return nullptr +} + +inline IsMotionPtr& Problem::getIsMotionVector() +{ + return processor_is_motion_vec_; } inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap() diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index ff4c8b3acbdb47dfcd5ff6b28748185760be62fc..57c164469f6e12c888ea9b6d280f8df7dfc3edaf 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -466,7 +466,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion protected: // Attributes - std::string state_structure_;///< The structure of the state vector (to retrieve state blocks from frames) SizeEigen x_size_; ///< The size of the state vector SizeEigen data_size_; ///< the size of the incoming data SizeEigen delta_size_; ///< the size of the deltas diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 26ade79a1b0a3b7b285838683815ce2dd97d7adb..5b741765daf9ab29ceeecba4c7198081d9039092 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -34,7 +34,7 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) : hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)), map_ptr_(std::make_shared<MapBase>()), - processor_motion_ptr_(), + processor_is_motion_vec_(std::vector<IsMotionPtr>()), prior_is_set_(false), frame_structure_(_frame_structure) { @@ -227,9 +227,9 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // if (prc_ptr->isMotion() && prior_is_set_) (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); - // setting the main processor motion - if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr) - processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr); + // adding processor is motion to the processor is motion vector + if (prc_ptr->isMotion()) + processor_is_motion_vec_.push_back(std::dynamic_pointer_cast<IsMotion>(prc_ptr)); return prc_ptr; } @@ -269,9 +269,9 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // if (prc_ptr->isMotion() && prior_is_set_) (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); - // setting the main processor motion - if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr) - processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr); + // adding processor is motion to the processor is motion vector + if (prc_ptr->isMotion()) + processor_is_motion_vec_.push_back(std::dynamic_pointer_cast<IsMotion>(prc_ptr)); return prc_ptr; } @@ -291,7 +291,12 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const return (*sen_it); } -IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name) +void Problem::addProcessorMotion(IsMotionPtr _processor_motion_ptr) +{ + processor_is_motion_vec_.push_back(_processor_motion_ptr); +} + +IsMotionPtr Problem::addProcessorMotion(const std::string& _processor_name) { for (auto sen : getHardware()->getSensorList()) // loop all sensors { @@ -306,9 +311,10 @@ IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name) { if ((*prc_it)->isMotion()) // found, and it's motion! { - std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl; - processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it); - return processor_motion_ptr_; + std::cout << "Found processor '" << _processor_name << "', of type Motion, and added to the motion processor vector." << std::endl; + IsMotionPtr motion_processor_ptr = std::dynamic_pointer_cast<IsMotion>(*prc_it); + processor_is_motion_vec_.push_back(motion_processor_ptr); + return motion_processor_ptr; } else // found, but it's not motion! { @@ -322,15 +328,11 @@ IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name) return nullptr; } -void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr) -{ - processor_motion_ptr_ = _processor_motion_ptr; -} - -void Problem::clearProcessorMotion() -{ - processor_motion_ptr_.reset(); -} +// Not used in the code based --> TODO +// void Problem::clearProcessorMotion() +// { +// processor_motion_ptr_.reset(); +// } FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, // const SizeEigen _dim, // @@ -395,17 +397,30 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) co else _state = zeroState(); } +VectorXd full_state = problem->getCurrentState(); +Vector3d angular_momentum = full_state.segment<3>(16); void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const { // try to get the state from processor_motion if any, otherwise... - if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state)) - { + // if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state)) + // { + // FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); + // if (closest_frame != nullptr) + // closest_frame->getState(_state); + // else + // _state = zeroState(); + // } + if (processor_is_motion_vec_.empty()){ FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); if (closest_frame != nullptr) closest_frame->getState(_state); else - _state = zeroState(); + _state = zeroState(); // TODO: generic implementation ? + } + else { + std::list<Eigen::VectorXd> states_to_concat; + } } diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 3cf7743c4e673dc358f076db4dbc56376a71f46e..302923f94a6c23e07e493e1d463d820db6f8d467 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -60,27 +60,28 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr); } -void ProcessorBase::remove() -{ - if (!is_removing_) - { - is_removing_ = true; - ProcessorBasePtr this_p = shared_from_this(); - - // clear Problem::processor_motion_ptr_ - if (isMotion()) - { - ProblemPtr P = getProblem(); - if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) ) - P->clearProcessorMotion(); - } - - // remove from upstream - SensorBasePtr sen = sensor_ptr_.lock(); - if(sen) - sen->removeProcessor(this_p); - } -} +// Not used in the code base --> TODO +// void ProcessorBase::remove() +// { +// if (!is_removing_) +// { +// is_removing_ = true; +// ProcessorBasePtr this_p = shared_from_this(); + +// // clear Problem::processor_motion_ptr_ +// if (isMotion()) +// { +// ProblemPtr P = getProblem(); +// if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) ) +// P->clearProcessorMotion(); +// } + +// // remove from upstream +// SensorBasePtr sen = sensor_ptr_.lock(); +// if(sen) +// sen->removeProcessor(this_p); +// } +// } void ProcessorBase::link(SensorBasePtr _sen_ptr) { diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 387f4a2170a296312fa057cc5971ee10c3ca84c4..56e9adcadd9bd4500467e679c5826d7de2545fe1 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -629,8 +629,9 @@ void ProcessorMotion::setProblem(ProblemPtr _problem) this->setOrigin(this->getProblem()->getLastKeyFrame()); // set the main processor motion - if (this->getProblem()->getProcessorMotion() == nullptr) - this->getProblem()->setProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this())); + if (this->getProblem()->getProcessorMotion() == nullptr){ + this->getProblem()->addProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this())); + } }; bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)