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

replace depth filter with bandle adjustment init

Browse Source
This commit is contained in:
gfuse
2021-09-24 16:50:36 +02:00
parent b9a45107b6
commit 7e7c60f5a1
26 changed files with 1353 additions and 2059 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/frame_handler_log.txt
+1157 -757
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/gtsam/imu_integration.cpp
-131
View File
@@ -1,131 +0,0 @@
//
// Created by gfuse on 4/9/21.
//
//#define NDEBUG
#include "imu_integration.h"
#include <memory>
#include <utility>
#include "vio/feature.h"
#include "vio/point.h"
#include "vio/svoVisionFactor.h"
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose3.h>
#include <vio/params_helper.h>
#include <vio/config.h>
#include <vio/for_it.hpp>
#define debug 0
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
valuesPtr=std::make_shared<gtsam::Values>();
parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(0.0,0.0,0.0)));
parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC(); // acc bias in continuous
parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC(); // gyro bias in continuous
parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP(); // error in the bias used for preintegration
imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection
optimizerParamPtr=std::make_shared<gtsam::ISAM2Params>();
optimizerParamPtr->relinearizeThreshold = 0.1;
optimizerParamPtr->relinearizeSkip = 1;
optimizerParamPtr->evaluateNonlinearError=true;
optimizerParamPtr->enableDetailedResults=true;
if(debug)logtime = fopen((std::string(getenv("HOME"))+"/imu_preIntegrate.txt").c_str(), "w+");
assert(preintegratedPtr);
}
Imu_Integration::~Imu_Integration(){
}
bool Imu_Integration::update(double* imu){
if(syn)return false;
if(imu_factor_id<1)return false;
auto in=std::chrono::steady_clock::now();
if(std::chrono::duration<double>(in-t_1).count()>20.0){
t_1=in;
preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[2]-svo::Config::Teta_off(),0.0),0.0005);
}else{
preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[2]-svo::Config::Teta_off(),0.0),std::chrono::duration<double>(in-t_1).count());
t_1=in;
++imu_n;
}
return true;
}
bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
gtsam::Values result = optimizer.optimize();
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);
++imu_factor_id;
syn=false;
gtsam::Marginals marginals(*graphPtr,result);
if(debug)fprintf(logtime, "%d,%f,%f,%f,%f,\n",
imu_factor_id-1,
statePtr->pose().x(),
statePtr->pose().y(),
statePtr->pose().z(),
statePtr->pose().rotation().pitch());
new_frame->T_f_w_=svo::SE2_5(statePtr->pose().x(),statePtr->pose().z(),statePtr->pose().rotation().pitch());
new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1));
graphPtr->resize(0);
valuesPtr->clear();
valuesPtr->insert(P(imu_factor_id-1),gtsam::Pose3(new_frame->T_f_w_.T->matrix()));
valuesPtr->insert(V(imu_factor_id-1),statePtr->v());
valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr);
graphPtr->addPrior(P(imu_factor_id-1),gtsam::Pose3(new_frame->T_f_w_.T->matrix()),marginals.marginalCovariance(P(imu_factor_id-1)));
graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1)));
graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1)));
return true;
}
bool Imu_Integration::preintegrate_predict(svo::SE2_5& frame) {
if(imu_n<1){
syn= false;
return false;
}else{
syn=true;
}
Integ_predict=preintegratedPtr->predict(*statePtr,*imu_biasPtr);
frame =svo::SE2_5(Integ_predict.pose().x(),Integ_predict.pose().z(),Integ_predict.pose().rotation().pitch());
return true;
}
void Imu_Integration::reset() {
preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
imu_n=0;
syn=false;
}
bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,
boost::shared_ptr<svo::Frame>& last_frame,
const double cell_match){
valuesPtr->insert(P(imu_factor_id), Integ_predict.pose());
valuesPtr->insert(V(imu_factor_id), Integ_predict.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),
*preintegratedPtr);
graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
imu_n=0;
graphPtr->emplace_shared<svo::VisionFactor>(gtsam::Pose3(new_frame->T_f_w_.T->matrix()),
gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(3) << 1.0, 1.0, 1.0).finished()/(cell_match+1)),
P(imu_factor_id));
gtsam::LevenbergMarquardtParams params0;
params0.verbosityLM = gtsam::LevenbergMarquardtParams::SILENT;
params0.setlambdaFactor(10.0);
params0.setlambdaInitial(100);
params0.setlambdaUpperBound(1e10);
gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr,params0);
reset(optimizer,new_frame);
return true;
}
bool Imu_Integration::init(boost::shared_ptr<svo::Frame>& new_frame){
statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.T->matrix()),
gtsam::Vector3(1e-9,1e-9,1e-9));
valuesPtr->insert(P(0),statePtr->pose());
valuesPtr->insert(V(0),statePtr->v());
valuesPtr->insert(B(0),*imu_biasPtr);
graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-5, 1e-15, 1e-5, 1e-15, 1e-8, 1e-15).finished()));
graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2));
++imu_factor_id;
return true;
}
GPU_version/vio/gtsam/imu_integration.h
-50
View File
@@ -1,50 +0,0 @@
//
// Created by gufe on 4/9/21.
//
#ifndef SVO_IMU_INTEGRATION_H
#define SVO_IMU_INTEGRATION_H
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <sophus/se3.h>
#include <vio/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::L; // Landmark
class Imu_Integration{
public:
Imu_Integration(Sophus::SE3& SE_init);
~Imu_Integration();
void reset();
bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
bool update(double* imu= nullptr);
bool predict(boost::shared_ptr<svo::Frame>&,
boost::shared_ptr<svo::Frame>& last_frame ,const double match);
bool init(boost::shared_ptr<svo::Frame>&);
bool preintegrate_predict(svo::SE2_5& frame);
private:
FILE* logtime;
std::shared_ptr<gtsam::NavState> statePtr;
std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
std::shared_ptr<gtsam::Values> valuesPtr;
std::shared_ptr<gtsam::NonlinearFactorGraph> graphPtr;
boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> preintegratedPtr;
boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
std::chrono::steady_clock::time_point t_1;
std::uint32_t imu_factor_id=0;
uint imu_n=0;
std::shared_ptr<gtsam::ISAM2Params> optimizerParamPtr;
gtsam::NavState Integ_predict;
bool syn=false;
};
#endif //SVO_IMU_INTEGRATION_H
GPU_version/vio/gtsam/svoVisionFactor.h
-64
View File
@@ -1,64 +0,0 @@
//
// 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 <vio/robust_cost.h>
#include <vio/math_utils.h>
#include <vio/frame.h>
namespace vio {
class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
private:
// measurement information
gtsam::Pose3 pose; ///<Estimated pose in svo after GaussNewton optimization.
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<VisionFactor> shared_ptr;
/**
* Constructor
* @param pose Camera pose.
* @param pointKey is the index of the landmark
*/
VisionFactor(const gtsam::Pose3& pose,
const gtsam::SharedNoiseModel& H,
gtsam::Key poseKey) :
gtsam::NoiseModelFactor1<gtsam::Pose3>(H, poseKey), pose(pose){}
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&> H1 = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
Eigen::Matrix<double,3,3> E_r=0.5*(p.rotation().matrix().transpose()*pose.rotation().matrix()-pose.rotation().matrix().transpose()*p.rotation().matrix());
Vector3d e_t=p.translation()-pose.translation();
if(H1)(*H1) = (gtsam::Matrix(3,6)<< 1,0,0,0,0,0,
0,0,1,0,0,0,
0,0,0,0,1,0).finished();
return (gtsam::Matrix(3,1)<<e_t.x(),e_t.z(),-E_r(0,2)).finished();
}
// 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
GPU_version/vio/gtsam/svoVisionFactorLandMark.h
-87
View File
@@ -1,87 +0,0 @@
//
// Created by root on 4/13/21.
//
#ifndef SVO_VisionFactorLandMark_H
#define SVO_VisionFactorLandMark_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 <vio/frame.h>
namespace vio {
class VisionFactorLandMark: public gtsam::NoiseModelFactor3<gtsam::Pose3,gtsam::Pose3,gtsam::Point3> {
private:
// measurement information
gtsam::Point3 feature_new; ///<Unit-bearing vector of the feature.
gtsam::Point3 feature_ref; ///<Unit-bearing vector of the feature.
int level;
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<VisionFactorLandMark> 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.
*/
VisionFactorLandMark(const gtsam::Point3& feature_new,const gtsam::Point3& feature_ref, int level,
const gtsam::SharedNoiseModel& H,
gtsam::Key poseKey_ref, gtsam::Key poseKey_new,gtsam::Key landmarkKey ) :
gtsam::NoiseModelFactor3<gtsam::Pose3,gtsam::Pose3,gtsam::Point3>(H, poseKey_ref,poseKey_new,landmarkKey), feature_new(feature_new),
feature_ref(feature_ref),level(level){}
virtual ~VisionFactorLandMark(){}
/// error function
/// @param p_new the Estimated pose in Pose3
/// @param P_ref the Estimated point of landmark
/// @param l the Estimated point of landmark
/// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose3& p_ref,const gtsam::Pose3& p_new, const gtsam::Point3& l,
boost::optional<gtsam::Matrix&> H1 = boost::none,boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
gtsam::Matrix26 J,J1;
gtsam::Matrix23 J2,J3;
gtsam::Point3 point(p_ref*l);
svo::Frame::jacobian_xyz2uv(point, J);
svo::Frame::jacobian_l2uv(point,J2);
gtsam::Point3 point2(p_new*l);
svo::Frame::jacobian_xyz2uv(point2, J1);
svo::Frame::jacobian_l2uv(point2,J3);
Vector2d e1=vk::project2d(feature_ref) - vk::project2d(point)*1.0 / (1<<level);
Vector2d e2=vk::project2d(feature_new) - vk::project2d(point2)*1.0 / (1<<level);
Vector4d e=(gtsam::Matrix(4,1)<< e1,e2).finished();
//e*=1.0 / (1<<level);
if(H1)(*H1) = (gtsam::Matrix(4,6)<< J,gtsam::Matrix(2,6).setZero()).finished()*1.0 / (1<<level);
if(H2)(*H2) = (gtsam::Matrix(4,6)<< gtsam::Matrix(2,6).setZero(),J1).finished()*1.0 / (1<<level);
if(H3)(*H3) = (gtsam::Matrix(4,3)<< J2,J3).finished()*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 VisionFactorLandMark(*this))); }
};
} // namespace gtsamexamples
#endif //SVO_SVOVISIONFACTOR_H
GPU_version/vio/gtsam/svoVisionFactorSmall.h
-70
View File
@@ -1,70 +0,0 @@
//
// 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 <vio/frame.h>
namespace vio {
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);
Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J * 1.0 / (1<<level)).finished();
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
GPU_version/vio/include/vio/depth_filter.h
-172
View File
@@ -1,172 +0,0 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef SVO_DEPTH_FILTER_H_
#define SVO_DEPTH_FILTER_H_
#include <queue>
#include <boost/thread.hpp>
#include <boost/function.hpp>
#include <vio/global.h>
#include <vio/feature_detection.h>
#include <vio/matcher.h>
namespace vio {
class Frame;
class Feature;
class Point;
/// A seed is a probabilistic depth estimate for a single pixel.
struct Seed
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static int batch_counter;
static int seed_counter;
int batch_id; //!< Batch id is the id of the keyframe for which the seed was created.
int id; //!< Seed ID, only used for visualization.
std::shared_ptr<Feature> ftr; //!< Feature in the keyframe for which the depth should be computed.
float a; //!< a of Beta distribution: When high, probability of inlier is large.
float b; //!< b of Beta distribution: When high, probability of outlier is large.
float mu; //!< Mean of normal distribution.
float z_range; //!< Max range of the possible depth.
float sigma2; //!< Variance of normal distribution.
Matrix2d patch_cov; //!< Patch covariance in reference image.
Seed(std::shared_ptr<Feature> ftr, float depth_mean, float depth_min);
};
/// Depth filter implements the Bayesian Update proposed in:
/// "Video-based, Real-Time Multi View Stereo" by G. Vogiatzis and C. Hernández.
/// In Image and Vision Computing, 29(7):434-441, 2011.
///
/// The class uses a callback mechanism such that it can be used also by other
/// algorithms than nslam and for simplified testing.
class DepthFilter
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef boost::unique_lock<boost::mutex> lock_t;
typedef boost::function<void ( std::shared_ptr<Point>, double )> callback_t;
/// Depth-filter config parameters
struct Options
{
bool check_ftr_angle; //!< gradient features are only updated if the epipolar line is orthogonal to the gradient.
bool epi_search_1d; //!< restrict Gauss Newton in the epipolar search to the epipolar line.
bool verbose; //!< display output.
bool use_photometric_disparity_error; //!< use photometric disparity error instead of 1px error in tau computation.
int max_n_kfs; //!< maximum number of keyframes for which we maintain seeds.
double sigma_i_sq; //!< image noise.
double seed_convergence_sigma2_thresh; //!< threshold on depth uncertainty for convergence.
Options()
: check_ftr_angle(false),
epi_search_1d(false),
verbose(false),
use_photometric_disparity_error(false),
max_n_kfs(10),
sigma_i_sq(5e-4),
seed_convergence_sigma2_thresh(200.0)
{}
} options_;
DepthFilter(
feature_detection::DetectorPtr feature_detector,
callback_t seed_converged_cb);
virtual ~DepthFilter();
/// Start this thread when seed updating should be in a parallel thread.
void startThread();
/// Stop the parallel thread that is running.
void stopThread();
/// Add frame to the queue to be processed.
void addFrame(FramePtr frame);
/// Add new keyframe to the queue
void addKeyframe(FramePtr frame, double depth_mean, double depth_min);
/// Remove all seeds which are initialized from the specified keyframe. This
/// function is used to make sure that no seeds points to a non-existent frame
/// when a frame is removed from the map.
void removeKeyframe(FramePtr frame);
/// If the map is reset, call this function such that we don't have pointers
/// to old frames.
void reset();
/// Returns a copy of the seeds belonging to frame. Thread-safe.
/// Can be used to compute the Next-Best-View in parallel.
/// IMPORTANT! Make sure you hold a valid reference counting pointer to frame
/// so it is not being deleted while you use it.
void getSeedsCopy(const FramePtr& frame, std::list<std::shared_ptr<Seed>>& seeds);
/// Return a reference to the seeds. This is NOT THREAD SAFE!
//std::list<Seed>& getSeeds() { return seeds_; } Bug 238
std::list<std::shared_ptr<Seed>, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
/// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
static bool updateSeed(
const float x,
const float tau2,
std::shared_ptr<Seed> seed);
/// Compute the uncertainty of the measurement.
static double computeTau(
const SE3& T_ref_cur,
const Vector3d& f,
const double z,
const double px_error_angle);
protected:
feature_detection::DetectorPtr feature_detector_;
callback_t seed_converged_cb_;
std::list<std::shared_ptr<Seed>, Eigen::aligned_allocator<Seed>> seeds_;
boost::mutex seeds_mut_;
bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted.
boost::thread* thread_;
std::queue<FramePtr> frame_queue_;
boost::mutex frame_queue_mut_;
boost::condition_variable frame_queue_cond_;
FramePtr new_keyframe_; //!< Next keyframe to extract new seeds.
bool new_keyframe_set_; //!< Do we have a new keyframe to process?.
double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds.
double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds.
Matcher matcher_;
#if VIO_DEBUG
FILE* log_=nullptr;
#endif
//uint seeds_cont_=0;
/// Initialize new seeds from a frame.
void initializeSeeds(FramePtr frame);
/// Update all seeds with a new measurement frame.
virtual void updateSeeds(FramePtr frame);
/// When a new keyframe arrives, the frame queue should be cleared.
void clearFrameQueue();
/// A thread that is continuously updating the seeds.
void updateSeedsLoop();
};
} // namespace vio
#endif // SVO_DEPTH_FILTER_H_
GPU_version/vio/include/vio/frame.h
+2 -18
View File
@@ -93,7 +93,7 @@ namespace vio {
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2c(const Vector3d& xyz_w) const {
return cam_->world2cam( T_f_w_.se3()*xyz_w);
return cam_->world2cam( T_f_w_.se3().inverse()*xyz_w);
}
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); }
@@ -106,13 +106,9 @@ namespace vio {
/// Transforms point coordinates in world-frame (w) to camera-frams (f).
inline Vector3d w2f(const Vector3d& xyz_w) const {
return Vector3d(T_f_w_.se3()*xyz_w);
return Vector3d(T_f_w_.se3().inverse()*xyz_w);
}
/// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
inline Vector3d f2w(const Vector3d& f) const {
return getSE3Inv() * f;
}
/// Projects Point from unit sphere (f) in camera pixels (c).
inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
@@ -128,18 +124,6 @@ namespace vio {
return T_f_w_.se3();
}
inline SE3 getSE3Inv() const{
//TODO add 15 degree roll orientation
SE2 inv=T_f_w_.inverse();
Quaterniond q;
q = AngleAxisd(-0.122173, Vector3d::UnitX())
* AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1)));
return out;
}
/// Frame jacobian for projection of 3D point in (f)rame coordinate to
/// unit plane coordinates uv (focal length = 1).
GPU_version/vio/include/vio/frame_handler_base.h
-12
View File
@@ -32,9 +32,6 @@ class AbstractCamera;
namespace vio
{
class Point;
class Matcher;
class DepthFilter;
/// Base class for various VO pipelines. Manages the map and the state machine.
class FrameHandlerBase : boost::noncopyable
@@ -74,14 +71,8 @@ public:
/// Get the current stage of the algorithm.
Stage stage() const { return stage_; }
/// Get tracking quality.
TrackingQuality trackingQuality() const { return tracking_quality_; }
/// Get the processing time of the previous iteration.
//double lastProcessingTime() const { return timer_.getTime(); }
/// Get the number of feature observations of the last frame.
// size_t lastNumObservations() const { return num_obs_last_; }
protected:
Stage stage_; //!< Current stage of the algorithm.
@@ -89,7 +80,6 @@ protected:
bool set_start_; //!< Flag the user can set to start the system when the next image is received.
Map map_; //!< Map of keyframes created by the slam system.
size_t num_obs_last_; //!< Number of observations in the previous frame.
TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features.
/// Before a frame is processed, this function is called.
bool startFrameProcessingCommon(const double timestamp);
@@ -106,8 +96,6 @@ protected:
/// Reset the frame handler. Implement in derived class.
virtual void resetAll() { resetCommon(); }
/// Set the tracking quality based on the number of tracked features.
virtual void setTrackingQuality(const size_t num_observations);
/// Optimize some of the observed 3D points.
virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
GPU_version/vio/include/vio/frame_handler_mono.h
+3 -2
View File
@@ -25,6 +25,7 @@
#include <vio/ukf.h>
#include <vio/cl_class.h>
#include <vio/sparse_img_align_gpu.h>
#include <vio/global_optimizer.h>
namespace vio {
@@ -42,7 +43,7 @@ public:
/// Access the depth filter.
DepthFilter* depthFilter() const{ return depth_filter_; }
BA_Glob* depthFilter() const{ return ba_glob_; }
void UpdateIMU(double* value,const ros::Time& time);
@@ -58,7 +59,7 @@ protected:
FramePtr last_frame_; //!< Last frame, not necessarily a keyframe.
vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
initialization::KltHomographyInit* klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
BA_Glob* ba_glob_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
opencl* gpu_fast_;
ros::Time time_;
GPU_version/vio/include/vio/global_optimizer.h
+68
View File
@@ -0,0 +1,68 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef SVO_DEPTH_FILTER_H_
#define SVO_DEPTH_FILTER_H_
#include <queue>
#include <boost/thread.hpp>
#include <boost/function.hpp>
#include <vio/global.h>
#include <vio/feature_detection.h>
#include <vio/matcher.h>
#include <vio/map.h>
namespace vio {
class BA_Glob
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef boost::unique_lock<boost::mutex> lock_;
BA_Glob(Map& map);
virtual ~BA_Glob();
/// Start this thread when seed updating should be in a parallel thread.
void startThread();
/// Stop the parallel thread that is running.
void stopThread();
/// If the map is reset, call this function such that we don't have pointers
/// to old frames.
void reset();
protected:
bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted.
boost::thread* thread_;
Map& map_;
#if VIO_DEBUG
FILE* log_=nullptr;
#endif
/// A thread that is continuously optimizing the map.
void updateLoop();
};
} // namespace vio
#endif // SVO_DEPTH_FILTER_H_
GPU_version/vio/include/vio/initialization.h
+2 -2
View File
@@ -84,12 +84,12 @@ protected:
vector<int> outliers_hom, inlier_hom;
vector<int> outliers_ekf,inlier_ekf;
assert(ukf_!= nullptr);
/*#if VIO_DEBUG
#if VIO_DEBUG
for(int i=0;i<f_cur.size();++i)fprintf(log_,"[%s] cur: x=%f, y=%f, z=%f ref: x=%f y=%f z=%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),
f_cur.at(i).x(),f_cur.at(i).y(),f_cur.at(i).z(),
f_ref.at(i).x(),f_ref.at(i).y(),f_ref.at(i).z());
#endif*/
#endif
vk::computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
reprojection_threshold, focal_length,
GPU_version/vio/include/vio/map.h
+1 -57
View File
@@ -21,60 +21,16 @@
#include <boost/noncopyable.hpp>
#include <boost/thread.hpp>
#include <vio/global.h>
#include <vio/point.h>
namespace vio {
class Point;
class Feature;
class Seed;
/// Container for converged 3D points that are not already assigned to two keyframes.
class MapPointCandidates
{
public:
typedef pair<std::shared_ptr<Point>, std::shared_ptr<Feature>> PointCandidate;
typedef list<PointCandidate> PointCandidateList;
/// The depth-filter is running in a parallel thread and fills the canidate list.
/// This mutex controls concurrent access to point_candidates.
boost::mutex mut_;
/// Candidate points are created from converged seeds.
/// Until the next keyframe, these points can be used for reprojection and pose optimization.
PointCandidateList candidates_;
list< std::shared_ptr<Point> > trash_points_;
MapPointCandidates();
~MapPointCandidates();
/// Add a candidate point.
void newCandidatePoint(std::shared_ptr<Point> point, double depth_sigma2);
/// Adds the feature to the frame and deletes candidate from list.
void addCandidatePointToFrame(FramePtr frame);
/// Remove a candidate point from the list of candidates.
bool deleteCandidatePoint(std::shared_ptr<Point> point);
/// Remove all candidates that belong to a frame.
void removeFrameCandidates(FramePtr frame);
/// Reset the candidate list, remove and delete all points.
void reset();
void deleteCandidate(PointCandidate& c);
void emptyTrash();
};
static bool syn_=false;
/// Map object which saves all keyframes which are in a map.
class Map : boost::noncopyable
{
public:
list< FramePtr > keyframes_; //!< List of keyframes in the map.
list< std::shared_ptr<Point> > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
MapPointCandidates point_candidates_;
Map();
~Map();
@@ -100,24 +56,14 @@ public:
/// Given a frame, return all keyframes which have an overlapping field of view.
void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const;
/// Return the keyframe which is spatially closest and has overlapping field of view.
FramePtr getClosestKeyframe(const FramePtr& frame) const;
/// Return the keyframe which is furthest apart from pos.
FramePtr getFurthestKeyframe(const Vector2d& pos) const;
bool getKeyframeById(const int id, FramePtr& frame) const;
/// Transform the whole map with rotation R, translation t and scale s.
void transform(const Matrix2d& R, const Vector2d& t, const double& s);
/// Empty trash bin of deleted keyframes and map points. We don't delete the
/// points immediately to ensure proper cleanup and to provide the visualizer
/// a list of objects which must be removed.
void emptyTrash();
/// Return the keyframe which was last inserted in the map.
inline FramePtr lastKeyframe() { return keyframes_.back(); }
/// Return the number of keyframes in the map
inline size_t size() const { return keyframes_.size(); }
@@ -126,8 +72,6 @@ public:
/// A collection of debug functions to check the data consistency.
namespace map_debug {
void mapStatistics(Map* map);
void mapValidation(Map* map, int id);
void frameValidation(FramePtr frame, int id);
void pointValidation(std::shared_ptr<Point> point, int id);
GPU_version/vio/include/vio/reprojector.h
+2 -7
View File
@@ -23,6 +23,8 @@
#include <vio/cl_class.h>
#include <vio/initialization.h>
#include <vio/vision.h>
#include <vio/map.h>
#include <vio/point.h>
namespace vk {
class AbstractCamera;
@@ -30,9 +32,6 @@ class AbstractCamera;
namespace vio {
class Map;
class Point;
/// Project points from the map into the image and find the corresponding
/// feature (corner). We don't search a match for every point but only for one
/// point per cell. Thereby, we achieve a homogeneously distributed set of
@@ -62,9 +61,6 @@ public:
/// Project points from the map into the image. First finds keyframes with
/// overlapping field of view and projects only those map-points.
void reprojectMap(
FramePtr frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs);
void reprojectMap2(
FramePtr frame,
@@ -104,7 +100,6 @@ private:
void initializeGrid(vk::AbstractCamera* cam);
void resetGrid();
bool reprojectCell(Cell& cell, FramePtr frame);
bool reprojectPoint(FramePtr frame, std::shared_ptr<Point> point);
};
} // namespace vio
GPU_version/vio/src/depth_filter.cpp
-386
View File
@@ -1,386 +0,0 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <vio/math_utils.h>
#include <vio/abstract_camera.h>
#include <vio/vision.h>
#include <boost/bind.hpp>
#include <boost/math/distributions/normal.hpp>
#include <vio/global.h>
#include <vio/depth_filter.h>
#include <vio/frame.h>
#include <vio/point.h>
#include <vio/feature.h>
#include <vio/matcher.h>
#include <vio/config.h>
#include <vio/feature_detection.h>
#if VIO_DEBUG
#include <sys/types.h>
#include <sys/stat.h>
#endif
namespace vio {
int Seed::batch_counter = 0;
int Seed::seed_counter = 0;
Seed::Seed(std::shared_ptr<Feature> ftr, float depth_mean, float depth_min) :
batch_id(batch_counter),
id(seed_counter++),
ftr(ftr),
a(10),
b(10),
mu(1.0/depth_mean),
z_range(1.0/depth_min),
sigma2(z_range*z_range/36)
{}
DepthFilter::DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb) :
feature_detector_(feature_detector),
seed_converged_cb_(seed_converged_cb),
seeds_updating_halt_(false),
thread_(NULL),
new_keyframe_set_(false),
new_keyframe_min_depth_(0.0),
new_keyframe_mean_depth_(0.0)
{
#if VIO_DEBUG
log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+");
assert(log_);
chmod((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(), ACCESSPERMS);
#endif
}
DepthFilter::~DepthFilter()
{
stopThread();
}
void DepthFilter::startThread()
{
thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this);
}
void DepthFilter::stopThread()
{
if(thread_ != NULL)
{
seeds_updating_halt_ = true;
thread_->interrupt();
usleep(5000);
thread_->join();
thread_ = NULL;
}
#if VIO_DEBUG
fclose(log_);
#endif
}
void DepthFilter::addFrame(FramePtr frame)
{
#if VIO_DEBUG
fprintf(log_,"[%s] add new frame\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
if(thread_ != NULL)
{
{
if(frame_queue_.empty())lock_t lock(frame_queue_mut_);
if(frame_queue_.size() > 10)
frame_queue_.pop();
frame_queue_.push(frame);
}
seeds_updating_halt_ = false;
frame_queue_cond_.notify_one();
}else{
updateSeeds(frame);
}
}
void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_min)
{
new_keyframe_min_depth_ = depth_min;
new_keyframe_mean_depth_ = depth_mean;
#if VIO_DEBUG
fprintf(log_,"[%s] add key frame\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
if(!seeds_.empty())
{
new_keyframe_ = frame;
new_keyframe_set_ = true;
seeds_updating_halt_ = true;
frame_queue_cond_.notify_one();
}else{
new_keyframe_ = frame;
new_keyframe_set_ = true;
seeds_updating_halt_ = true;
frame_queue_cond_.notify_one();
initializeSeeds(frame);
}
}
void DepthFilter::initializeSeeds(FramePtr frame)
{
Features new_features;
#if VIO_DEBUG
fprintf(log_,"[%s] init seeds\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
feature_detector_->setExistingFeatures(frame->fts_);
feature_detection_mut_.lock();
feature_detector_->detect(frame, frame->img_pyr_,
Config::triangMinCornerScore(), new_features);
feature_detection_mut_.unlock();
// initialize a seed for every new feature
seeds_updating_halt_ = true;
seeds_mut_.lock();
++Seed::batch_counter;
for(auto&& ftr:new_features)seeds_.push_back(make_shared<Seed>(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
#if VIO_DEBUG
fprintf(log_,"[%s] DepthFilter: Initialized %d new seeds\n",vio::time_in_HH_MM_SS_MMM().c_str(),new_features.size());
#endif
seeds_updating_halt_ = false;
seeds_mut_.unlock();
}
void DepthFilter::removeKeyframe(FramePtr frame)
{
seeds_updating_halt_ = true;
#if VIO_DEBUG
fprintf(log_,"[%s] remove key frame\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
seeds_mut_.lock();
std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin();
size_t n_removed = 0;
while(it!=seeds_.end())
{
if((*it)->ftr->frame->id_ == frame->id_)
{
it = seeds_.erase(it);
++n_removed;
}
else
++it;
}
seeds_updating_halt_ = false;
seeds_mut_.unlock();
}
void DepthFilter::reset()
{
seeds_updating_halt_ = true;
{
seeds_mut_.lock();
seeds_.clear();
seeds_mut_.unlock();
}
while(!frame_queue_.empty())
frame_queue_.pop();
seeds_updating_halt_ = false;
#if VIO_DEBUG
fprintf(log_,"[%s] DepthFilter: RESET.\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
}
void DepthFilter::updateSeedsLoop()
{
while(!boost::this_thread::interruption_requested())
{
FramePtr frame;
{
lock_t lock(frame_queue_mut_);
while(frame_queue_.empty() && new_keyframe_set_ == false)
frame_queue_cond_.wait(lock);
lock.unlock();
if(new_keyframe_set_)
{
new_keyframe_set_ = false;
clearFrameQueue();
frame = new_keyframe_;
}
else
{
frame = frame_queue_.front();
frame_queue_.pop();
}
}
updateSeeds(frame);
if(frame->isKeyframe())
initializeSeeds(frame);
}
}
void DepthFilter::updateSeeds(FramePtr frame)
{
// update only a limited number of seeds, because we don't have time to do it
// for all the seeds in every frame!
size_t n_updates=0, n_failed_matches=0;
lock_t lock(seeds_mut_);
std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin();
#if VIO_DEBUG
fprintf(log_,"[%s] update %d seeds\n",vio::time_in_HH_MM_SS_MMM().c_str(),seeds_.size());
#endif
return;
const double focal_length = frame->cam_->errorMultiplier2();
double px_noise = 1.0;
double px_error_angle = atan2(px_noise,(2.0*focal_length))*2.0; // law of chord (sehnensatz)
while( it!=seeds_.end())
{
// set this value true when seeds updating should be interrupted
if((*it)->ftr->frame == NULL){
it = seeds_.erase(it);
continue;
}
// check if point is visible in the current image
//SE2 T(frame->T_f_w_.so2()*(*it)->ftr->frame->T_f_w_.inverse().so2(),frame->T_f_w_.se2().translation()+(*it)->ftr->frame->T_f_w_.inverse().translation());
SE2 T((*it)->ftr->frame->T_f_w_.so2()*frame->T_f_w_.inverse().so2(),frame->T_f_w_.inverse().translation()+(*it)->ftr->frame->T_f_w_.translation());
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(-0.122173, Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/(*it)->mu * (*it)->ftr->f) );
#if VIO_DEBUG
fprintf(log_,"[%s] If point is visible? %f, %f, %f mu:%f unit_bearing:%f %f %f T_ref_cur: x=%f y=%f z=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
xyz_f.x(),xyz_f.y(),xyz_f.z(),(*it)->mu,(*it)->ftr->f.x(),(*it)->ftr->f.y(),(*it)->ftr->f.z(),
T_ref_cur.translation().x(),T_ref_cur.translation().y(),T_ref_cur.translation().z());
#endif
if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {
++it; // point does not project in image
continue;
}
float z_min = (*it)->mu + sqrt((*it)->sigma2);
float z_max = max((*it)->mu - sqrt((*it)->sigma2), 0.00000001f);
#if VIO_DEBUG
fprintf(log_,"[%s] Z inverse min: %f, Z inverse max: %f sigma2:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max,(*it)->sigma2);
#endif
double z;
if(!matcher_.findEpipolarMatchDirect(
*(*it)->ftr->frame, *frame, *(*it)->ftr, 1.0/(*it)->mu, 1.0/z_min, 1.0/z_max, z,log_))
{
(*it)->b++; // increase outlier probability when no match was found
++it;
++n_failed_matches;
continue;
}
double tau = computeTau(T_ref_cur, (*it)->ftr->f, z, px_error_angle);
double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
#if VIO_DEBUG
fprintf(log_,"[%s] calculated Z= %f tau: %f tau_inv: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z,
tau,tau_inverse);
#endif
// update the estimate
if(!updateSeed(1.0/z, tau_inverse*tau_inverse, *it)){
++it;
++n_failed_matches;
continue;
}
++n_updates;
if(frame->isKeyframe())
{
// The feature detector should not initialize new seeds close to this location
feature_detector_->setGridOccpuancy(matcher_.px_cur_);
}
// if the seed has converged, we initialize a new candidate point and remove the seed
if(sqrt((*it)->sigma2) < (*it)->z_range/options_.seed_convergence_sigma2_thresh)
{
assert((*it)->ftr->point == NULL); // TODO this should not happen anymore
Vector3d xyz_world((*it)->ftr->frame->se3() * ((*it)->ftr->f/(*it)->mu));
std::shared_ptr<Point> point = std::make_shared<Point>(xyz_world, (*it)->ftr);
(*it)->ftr->point = point;
seed_converged_cb_(point, (*it)->sigma2); // put in candidate list
it = seeds_.erase(it);
}
else if(isnan(z_min))
{
it = seeds_.erase(it);
}
else
++it;
#if VIO_DEBUG
fprintf(log_,"[%s] seeds updated\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
}
}
void DepthFilter::clearFrameQueue()
{
while(!frame_queue_.empty())
frame_queue_.pop();
}
void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<std::shared_ptr<Seed>>& seeds)
{
lock_t lock(seeds_mut_);
for(std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
{
if ((*it)->ftr->frame->id_ == frame->id_)
seeds.push_back(*it);
}
}
bool DepthFilter::updateSeed(const float x, const float tau2, std::shared_ptr<Seed> seed)
{
float norm_scale = sqrt(seed->sigma2 + tau2);
if(std::isnan(norm_scale))
return false;
boost::math::normal_distribution<float> nd(seed->mu, norm_scale);
float s2 = 1./(1./seed->sigma2 + 1./tau2);
float m = s2*(seed->mu/seed->sigma2 + x/tau2);
float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);
float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;
float normalization_constant = C1 + C2;
C1 /= normalization_constant;
C2 /= normalization_constant;
float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.);
float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.))
+ C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f));
// update parameters
float mu_new = C1*m+C2*seed->mu;
seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new;
seed->mu = mu_new;
seed->a = (e-f)/(f-e/f);
seed->b = seed->a*(1.0f-f)/f;
return true;
}
double DepthFilter::computeTau(
const SE3& T_ref_cur,
const Vector3d& f,
const double z,
const double px_error_angle)
{
Vector3d t(T_ref_cur.translation());
Vector3d a = f*z-t;
double t_norm = t.norm();
double beta_plus = acos(a.dot(-t)/(t_norm*a.norm())) + px_error_angle;
return (t_norm*sin(beta_plus)/sin(PI-acos(f.dot(t)/t_norm)-beta_plus) - z); // ( triangle angles sum to PI and law of sines ) tau
}
} // namespace vio
GPU_version/vio/src/frame.cpp
+1 -3
View File
@@ -144,10 +144,8 @@ void Frame::removeKeyPoint(std::shared_ptr<Feature> ftr)
bool Frame::isVisible(const Vector3d& xyz_w) const
{
if(!id_)return false;
usleep(1000);
SE3 tem=this->se3();
assert(!xyz_w.hasNaN());
Vector3d xyz_f = tem*xyz_w;
Vector3d xyz_f = this->se3().inverse()*xyz_w;
if(xyz_f.z() < 0.0)
return false; // point is behind the camera
Vector2d px = f2c(xyz_f);
GPU_version/vio/src/frame_handler_base.cpp
+1 -21
View File
@@ -35,8 +35,7 @@ FrameHandlerBase::FrameHandlerBase() :
stage_(STAGE_PAUSED),
set_reset_(false),
set_start_(false),
num_obs_last_(0),
tracking_quality_(TRACKING_INSUFFICIENT)
num_obs_last_(0)
{
}
@@ -55,7 +54,6 @@ bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
if(stage_ == STAGE_PAUSED)
return false;
// some cleanup from last iteration, can't do before because of visualization
map_.emptyTrash();
return true;
@@ -74,7 +72,6 @@ int FrameHandlerBase::finishFrameProcessingCommon(
(stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING ))
{
//stage_ = STAGE_RELOCALIZING;
tracking_quality_ = TRACKING_INSUFFICIENT;
}
if(set_reset_)
resetAll();
@@ -88,26 +85,9 @@ void FrameHandlerBase::resetCommon()
stage_ = STAGE_PAUSED;
set_reset_ = false;
set_start_ = false;
tracking_quality_ = TRACKING_INSUFFICIENT;
num_obs_last_ = 0;
}
void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
{
tracking_quality_ = TRACKING_GOOD;
if(num_observations < Config::qualityMinFts())
{
//SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
tracking_quality_ = TRACKING_INSUFFICIENT;
}
const int feature_drop = static_cast<int>(std::min(num_obs_last_, Config::maxFts())) - num_observations;
if(feature_drop > Config::qualityMaxFtsDrop())
{
//SVO_WARN_STREAM("Lost "<< feature_drop <<" features!");
tracking_quality_ = TRACKING_INSUFFICIENT;
}
}
bool ptLastOptimComparator(Point* lhs, Point* rhs)
{
return (lhs->last_structure_optim_ < rhs->last_structure_optim_);
GPU_version/vio/src/frame_handler_mono.cpp
+10 -31
View File
@@ -22,7 +22,7 @@
#include <vio/point.h>
#include <vio/pose_optimizer.h>
#include <vio/sparse_img_align.h>
#include <vio/depth_filter.h>
#include <vio/global_optimizer.h>
#include <vio/for_it.hpp>
#include <assert.h>
#if VIO_DEBUG
@@ -34,7 +34,7 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
FrameHandlerBase(),
cam_(cam),
reprojector_(cam_, map_),
depth_filter_(NULL),
ba_glob_(NULL),
ukfPtr_(init),
time_(ros::Time::now())
{
@@ -87,14 +87,13 @@ void FrameHandlerMono::initialize()
feature_detection::DetectorPtr feature_detector(
new feature_detection::FastDetector(
cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels()));
DepthFilter::callback_t depth_filter_cb = boost::bind(&MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
depth_filter_->startThread();
ba_glob_ = new BA_Glob(map_);
ba_glob_->startThread();
}
FrameHandlerMono::~FrameHandlerMono()
{
delete depth_filter_;
delete ba_glob_;
}
void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time)
@@ -103,10 +102,8 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
if(!startFrameProcessingCommon(timestamp)){
return;
}
// some cleanup from last iteration, can't do before because of visualization
overlap_kfs_.clear();
// create new frame
new_frame_=std::make_shared<Frame>(cam_, img.clone(), timestamp);
time_=time;
@@ -118,7 +115,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
res = processSecondFrame();
else if(stage_ == STAGE_FIRST_FRAME)
res = processFirstFrame();
last_frame_ = new_frame_;
// finish processing
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
@@ -161,10 +157,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
}
new_frame_->setKeyframe();
double depth_mean, depth_min;
frame_utils::getSceneDepth(klt_homography_init_->frame_ref_, depth_mean, depth_min);
depth_filter_->addKeyframe(klt_homography_init_->frame_ref_, depth_mean, 0.5*depth_min);
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
// add frame to map
map_.addKeyframe(new_frame_);
stage_ = STAGE_DEFAULT_FRAME;
@@ -225,7 +218,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
#endif
if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<4){
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<5){
new_frame_=last_frame_;
return RESULT_FAILURE;
}
@@ -237,19 +230,15 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
fprintf(log_,"[%s] Update EKF and 3D points the number of feature in the new frame: %d and number of obs: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->fts_.size(),new_frame_->nObs());
#endif
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
depth_filter_->addFrame(new_frame_);
// select keyframe
if(!needNewKf())//edited
{
return RESULT_NO_KEYFRAME;
}
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
double depth_mean=0.0, depth_min=0.0;
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
new_frame_->setKeyframe();
#if VIO_DEBUG
fprintf(log_,"[%s] Choose frame as a key frame Scene Depth mean:%f ,depth min:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
@@ -260,19 +249,14 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
for(auto&& it:new_frame_->fts_){
if(it->point != NULL)it->point->addFrameRef(it);
}
map_.point_candidates_.addCandidatePointToFrame(new_frame_);
// init new depth-filters
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
//map_.point_candidates_.addCandidatePointToFrame(new_frame_);
// if limited number of keyframes, remove the one furthest apart
if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
{
FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());
depth_filter_->removeKeyframe(furthest_frame); // TODO this interrupts the mapper thread, maybe we can solve this better
map_.safeDeleteFrame(furthest_frame);
}
// add keyframe to map
map_.addKeyframe(new_frame_);
return RESULT_IS_KEYFRAME;
@@ -285,7 +269,7 @@ void FrameHandlerMono::resetAll()
last_frame_.reset();
new_frame_.reset();
overlap_kfs_.clear();
depth_filter_->reset();
ba_glob_->reset();
}
bool FrameHandlerMono::needNewKf()
{
@@ -298,11 +282,6 @@ bool FrameHandlerMono::needNewKf()
closest_kfs=SE2_5(it.first->T_f_w_.se2());
}
}
#if VIO_DEBUG
fprintf(log_,"[%s] key frame check new frame:%f %f %f, key close:%f %f %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x(),new_frame_->T_f_w_.se2().translation().y(),new_frame_->T_f_w_.pitch(),
closest_kfs.se2().translation().x(),closest_kfs.se2().translation().y(),closest_kfs.pitch());
#endif
if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.43 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.15)return true;
return false;
}
GPU_version/vio/src/global_optimizer.cpp
+90
View File
@@ -0,0 +1,90 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <vio/vision.h>
#include <boost/bind.hpp>
#include <boost/math/distributions/normal.hpp>
#include <vio/global.h>
#include <vio/global_optimizer.h>
#include <vio/frame.h>
#include <vio/point.h>
#include <vio/feature.h>
#include <vio/config.h>
#if VIO_DEBUG
#include <sys/types.h>
#include <sys/stat.h>
#endif
namespace vio {
BA_Glob::BA_Glob(Map& map) :map_(map),seeds_updating_halt_(false),thread_(NULL)
{
#if VIO_DEBUG
log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+");
assert(log_);
chmod((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(), ACCESSPERMS);
#endif
}
BA_Glob::~BA_Glob()
{
stopThread();
}
void BA_Glob::startThread()
{
thread_ = new boost::thread(&BA_Glob::updateLoop, this);
}
void BA_Glob::stopThread()
{
if(thread_ != NULL)
{
seeds_updating_halt_ = true;
thread_->interrupt();
usleep(5000);
thread_->join();
thread_ = NULL;
}
#if VIO_DEBUG
fclose(log_);
#endif
}
void BA_Glob::reset()
{
seeds_updating_halt_ = true;
{
}
seeds_updating_halt_ = false;
#if VIO_DEBUG
fprintf(log_,"[%s] DepthFilter: RESET.\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
}
void BA_Glob::updateLoop()
{
while(!boost::this_thread::interruption_requested())
{
usleep(20000);
}
}
} // namespace vio
GPU_version/vio/src/initialization.cpp
-9
View File
@@ -84,15 +84,6 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
return NO_KEYFRAME;
}
frame_cur->T_f_w_ = T_cur_from_ref_;
// Rescale the map such that the mean scene depth is equal to the specified scale
/* vector<double> depth_vec;
for(size_t i=0; i<xyz_in_cur_.size(); ++i)
depth_vec.push_back((xyz_in_cur_[i]).z());
double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median;
frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2());
frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(),
-frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()))));*/
// For each inlier create 3D point and add feature in both frames
for(auto&& id:inliers_){
Features::iterator first=features_ref_.begin();
GPU_version/vio/src/map.cpp
+1 -153
View File
@@ -34,13 +34,11 @@ Map::~Map()
void Map::reset()
{
keyframes_.clear();
point_candidates_.reset();
emptyTrash();
}
bool Map::safeDeleteFrame(FramePtr frame)
{
if(syn_==true)return false;
bool found = false;
size_t position;
for(auto&& keyframe:_for(keyframes_)){
@@ -54,7 +52,6 @@ bool Map::safeDeleteFrame(FramePtr frame)
std::list<std::shared_ptr<Frame>>::iterator left=keyframes_.begin();
std::advance(left,position);
keyframes_.erase(left);
point_candidates_.removeFrameCandidates(frame);
if(found)
return true;
@@ -108,7 +105,6 @@ void Map::getCloseKeyframes(
const FramePtr& frame,
std::list< std::pair<FramePtr,double> >& close_kfs) const
{
syn_=true;
for(auto kf : keyframes_)
{
// check if kf has overlaping field of view with frame, use therefore KeyPoints
@@ -126,34 +122,13 @@ void Map::getCloseKeyframes(
}
}
}
syn_=false;
}
FramePtr Map::getClosestKeyframe(const FramePtr& frame) const
{
list< pair<FramePtr,double> > close_kfs;
getCloseKeyframes(frame, close_kfs);
if(close_kfs.empty())
{
return nullptr;
}
// Sort KFs with overlap according to their closeness
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
if(close_kfs.front().first != frame)
return close_kfs.front().first;
close_kfs.pop_front();
return close_kfs.front().first;
}
FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const
{
FramePtr furthest_kf;
double maxdist = 0.0;
syn_=true;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
{
double dist = ((*it)->pos()-pos).norm();
@@ -162,144 +137,17 @@ FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const
furthest_kf = *it;
}
}
syn_=false;
return furthest_kf;
}
bool Map::getKeyframeById(const int id, FramePtr& frame) const
{
bool found = false;
syn_=true;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
if((*it)->id_ == id) {
found = true;
frame = *it;
break;
}
syn_=false;
return found;
}
void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s)
{
Eigen::Matrix<double, 3,3> R3;
R3<<R(0,0),0.0,R(0,1),
0.0,1.0,0.0,
R(1,0),0.0,R(1,1);
syn_=true;
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
{
SE2_5 tem(SE2(R*(*it)->T_f_w_.inverse().rotation_matrix(),s*R*(*it)->pos() + t));
(*it)->T_f_w_ = tem.inverse();
for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)
{
if((*ftr)->point == NULL)
continue;
if((*ftr)->point->last_published_ts_ == -1000)
continue;
(*ftr)->point->last_published_ts_ = -1000;
(*ftr)->point->pos_ = s*R3*(*ftr)->point->pos_ + Vector3d(t(0),0.0,t(1));
}
}
syn_=false;
}
void Map::emptyTrash()
{
if(trash_points_.empty())return;
for(auto&& t:trash_points_)t.reset();
trash_points_.clear();
point_candidates_.emptyTrash();
}
MapPointCandidates::MapPointCandidates()
{}
MapPointCandidates::~MapPointCandidates()
{
reset();
}
void MapPointCandidates::newCandidatePoint(std::shared_ptr<Point> point, double depth_sigma2)
{
point->type_ = Point::TYPE_CANDIDATE;
boost::unique_lock<boost::mutex> lock(mut_);
candidates_.push_back(PointCandidate(point, point->obs_.front()));
}
void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
{
boost::unique_lock<boost::mutex> lock(mut_);
PointCandidateList::iterator it=candidates_.begin();
while(it != candidates_.end())
{
if(it->first->obs_.front()->frame == frame)
{
// insert feature in the frame
it->first->type_ = Point::TYPE_UNKNOWN;
it->first->n_failed_reproj_ = 0;
it->second->frame->addFeature(it->second);
it = candidates_.erase(it);
}
else
++it;
}
}
bool MapPointCandidates::deleteCandidatePoint(std::shared_ptr<Point> point)
{
boost::unique_lock<boost::mutex> lock(mut_);
for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it)
{
if(it->first == point)
{
deleteCandidate(*it);
candidates_.erase(it);
return true;
}
}
return false;
}
void MapPointCandidates::removeFrameCandidates(FramePtr frame)
{
boost::unique_lock<boost::mutex> lock(mut_);
auto it=candidates_.begin();
while(it!=candidates_.end())
{
if(it->second->frame->id_ == frame->id_)
{
deleteCandidate(*it);
it = candidates_.erase(it);
}
else
++it;
}
}
void MapPointCandidates::reset()
{
boost::unique_lock<boost::mutex> lock(mut_);
std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){
c.first.reset();
c.second.reset();
});
candidates_.clear();
}
void MapPointCandidates::deleteCandidate(PointCandidate& c)
{
// camera-rig: another frame might still be pointing to the candidate point
// therefore, we can't delete it right now.
c.second.reset();
c.first->type_ = Point::TYPE_DELETED;
trash_points_.push_back(c.first);
}
void MapPointCandidates::emptyTrash()
{
for(auto&& point:trash_points_)point.reset();
trash_points_.clear();
}
namespace map_debug {
GPU_version/vio/src/matcher.cpp
+2 -2
View File
@@ -161,7 +161,7 @@ bool Matcher::findMatchDirect(
warp::getWarpMatrixAffine(
*ref_ftr_->frame->cam_, *(cur_frame.cam_), ref_ftr_->px, ref_ftr_->f,
(Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(),
cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_);
cur_frame.se3().inverse() * ref_ftr_->frame->se3(), ref_ftr_->level, A_cur_ref_);
search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_))return false;
@@ -198,7 +198,7 @@ bool Matcher::findEpipolarMatchDirect(
double& depth,FILE* log)
{
if(isnan(d_min) || isnan(d_max))return false;
SE2_5 T_cur_ref(SE2(cur_frame.T_f_w_.so2() * ref_frame.T_f_w_.inverse().so2(),cur_frame.T_f_w_.se2().translation() - ref_frame.T_f_w_.se2().translation()));
SE2_5 T_cur_ref(SE2(cur_frame.T_f_w_.inverse() * ref_frame.T_f_w_.se2()));
int zmssd_best = PatchScore::threshold();
Vector2d uv_best;
GPU_version/vio/src/math_utils.cpp
+5 -1
View File
@@ -11,6 +11,9 @@
namespace vk {
using namespace Eigen;
/*
* R and T are the rotation and translation of the current frames with respect to the refrence frame
* */
Vector3d
triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t,
@@ -81,7 +84,7 @@ computeInliers(const vector<Vector3d>& features1, // c2
//triangulate all features and compute reprojection errors and inliers
for(size_t j=0; j<features1.size(); ++j)
{
if(t.z()>0.0)
if(t.z()>0.0){
xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
@@ -93,6 +96,7 @@ computeInliers(const vector<Vector3d>& features1, // c2
tot_error += e1+e2;
}
}
}
return tot_error;
}
GPU_version/vio/src/pose_optimizer.cpp
+1 -1
View File
@@ -41,7 +41,7 @@ void optimizeGaussNewton(
double chi2(0.0);
vector<double> chi2_vec_init, chi2_vec_final;
vk::robust_cost::HuberWeightFunction weight_function;
SE2_5 T_old(frame->T_f_w_);
SE2_5 T_old(frame->T_f_w_.se2());
Matrix3d A;
Vector3d b;
GPU_version/vio/src/reprojector.cpp
+5 -21
View File
@@ -89,7 +89,6 @@ namespace vio {
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
overlap_kfs.reserve(options_.max_n_kfs);
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
for (auto &&it_frame:_for(close_kfs)) {
if (it_frame.index > options_.max_n_kfs)continue;
std::vector<cv::KeyPoint> keypoints_kfs;
@@ -116,7 +115,7 @@ namespace vio {
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) (*point)->px.x(),
(int) (*point)->px.y());
SE3 T_ref_cur=it_frame.item.first->getSE3Inv()*frame->se3();
SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3();
Vector3d new_point=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
frame->c2f(px),f.item->f);
if(new_point.z()<0.0)break;
@@ -177,21 +176,19 @@ namespace vio {
it = cell.erase(it);
continue;
}
syn_=true;
///TODO check point candidate handling
if(!matcher_.findMatchDirect(*it->pt, *frame, it->px))
{
it->pt->n_failed_reproj_++;
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
/* if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
map_.safeDeletePoint(it->pt);
if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30)
map_.point_candidates_.deleteCandidatePoint(it->pt);
map_.point_candidates_.deleteCandidatePoint(it->pt);*/
it = cell.erase(it);
// ++it;
continue;
}
syn_=false;
it->pt->n_succeeded_reproj_++;
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
it->pt->type_ = Point::TYPE_GOOD;
@@ -222,17 +219,4 @@ namespace vio {
return false;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO original implementation
bool Reprojector::reprojectPoint(FramePtr frame, std::shared_ptr<Point> point) {
Vector2d px(frame->w2c(point->pos_));
if (frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
{
const int k = static_cast<int>(px[1] / grid_.cell_size) * grid_.grid_n_cols
+ static_cast<int>(px[0] / grid_.cell_size);
grid_.cells.at(k)->push_back(Candidate(point, px));
return true;
}
return false;
}
}
GPU_version/vio/src/vo_node.cpp
+1 -1
View File
@@ -18,7 +18,7 @@
#include <vio/getOdom.h>
#include <vio/start.h>
#include <vio/stop.h>
#include <vio/depth_filter.h>
#include <vio/global_optimizer.h>
#include <ros/callback_queue.h>
#if VIO_DEBUG
#include <vio/visualizer.h>