Skip to content
Snippets Groups Projects
Commit fe9e25ea authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Fix problem print timestamp + fix ftr prior setProblem

parent bb133d37
No related branches found
No related tags found
No related merge requests found
...@@ -959,7 +959,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c ...@@ -959,7 +959,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
if (metric) if (metric)
{ {
cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5) cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
<< F->getTimeStamp().get(); << F->getTimeStamp();
cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )"; cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
cout << endl; cout << endl;
} }
......
...@@ -180,7 +180,7 @@ void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorX ...@@ -180,7 +180,7 @@ void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorX
FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov); //std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov); FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov); //std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
// set feature problem // set feature problem
ftr_prior->setProblem(getProblem()); if(getProblem()) ftr_prior->setProblem(getProblem());
// create & add factor absolute // create & add factor absolute
if (is_quaternion) if (is_quaternion)
...@@ -431,6 +431,8 @@ void SensorBase::setProblem(ProblemPtr _problem) ...@@ -431,6 +431,8 @@ void SensorBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem); NodeBase::setProblem(_problem);
for (auto prc : processor_list_) for (auto prc : processor_list_)
prc->setProblem(_problem); prc->setProblem(_problem);
for(auto ftr_prior : params_prior_map_)
ftr_prior.second->setProblem(_problem);
} }
void SensorBase::link(HardwareBasePtr _hw_ptr) void SensorBase::link(HardwareBasePtr _hw_ptr)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment