diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 7bc9505f2a3316de7678d54070ea92e38440ebc9..3ba49645c9c81613e3cc32855ca16786dd17dcd9 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -174,7 +174,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* Add a new processor of type is motion to the processor is motion list.
*/
void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr);
- void clearProcessorIsMotion(IsMotionPtr proc);
+ void removeProcessorIsMotion(IsMotionPtr proc);
public:
IsMotionPtr getProcessorIsMotion();
@@ -321,6 +321,7 @@ class Problem : public std::enable_shared_from_this<Problem>
TimeStamp getTimeStamp ( ) const;
VectorComposite getState ( const StateStructure& _structure = "" ) const;
VectorComposite getState ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
+ VectorComposite getOdometry ( const StateStructure& _structure = "" ) const;
// Zero state provider
VectorComposite stateZero ( const StateStructure& _structure = "" ) const;
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 7f3e2864dce16dc3277b7d5b2eecbbf786f28b74..62eb56dfee89579decc05055b621dcc193782527 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -52,14 +52,12 @@ class IsMotion
virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
VectorComposite getOdometry ( ) const;
+ void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;}
bool isStateGetter() const;
int getStatePriority() const;
void setStatePriority(int);
- private:
- void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;}
-
public:
const StateStructure& getStateStructure ( ) { return state_structure_; };
void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index c7a125ac0d96b1d1b0799ab9e983bccf76bf36db..f820fc5bab837b85e68263609f5a9102a7580bd4 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -69,6 +69,14 @@ class VectorComposite : public std::unordered_map < char, Eigen::VectorXd >
void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
void setZero();
+ bool includesStructure(const StateStructure& _structure)
+ {
+ for (auto key : _structure)
+ if (count(key) == 0)
+ return false;
+ return true;
+ }
+
friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c2460d5d98a30f86d42bea47ede344e1076b0abd..121ffc87319209b55d361ef8e6c0d0defff93ce7 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -537,6 +537,49 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
return state;
}
+VectorComposite Problem::getOdometry(const StateStructure& _structure) const
+{
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
+ VectorComposite odom_state;
+
+ // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state
+ for (const auto& prc_pair : processor_is_motion_map_)
+ {
+ const auto& prc_state = prc_pair.second->getOdometry();
+
+ // transfer processor vector blocks to problem state
+ for (const auto& pair_key_vec : prc_state)
+ {
+ if (odom_state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
+ odom_state.insert(pair_key_vec);
+ }
+ }
+
+ // check for empty blocks and fill them with the the prior, or with zeros in the worst case
+
+ VectorComposite state_last;
+
+ if (prior_options_ and prior_options_->mode != "nothing")
+ state_last = prior_options_->state;
+
+ for (const auto& key : structure)
+ {
+ if (odom_state.count(key) == 0)
+ {
+ auto state_last_it = state_last.find(key);
+
+ if (state_last_it != state_last.end())
+ odom_state.emplace(key, state_last_it->second);
+
+ else
+ odom_state.emplace(key, stateZero(string(1,key)).at(key));
+ }
+ }
+
+ return odom_state;
+}
+
SizeEigen Problem::getDim() const
{
return dim_;
@@ -587,7 +630,7 @@ void Problem::addProcessorIsMotion(IsMotionPtr _is_motion_ptr)
appendToStructure(_is_motion_ptr->getStateStructure());
}
-void Problem::clearProcessorIsMotion(IsMotionPtr proc)
+void Problem::removeProcessorIsMotion(IsMotionPtr proc)
{
WOLF_WARN_COND(processor_is_motion_map_.count(proc->getStatePriority()) == 0, "Problem::clearProcessorIsMotion: missing processor");
@@ -986,6 +1029,8 @@ void Problem::setPriorOptions(const std::string& _mode,
prior_options_->state = _state;
prior_options_->time_tolerance = _time_tolerance;
+ assert(prior_options_->state.includesStructure(frame_structure_));
+
if (prior_options_->mode == "factor")
{
bool isPositive = true;
@@ -1017,8 +1062,11 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
- prior_keyframe = emplaceFrame(_ts,
- prior_options_->state);
+ prior_keyframe = emplaceFrame(_ts, prior_options_->state);
+
+ // Update origin for odometry processors
+ for (auto proc_pair : processor_is_motion_map_)
+ proc_pair.second->setOdometry(prior_options_->state);
if (prior_options_->mode == "fix")
prior_keyframe->fix();
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
index 106a60df2bc0077c8903208d9c851d762efb58e2..4309f6a68fb889ac2151b5e33c6429d80ce23b9b 100644
--- a/src/processor/is_motion.cpp
+++ b/src/processor/is_motion.cpp
@@ -13,3 +13,10 @@ void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
setOdometry(_prb_ptr->stateZero(state_structure_));
_prb_ptr->addProcessorIsMotion(_motion_ptr);
}
+
+void IsMotion::setOdometry(const VectorComposite& _zero_odom)
+{
+ for (auto key : state_structure_)
+ if (_zero_odom.count(key) != 0)
+ odometry_[key] = _zero_odom.at(key); //overwrite or insert only keys of the state_structure_
+}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index e8f891b8c51098909b5db479f7e3d3a8139457c0..794699f4533a7d2f5e6271e8f31b05b0b36cbd69 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -91,7 +91,7 @@ void ProcessorBase::remove()
ProblemPtr P = getProblem();
auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() );
if(P && this_proc_cast_attempt )
- P->clearProcessorIsMotion(this_proc_cast_attempt);
+ P->removeProcessorIsMotion(this_proc_cast_attempt);
}
// remove from upstream