public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

complete pipline and test with unity_dataset

Browse Source
This commit is contained in:
gfuse
2021-04-15 18:20:08 +02:00
parent f4d1ce4f4f
commit 382c41725b
10 changed files with 223 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/CMakeLists.txt
+14 -14
View File
@@ -134,23 +134,23 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON)
################################################################################
# TESTS
ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
TARGET_LINK_LIBRARIES(test_feature_align svo)
#ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
#TARGET_LINK_LIBRARIES(test_feature_align svo)
ADD_EXECUTABLE(test_pipeline test/test_pipeline.cpp)
TARGET_LINK_LIBRARIES(test_pipeline svo)
#ADD_EXECUTABLE(test_pipeline test/test_pipeline.cpp)
#TARGET_LINK_LIBRARIES(test_pipeline svo)
ADD_EXECUTABLE(test_matcher test/test_matcher.cpp)
TARGET_LINK_LIBRARIES(test_matcher svo)
#ADD_EXECUTABLE(test_matcher test/test_matcher.cpp)
#TARGET_LINK_LIBRARIES(test_matcher svo)
ADD_EXECUTABLE(test_feature_detection test/test_feature_detection.cpp)
TARGET_LINK_LIBRARIES(test_feature_detection svo)
#ADD_EXECUTABLE(test_feature_detection test/test_feature_detection.cpp)
#TARGET_LINK_LIBRARIES(test_feature_detection svo)
ADD_EXECUTABLE(test_depth_filter test/test_depth_filter.cpp)
TARGET_LINK_LIBRARIES(test_depth_filter svo)
#ADD_EXECUTABLE(test_depth_filter test/test_depth_filter.cpp)
#TARGET_LINK_LIBRARIES(test_depth_filter svo)
ADD_EXECUTABLE(test_sparse_img_align test/test_sparse_img_align.cpp)
TARGET_LINK_LIBRARIES(test_sparse_img_align svo)
#ADD_EXECUTABLE(test_sparse_img_align test/test_sparse_img_align.cpp)
#TARGET_LINK_LIBRARIES(test_sparse_img_align svo)
ADD_EXECUTABLE(test_pose_optimizer test/test_pose_optimizer.cpp)
TARGET_LINK_LIBRARIES(test_pose_optimizer svo)
#ADD_EXECUTABLE(test_pose_optimizer test/test_pose_optimizer.cpp)
#TARGET_LINK_LIBRARIES(test_pose_optimizer svo)
svo/include/svo/imu_integration.h
+8 -7
View File
@@ -99,19 +99,20 @@ class Imu_Integration{
public:
Imu_Integration(Sophus::SE3& SE_init);
~Imu_Integration();
bool reset();
bool reset(gtsam::Values& result);
bool update(float* imu= nullptr);
bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&);
bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
private:
std::shared_ptr<gtsam::NavState> statePtr;
std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
std::unique_ptr<gtsam::Values> valuesPtr;
std::unique_ptr<gtsam::NonlinearFactorGraph> graphPtr;
std::shared_ptr<gtsam::Values> valuesPtr;
std::shared_ptr<gtsam::NonlinearFactorGraph> graphPtr;
boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
std::shared_ptr<gtsam::PreintegrationType> preintegratedPtr;
std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> preintegratedPtr;
boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
boost::shared_ptr<gtsam::Cal3_S2> cameraPtr;
std::uint32_t t_1=1e-18;
std::chrono::steady_clock::time_point t_1;
std::uint32_t imu_factor_id=0;
double imu_mean=0.0;
bool syn=false;
};
#endif //SVO_IMU_INTEGRATION_H
svo/include/svo/svoVisionFactor.h
+71
View File
@@ -0,0 +1,71 @@
//
// Created by root on 4/13/21.
//
#ifndef SVO_SVOVISIONFACTOR_H
#define SVO_SVOVISIONFACTOR_H
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose3.h>
#include <vikit/robust_cost.h>
#include <vikit/math_utils.h>
#include <svo/frame.h>
namespace svo {
class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
private:
// measurement information
gtsam::Point3 feature; ///<Unit-bearing vector of the feature.
gtsam::Point3 landmark; ///<Unit-bearing vector of the feature.
boost::optional<gtsam::Point3> body_P_sensor_; ///< TODO The pose of the sensor in the body frame
int level; ///< Image pyramid level where feature was extracted.
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<VisionFactor> shared_ptr;
/**
* Constructor
* @param feature Unit-bearing vector of the feature.
* @param model is the standard deviation
* @param pointKey is the index of the landmark
* @param poseKey is the index of the camera pose
* @param landmark 3D position of the landmark
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param level Image pyramid level where feature was extracted.
*/
VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp,
gtsam::Key poseKey,boost::optional<gtsam::Point3> body_P_sensor = boost::none) :
gtsam::NoiseModelFactor1<gtsam::Pose3>(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){}
virtual ~VisionFactor(){}
/// error function
/// @param p the Estimated pose in Pose3
/// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> Hp = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
gtsam::Matrix26 J;
gtsam::Point3 point(p*landmark);
svo::Frame::jacobian_xyz2uv(point, J);
if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J / (1<<level)).finished();
Vector2d e = vk::project2d(feature) - vk::project2d(point);
e *= 1.0 / (1<<level);
return e;
}
// The second is a 'clone' function that allows the factor to be copied. Under most
// circumstances, the following code that employs the default copy constructor should
// work fine.
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); }
};
} // namespace gtsamexamples
#endif //SVO_SVOVISIONFACTOR_H
svo/src/frame_handler_mono.cpp
+1 -1
View File
@@ -160,7 +160,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// pose optimization subject to change for adding IMU
SVO_START_TIMER("pose_optimizer");
size_t sfba_n_edges_final;
if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final))
if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh()))
return RESULT_FAILURE;
/*
double sfba_thresh, sfba_error_init, sfba_error_final;
svo/src/imu_integration.cpp
+74 -44
View File
@@ -7,19 +7,20 @@
#include <utility>
#include "svo/feature.h"
#include "svo/point.h"
#include "svo/svoVisionFactor.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>();
valuesPtr=std::make_unique<gtsam::Values>();
valuesPtr=std::make_shared<gtsam::Values>();
valuesPtr->insert(P(0),statePtr->pose());
valuesPtr->insert(V(0),statePtr->v());
valuesPtr->insert(B(0),*imu_biasPtr.get());
graphPtr=std::make_unique<gtsam::NonlinearFactorGraph>();
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(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));
graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-3));
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
@@ -27,58 +28,87 @@ Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
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);
preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // TODO change the noise used for projection
assert(preintegratedPtr);
}
Imu_Integration::~Imu_Integration(){
}
bool Imu_Integration::update(float* imu){
if(t_1<1e-4){
preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005);
t_1=imu[6];
}else{
preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),imu[6]-t_1);
t_1=imu[6];
if(syn)return false;
double m=0.0;
for(int i=0;i<6;++i){
m+=abs(imu[i]);
}
return true;
}
bool Imu_Integration::reset(){
preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr.get());
return true;
}
bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs){
// 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->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);
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;
}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;
}
return true;
}
return false;
// Now optimize.
gtsam::LevenbergMarquardtParams params;
params.setVerbosityLM("SUMMARY");
gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr.get(), *valuesPtr.get(), params);
gtsam::Values result = optimizer.optimize();
}
bool Imu_Integration::reset(gtsam::Values& result){
// 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;
preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
graphPtr->resize(0);
valuesPtr->erase(P(imu_factor_id-1));
valuesPtr->erase(V(imu_factor_id-1));
valuesPtr->erase(B(imu_factor_id-1));
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){
// Adding IMU factor and and pre-integration.
syn=true;
auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr, *imu_biasPtr);
++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);
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);
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)));
++num_obs;
}
// Now optimize.
//graphPtr->print();
//valuesPtr->print();
gtsam::LevenbergMarquardtParams params;
params.setVerbosityLM("SUMMARY");
gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr, params);
gtsam::Values result = optimizer.optimize();
reset(result);
syn=false;
new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
// new_frame->Cov_=gtsam::
//if(!reset())return false;
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_);
double sqrt_inv_cov = 1.0 / (1<<f->level);
e *= sqrt_inv_cov;
if(e.norm() > reproj_thresh_scaled)
{
// we don't need to delete a reference in the point since it was not created yet
f->point = NULL;
--num_obs;
}
}
return true;
}