debuging based on px30 -> still in progress

svo/include/svo/imu_integration.h

@@ -100,7 +100,7 @@ public:
     Imu_Integration(Sophus::SE3& SE_init);
     ~Imu_Integration();
     bool reset(gtsam::Values& result);
-    bool update(float* imu= nullptr);
+    bool update(double* imu= nullptr);
     bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
 private:
     std::shared_ptr<gtsam::NavState> statePtr;
@@ -112,7 +112,7 @@ private:
     boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
     std::chrono::steady_clock::time_point t_1;
     std::uint32_t imu_factor_id=0;
-    double imu_mean=0.0;
+    std::uint64_t imu_stamp=0.0;
     bool syn=false;
 };
 #endif //SVO_IMU_INTEGRATION_H

svo/src/imu_integration.cpp

@@ -8,47 +8,45 @@
 #include "svo/feature.h"
 #include "svo/point.h"
 #include "svo/svoVisionFactor.h"
+#include <gtsam/nonlinear/Marginals.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()),
                                                gtsam::Vector3(0.0,0.0,0.0));
-    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>();
+    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
     valuesPtr=std::make_shared<gtsam::Values>();
     valuesPtr->insert(P(0),statePtr->pose());
     valuesPtr->insert(V(0),statePtr->v());
     valuesPtr->insert(B(0),*imu_biasPtr);
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
-    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(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.01, 0.01, 0.01).finished()));
-    graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 0.1));
+    graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 0.01));
-    graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-3));
+    graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-4));
     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->accelerometerCovariance=gtsam::I_3x3 * 1e-3; // acc white noise in continuous
-    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * pow(0.00020568902491,2); // gyro white noise in continuous
+    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * 1e-3; // gyro white noise in continuous
     parameterPtr->integrationCovariance=gtsam::I_3x3 * 1e-8; // integration uncertainty continuous
-    parameterPtr->biasAccCovariance = gtsam::I_3x3 * pow(0.004905, 2);;   // acc bias in continuous
+    parameterPtr->biasAccCovariance = gtsam::I_3x3 * 1e-8;   // acc bias in continuous
-    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * pow(0.000001454441043, 2);  // gyro bias in continuous
+    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * 1e-8;  // gyro bias in continuous
     parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-5; // error in the bias used for preintegration
     preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
-    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // TODO change the noise used for projection
+    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 1e-8); // TODO change the noise used for projection
     assert(preintegratedPtr);
 }
 Imu_Integration::~Imu_Integration(){
 
 }
-bool Imu_Integration::update(float* imu){
+bool Imu_Integration::update(double* imu){
     if(syn)return false;
-    double m=0.0;
+    if(imu[6]>imu_stamp){
-    for(int i=0;i<6;++i){
-        m+=abs(imu[i]);
-    }
-    if((0.1666666667*m - imu_mean)>1e-6){
-        imu_mean=0.1666666667*m;
         auto in=std::chrono::steady_clock::now();
         if(std::chrono::duration<double>(in-t_1).count()>20.0){
             t_1=in;
+            imu_stamp=imu[6];
         }else{
             preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration<double>(in-t_1).count());
             t_1=in;
+            imu_stamp=imu[6];
         }
         return true;
     }
@@ -60,11 +58,11 @@ bool Imu_Integration::reset(gtsam::Values& result){
     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);
-    graphPtr->resize(0);
+    //graphPtr->resize(0);
-    valuesPtr->erase(P(imu_factor_id-1));
+    //valuesPtr->erase(P(imu_factor_id-1));
-    valuesPtr->erase(V(imu_factor_id-1));
+    //valuesPtr->erase(V(imu_factor_id-1));
-    valuesPtr->erase(B(imu_factor_id-1));
+    //valuesPtr->erase(B(imu_factor_id-1));
-    t_1=std::chrono::steady_clock::now();
+    //t_1=std::chrono::steady_clock::now();
     return true;
 }
 bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs,const double reproj_thresh){
@@ -72,6 +70,8 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
     syn=true;
     auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
     gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr, *imu_biasPtr);
+    std::cout<<"--------------------------------------------------------------------------PreIntegration----------------------------------------------"<<'\n';
+    //preintegratedPtr->print();
     ++imu_factor_id;
     valuesPtr->insert(P(imu_factor_id), Estimate_state.pose());
     valuesPtr->insert(V(imu_factor_id), Estimate_state.v());
@@ -79,28 +79,36 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
     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->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
+    imu_factor.print();
     num_obs=0;
     for(auto&& f:new_frame->fts_){
        if(f->point==NULL)continue;
-        graphPtr->add(svo::VisionFactor(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id)));
+        graphPtr->emplace_shared<svo::VisionFactor>(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id));
         ++num_obs;
     }
     // Now optimize.
+    std::cout<<"--------------------------------------------------------------------------graph----------------------------------------------"<<'\n';
     //graphPtr->print();
-    //valuesPtr->print();
+    std::cout<<"--------------------------------------------------------------------------values----------------------------------------------"<<'\n';
+    valuesPtr->print();
     gtsam::LevenbergMarquardtParams params;
     params.setVerbosityLM("SUMMARY");
     gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr, params);
     gtsam::Values result = optimizer.optimize();
+    gtsam::Marginals marginals(*graphPtr, result);
+    std::cout<<"--------------------------------------------------------------------------marginals----------------------------------------------"<<'\n';
+    //marginals.print();
     reset(result);
     syn=false;
     new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+    new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id));
+    std::cout<<new_frame->Cov_<<'\n';
     double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2();
     for(auto&& f:new_frame->fts_)
     {
         if(f->point == NULL)
             continue;
-        Eigen::Vector2d e = vk::project2d(f->f) - vk::project2d(result.at<gtsam::Pose3>(P(imu_factor_id)) * f->point->pos_);
+        Eigen::Vector2d e = vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_);
         double sqrt_inv_cov = 1.0 / (1<<f->level);
         e *= sqrt_inv_cov;
         if(e.norm() > reproj_thresh_scaled)

vio_svo/launch/Px30.launch

@@ -7,13 +7,13 @@
     <param name="imu_topic" value="/SensorNode/imu" type="str" />
     
     <!-- Camera calibration file -->
-    <rosparam file="$(find vio_svo)/param/camera_pinhole.yaml" />
-
-    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
     <rosparam file="$(find vio_svo)/param/px30.yaml" />
 
+    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+    <rosparam file="$(find vio_svo)/param/vo_accurate.yaml" />
+    
     <!-- Initial camera orientation, make it point downwards -->
-    <param name="init_rx" value="0.00" />
+    <param name="init_rx" value="3.14" />
     <param name="init_ry" value="0.00" />
     <param name="init_rz" value="0.00" />
 

vio_svo/src/vo_node.cpp

@@ -50,8 +50,7 @@ public:
       VoNode();
       ~VoNode();
       void imgCb(const sensor_msgs::ImageConstPtr& msg);
-      //void imuCb(const sensor_msgs::ImuPtr& imu);
+      void imuCb(const sensor_msgs::ImuPtr& imu);
-      void imuCb(const geometry_msgs::PoseStamped& imu);
       void imu();
 };
 
@@ -108,22 +107,15 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
         ROS_ERROR("cv_bridge exception: %s", e.what());
       }
 }
-void VoNode::imuCb(const geometry_msgs::PoseStamped &imu) {
+void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
-    float imu_in[6];
+    double imu_in[7];
-    /*imu_in[0] = imu->angular_velocity.x;
+    imu_in[0] = imu->linear_acceleration.x;
-    imu_in[1] = imu->angular_velocity.y;
+    imu_in[1] = imu->linear_acceleration.y;
-    imu_in[2] = imu->angular_velocity.z;
+    imu_in[2] = imu->linear_acceleration.z;
-    imu_in[3] = imu->linear_acceleration.x;
+    imu_in[3] = imu->angular_velocity.x;
-    imu_in[4] = imu->linear_acceleration.y;
+    imu_in[4] = imu->angular_velocity.y;
-    imu_in[5] = imu->linear_acceleration.z;
+    imu_in[5] = imu->angular_velocity.z;
-    imu_in[6] = imu->header.stamp.sec;
+    imu_in[6] = imu->header.stamp.toNSec();
-     */
-    imu_in[0] = imu.pose.position.z;
-    imu_in[1] = imu.pose.position.x;
-    imu_in[2] = imu.pose.position.y+9.8;
-    imu_in[3] = imu.pose.orientation.z;
-    imu_in[4] = imu.pose.orientation.x;
-    imu_in[5] = imu.pose.orientation.y;
     vo_->imu_integPtr_->update(imu_in);
 }
 void VoNode::imu(){