From 771091578f3b893a9d1ff250153ee37694dad913 Mon Sep 17 00:00:00 2001 From: Simon Gilgien Date: Thu, 21 May 2026 14:32:31 +0200 Subject: [PATCH 1/3] Allow to consider fixed poses when building edge set The rationale is that even if a pose is fixed, there might be connected edges leading to unfixed auxiliary parameters. Example use case: estimate camera calibration using static synthetic GCPs --- .../ROAMestimation/include/ROAMestimation/FactorGraphFilter.h | 3 ++- .../include/ROAMestimation/FactorGraphFilterImpl.h | 2 +- roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h index fd82a36..49e3377 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h @@ -529,8 +529,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 diff --git a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h index 24f83f1..21cde18 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h @@ -190,7 +190,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..363284a 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()); } } From 50f11e1ac5e99e2bac97c15fc2c74981e3b7486c Mon Sep 17 00:00:00 2001 From: Simon Gilgien Date: Fri, 29 May 2026 15:15:08 +0200 Subject: [PATCH 2/3] Allow to fix all poses in the graph. For use e.g. with a good INS/GNSS solution for boresight calibration. --- .../ROAMestimation/FactorGraphFilter.h | 20 ++++++++++++++----- .../ROAMestimation/FactorGraphFilterImpl.h | 2 ++ .../src/FactorGraphFilterImpl.cpp | 6 ++++++ 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h index 49e3377..9d0c5f2 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h @@ -47,7 +47,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 +407,16 @@ class FactorGraphFilter { */ virtual bool forgetOldNodes(double l) = 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 +475,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 * @@ -543,13 +553,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 21cde18..b23a6c7 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h @@ -156,6 +156,8 @@ class FactorGraphFilter_Impl: public FactorGraphFilter { bool forgetOldNodes(double l); + void setAllPosesFixed(bool fixed); + /* --------------------------- PRIOR CONTROL METHODS ------------------------------ */ MeasurementEdgeWrapper_Ptr addPriorOnPose(PoseVertexWrapper_Ptr pose, diff --git a/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp b/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp index 363284a..7e99cf2 100644 --- a/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp +++ b/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp @@ -2009,6 +2009,12 @@ bool FactorGraphFilter_Impl::forgetOldNodes(double l) { return forgetNodes_i(toForget); } +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); From 66c84c0d758a554578ec16a09e9fc41aaacb445e Mon Sep 17 00:00:00 2001 From: Simon Gilgien Date: Tue, 2 Jun 2026 17:15:09 +0200 Subject: [PATCH 3/3] FactorGraph: add methods to set the trajectory estimate Takes either a map or a function returning the pose vector given a timestamp. This is useful to implement fixed trajectory (to avoid double interpolation) --- .../ROAMestimation/FactorGraphFilter.h | 20 +++++++ .../ROAMestimation/FactorGraphFilterImpl.h | 3 ++ .../src/FactorGraphFilterImpl.cpp | 54 +++++++++++++++++++ 3 files changed, 77 insertions(+) diff --git a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h index 9d0c5f2..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 @@ -407,6 +408,25 @@ 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 diff --git a/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h b/roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h index b23a6c7..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,8 @@ 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 ------------------------------ */ diff --git a/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp b/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp index 7e99cf2..42d49f5 100644 --- a/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp +++ b/roamfree/ROAMestimation/src/FactorGraphFilterImpl.cpp @@ -2009,6 +2009,60 @@ 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);