diff --git a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h index fd82a36..fe42b6a 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -47,7 +48,7 @@ class FactorGraphFilter { virtual void setSolverMethod(SolverMethod method) = 0; /** - * \brief Set the threshold for early stopping estimation if chi2 does not improve more than + * \brief Set the threshold for early stopping estimation if chi2 does not improve more than */ virtual void setChi2Threshold(double threshold) = 0; @@ -407,6 +408,35 @@ class FactorGraphFilter { */ virtual bool forgetOldNodes(double l) = 0; + /** + * \brief Set the trajectory estimate + * + * This overload takes a function returning the pose estimate for every timestep + * + * @param trajectory a function returning the pose estimate for every timestep; + */ + virtual void setTrajectoryEstimate(boost::function trajectoryFunc) = 0; + + /** + * \brief Set the trajectory estimate + * + * This overload takes a map from timestamps to pose estimates. This will be interpolated if a pose is not provided + * closer than 1e-6 second from the required timestamp + * + * @param trajectory a function returning the pose estimate for every timestep; + * @return success + */ + virtual bool setTrajectoryEstimate(std::map) = 0; + + /** + * \brief Fix or unfix all poses + * + * All poses in the graph will be fixed or unfixed. Note that this does not affect + * subsequent poses (they will be unfixed by default). + * + * @param fixed whether to fix or unfix the poses + */ + virtual void setAllPosesFixed(bool fixed) = 0; /* --------------------------- PRIOR CONTROL METHODS ------------------------------ */ /** @@ -465,7 +495,7 @@ class FactorGraphFilter { */ virtual PoseVertexWrapper_Ptr getNearestPoseByTimestamp(double t, bool onlyBefore = false) = 0; - + /** * \brief returns the two pose whose timestamp is nearer with respect to t * @@ -529,8 +559,9 @@ class FactorGraphFilter { * according to their 'fixed' property * * @param nIterations the number of Gauss-Newton/Levenberg-Marquardt iteration to perform. + * @param includeFixedPoses whether to include edges from fixed poses (e.g. to estimate auxilliary parameters) */ - virtual bool estimate(int nIterations) = 0; + virtual bool estimate(int nIterations, bool includeFixedPoses=false) = 0; /** * \brief runs the estimations considering the markov blanket of the provided pose vector @@ -542,13 +573,13 @@ class FactorGraphFilter { * @param nIterations the number of Gauss-Newton/Levenberg-Marquardt iteration to perform. */ virtual bool estimate(PoseVertexWrapperVector poses, int nIterations) = 0; - + /** * \brief compytes the cross correlation covariances between the parameters */ - + virtual void computeCrossCovariances() = 0; - + virtual ~FactorGraphFilter() { } }; diff --git a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h index 24f83f1..1a64176 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h @@ -23,6 +23,7 @@ #include #include +#include #include #include "FactorGraphFilter.h" @@ -156,6 +157,10 @@ class FactorGraphFilter_Impl: public FactorGraphFilter { bool forgetOldNodes(double l); + void setTrajectoryEstimate(boost::function trajectoryFunc); + bool setTrajectoryEstimate(std::map); + void setAllPosesFixed(bool fixed); + /* --------------------------- PRIOR CONTROL METHODS ------------------------------ */ MeasurementEdgeWrapper_Ptr addPriorOnPose(PoseVertexWrapper_Ptr pose, @@ -190,7 +195,7 @@ class FactorGraphFilter_Impl: public FactorGraphFilter { /* --------------------------- ESTIMATION CONTROM METHODS ------------------------- */ - bool estimate(int nIterations); + bool estimate(int nIterations, bool includeFixedPoses=false); bool estimate(PoseVertexWrapperVector poses, int nIterations); diff --git a/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp b/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp index 17f6496..42d49f5 100644 --- a/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp +++ b/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp @@ -1724,7 +1724,7 @@ PoseVertex *FactorGraphFilter_Impl::getNthOldestPose_i(int n) { } } -bool FactorGraphFilter_Impl::estimate(int nIterations) { +bool FactorGraphFilter_Impl::estimate(int nIterations, bool includeFixedPoses) { // we handle priors only in case of full estimation handlePriorsOnOldestPose(); @@ -1735,7 +1735,7 @@ bool FactorGraphFilter_Impl::estimate(int nIterations) { g2o::HyperGraph::EdgeSet eset; for (auto pit = _poses.begin(); pit != _poses.end(); ++pit) { - if (!pit->second->fixed()) { + if (!pit->second->fixed() || includeFixedPoses) { eset.insert(pit->second->edges().begin(), pit->second->edges().end()); } } @@ -2009,6 +2009,66 @@ bool FactorGraphFilter_Impl::forgetOldNodes(double l) { return forgetNodes_i(toForget); } +void FactorGraphFilter_Impl::setTrajectoryEstimate(boost::function trajectoryFunc) { + Eigen::VectorXd estimate = _poses.begin()->second->estimate(); + for(PoseMapIterator it = _poses.begin(); it != _poses.end(); ++it) { + estimate = trajectoryFunc(it->first, &estimate); + it->second->setEstimate(estimate); + } +} +bool FactorGraphFilter_Impl::setTrajectoryEstimate(std::map trajectory) { + auto after = trajectory.begin(); + + for(PoseMapIterator it = _poses.begin(); it != _poses.end(); ++it) { + double ti = it->first; + while(after->first <= ti + 1e-6 && after != trajectory.end()) { + ++after; + } + + if (after == trajectory.begin()) { + return false; + } + auto before = after; + --before; + + // This should not happen except if trajectory has points closer than 1e-6 seconds + while (before->first > ti && before != trajectory.begin()) { + --before; + --after; + } + + if (before->first + 1e-6 >= ti) { + it->second->setEstimate(before->second); + } else if (after == trajectory.end()) { + return false; + } else if (after->first - 1e-6 <= ti) { + it->second->setEstimate(after->second); + } else { + // We have to interpolate + double t1 = before->first; + double t2 = after->first; + const Eigen::VectorXd& x1 = before->second; + const Eigen::VectorXd& x2 = after->second; + + Eigen::VectorXd x(7); + + const double delay = 0.0; + const int _OFF = -1; + +#include "generated/SE3InterpolationEdge_Xhat.cppready" + + it->second->setEstimate(x); + } + } + return true; +} + +void FactorGraphFilter_Impl::setAllPosesFixed(bool fixed) { + for(PoseMapIterator it = _poses.begin(); it != _poses.end(); ++it) { + it->second->setFixed(fixed); + } +} + void FactorGraphFilter_Impl::deferMeasurement(struct Sensor& sensor, double t, const Eigen::VectorXd& z, const Eigen::MatrixXd& cov) { struct Measurement meas(sensor, t, z, cov);