Allow to consider fixed poses when building edge set

roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilter.h

@@ -21,6 +21,7 @@
 
 #include <string>
 #include <vector>
+#include <boost/function.hpp>
 #include <boost/shared_ptr.hpp>
 
 #include <Eigen/Dense>
@@ -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<Eigen::VectorXd (double, const Eigen::VectorXd*)> 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<double, Eigen::VectorXd>) = 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() {
     }
 };

roamfree/ROAMestimation/include/ROAMestimation/FactorGraphFilterImpl.h

@@ -23,6 +23,7 @@
 #include <string>
 
 #include <boost/shared_ptr.hpp>
+#include <boost/function.hpp>
 #include <Eigen/Dense>
 
 #include "FactorGraphFilter.h"
@@ -156,6 +157,10 @@ class FactorGraphFilter_Impl: public FactorGraphFilter {
 
     bool forgetOldNodes(double l);
 
+    void setTrajectoryEstimate(boost::function<Eigen::VectorXd (double, const Eigen::VectorXd*)> trajectoryFunc);
+    bool setTrajectoryEstimate(std::map<double, Eigen::VectorXd>);
+    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);
 

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<Eigen::VectorXd (double, const Eigen::VectorXd*)> 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<double, Eigen::VectorXd> 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);