develop imu_integration

svo/include/svo/imu_integration.h

@@ -7,17 +7,93 @@
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/ProjectionFactor.h>
 #include <sophus/se3.h>
 #include <svo/frame.h>
 
 using gtsam::symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
 using gtsam::symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
 using gtsam::symbol_shorthand::P;  // Pose3 (x,y,z,r,p,y)
+using gtsam::symbol_shorthand::F;  // Feature
+
+//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop
+// Wrapper class
+template <typename T>
+class enumerate_impl
+{
+public:
+    // The return value of the operator* of the iterator, this
+    // is what you will get inside of the for loop
+    struct item
+    {
+        size_t index;
+        typename T::value_type & item;
+    };
+    typedef item value_type;
+
+    // Custom iterator with minimal interface
+    struct iterator
+    {
+        iterator(typename T::iterator _it, size_t counter=0) :
+                it(_it), counter(counter)
+        {}
+
+        iterator operator++()
+        {
+            return iterator(++it, ++counter);
+        }
+
+        bool operator!=(iterator other)
+        {
+            return it != other.it;
+        }
+
+        typename T::iterator::value_type item()
+        {
+            return *it;
+        }
+
+        value_type operator*()
+        {
+            return value_type{counter, *it};
+        }
+
+        size_t index()
+        {
+            return counter;
+        }
+
+    private:
+        typename T::iterator it;
+        size_t counter;
+    };
+
+    enumerate_impl(T & t) : container(t) {}
+
+    iterator begin()
+    {
+        return iterator(container.begin());
+    }
+
+    iterator end()
+    {
+        return iterator(container.end());
+    }
+
+private:
+    T & container;
+};
+
+// A templated free function allows you to create the wrapper class
+// conveniently
+template <typename T>
+enumerate_impl<T> enumerate(T & t)
+{
+    return enumerate_impl<T>(t);
+}
 
 class Imu_Integration{
 public:
@@ -33,6 +109,8 @@ private:
     std::unique_ptr<gtsam::NonlinearFactorGraph> graphPtr;
     boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
     std::shared_ptr<gtsam::PreintegrationType> preintegratedPtr;
+    boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
+    boost::shared_ptr<gtsam::Cal3_S2> cameraPtr;
     std::uint32_t t_1=1e-18;
     std::uint32_t imu_factor_id=0;
 };

svo/src/imu_integration.cpp

@@ -5,6 +5,8 @@
 #include "svo/imu_integration.h"
 #include <memory>
 #include <utility>
+#include "svo/feature.h"
+#include "svo/point.h"
 
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(gtsam::Rot3(SE_init.rotation_matrix()),SE_init.translation()),
@@ -18,15 +20,16 @@ Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()));
     graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 0.1));
     graphPtr->addPrior(B(0),*imu_biasPtr.get(),gtsam::noiseModel::Isotropic::Sigma(6, 1e-3));
-    parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();
+    parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
     parameterPtr->accelerometerCovariance=gtsam::I_3x3 * pow(0.0003924, 2); // acc white noise in continuous
     parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * pow(0.00020568902491,2); // gyro white noise in continuous
     parameterPtr->integrationCovariance=gtsam::I_3x3 * 1e-8; // integration uncertainty continuous
-    // PreintegrationCombinedMeasurements params:
     parameterPtr->biasAccCovariance = gtsam::I_3x3 * pow(0.004905, 2);;   // acc bias in continuous
     parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * pow(0.000001454441043, 2);  // gyro bias in continuous
     parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-5; // error in the bias used for preintegration
     preintegratedPtr = std::make_shared<gtsam::PreintegrationType>(parameterPtr,*imu_biasPtr.get());
+    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // error in the bias used for projection
+    cameraPtr=boost::make_shared<gtsam::Cal3_S2>(15,12,20,3,12);
     assert(preintegratedPtr);
 }
 Imu_Integration::~Imu_Integration(){
@@ -47,12 +50,35 @@ bool Imu_Integration::reset(){
     return true;
 }
 bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs){
-    // Adding IMU factor and GPS factor and optimizing.
+    // Adding IMU factor and and pre-integration.
     auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+    gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr.get(), *imu_biasPtr.get());
     ++imu_factor_id;
+    valuesPtr->insert(P(imu_factor_id), Estimate_state.pose());
+    valuesPtr->insert(V(imu_factor_id), Estimate_state.v());
+    valuesPtr->insert(B(imu_factor_id), *imu_biasPtr.get());
     gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
                                  preint_imu_combined);
-    graphPtr->add(imu_factor);
-    if(!reset())return false;
+    graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
+    for(auto&& f:enumerate(new_frame->fts_)){
+       if(f.item->point==NULL)continue;
+        valuesPtr->insert(F(f.index), new_frame->T_f_w_ * f.item->point->pos_);
+        graphPtr->emplace_shared<gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> >(
+                f.item->px, ProjectNoisePtr, P( imu_factor_id), F(f.index), cameraPtr);
+    }
+
+    // Now optimize.
+    gtsam::LevenbergMarquardtParams params;
+    params.setVerbosityLM("SUMMARY");
+    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr.get(), *valuesPtr.get(), params);
+    gtsam::Values result = optimizer.optimize();
+    // Overwrite the beginning of the preintegration for the next step.
+    statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
+    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(B(imu_factor_id)));
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr.get());
+    t_1=1e-18;
+    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+   // new_frame->Cov_=gtsam::
+    //if(!reset())return false;
     return true;
 }