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

add more restriction

Browse Source
This commit is contained in:
gfuse
2021-10-07 19:36:49 +02:00
parent 45ed85c0e9
commit 91a01ec03d
11 changed files with 31 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/sophus/so3.cpp
+1 -1
View File
@@ -161,7 +161,7 @@ Vector3d SO3
two_atan_nbyw_by_n = -M_PI/n;
}
}
two_atan_nbyw_by_n = 2*atan(n/w)/n;
two_atan_nbyw_by_n = 2*atan2(n,w)/n;
}
*theta = two_atan_nbyw_by_n*n;
GPU_version/vio/include/vio/atan_camera.h
+6 -2
View File
@@ -43,8 +43,12 @@ private:
{
if(r < 0.001 || s_ == 0.0)
return 1.0;
else
return (s_inv_* atan(r * tans_) / r);
else{
float y=2*r*sin(0.5 * (float)s_);
float x=cos(0.5 * (float)s_);
return (atan2(y,x) / (r * (float)s_));
//return (s_inv_* atan(r * tans_) / r);
}
};
//! Inverse radial distortion: returns un-distorted radius from distorted.
GPU_version/vio/include/vio/matcher.h
+2 -2
View File
@@ -66,8 +66,8 @@ class Matcher
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static const int halfpatch_size_ = 8;
static const int patch_size_ = 16;
static const int halfpatch_size_ = 4;
static const int patch_size_ = 8;
typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;
GPU_version/vio/param/vo_fast.yaml
+4 -4
View File
@@ -9,10 +9,10 @@ vio:
structureoptim_max_pts: 50
kfselect_mindist: 0.01
poseoptim_thresh: 8.0
init_min_inliers: 50
init_min_inliers: 40
ACC_ekf: 0.2 #acc white noise in continuous in EKF
GYO_ekf: 0.05 #gyro white noise in continuous in EKF
VO_ekf_translation: 0.5 #VO noise in EKF
VO_ekf_orientation: 1.5 #VO noise in EKF
GYO_ekf: 0.1 #gyro white noise in continuous in EKF
VO_ekf_translation: 1.0 #VO noise in EKF
VO_ekf_orientation: 3.0 #VO noise in EKF
CMD_ekf_translation: 0.1 #control vector noise in EKF
CMD_ekf_orientation: 0.01 #control vector noise in EKF
GPU_version/vio/src/cl_class.cpp
+1 -1
View File
@@ -38,7 +38,7 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) {
sources.push_back({ compute_residual.src_str, compute_residual.size });
program=new cl::Program(*context, sources);
double* camera=cam->params();
std::string options="-DFAST_THRESH=40 -DPATCH_SIZE=2 -DPATCH_HALFSIZE=1 -DF_X="+ std::to_string(camera[0]) +
std::string options="-DFAST_THRESH=40 -DPATCH_SIZE=8 -DPATCH_HALFSIZE=4 -DF_X="+ std::to_string(camera[0]) +
" -DF_Y="+std::to_string(camera[1])+
" -DC_X="+std::to_string(camera[2])+
" -DC_Y="+std::to_string(camera[3])+
GPU_version/vio/src/feature_alignment.cpp
+3 -3
View File
@@ -163,9 +163,9 @@ bool align2D(
return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
#endif
const int halfpatch_size_ = 8;
const int patch_size_ = 16;
const int patch_area_ = 256;
const int halfpatch_size_ = 4;
const int patch_size_ = 8;
const int patch_area_ = 64;
bool converged=false;
// compute derivative of template and prepare inverse compositional
GPU_version/vio/src/frame.cpp
-1
View File
@@ -178,7 +178,6 @@ bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
{
double z = frame->w2f((*it)->point->pos_).z();
depth_vec.push_back(z);
}
}
if(depth_vec.empty())
GPU_version/vio/src/frame_handler_mono.cpp
+6 -2
View File
@@ -179,14 +179,18 @@ 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
double depth_mean=0.0, depth_min=0.0;
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
if(depth_mean>10.0)return RESULT_NO_KEYFRAME;
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
// select keyframe
if(!needNewKf())//edited
{
return RESULT_NO_KEYFRAME;
}
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(),
GPU_version/vio/src/global_optimizer.cpp
+4 -6
View File
@@ -164,9 +164,6 @@ namespace vio {
if((*it_ftr)->point == NULL)
continue;
std::shared_ptr<Point> mp = (*it_ftr)->point;
if(mp->v_pt_ == NULL)
continue; // mp was updated before
mp->pos_ = mp->v_pt_->estimate();
mp->v_pt_ = NULL;
}
}
@@ -192,7 +189,7 @@ namespace vio {
{
g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
v->setId(id);
v->setFixed(fixed);
v->setFixed(false);
v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation()));
return v;
}
@@ -204,7 +201,8 @@ namespace vio {
{
g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
v->setId(id);
v->setMarginalized(fixed);
v->setFixed(fixed);
v->setMarginalized(true);
v->setEstimate(pos);
return v;
@@ -221,7 +219,7 @@ namespace vio {
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
e->setMeasurement(f_up);
e->information() = Eigen::Matrix2d::Identity();
e->information() = weight*Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(huber_width);
e->setRobustKernel(rk);
GPU_version/vio/src/matcher.cpp
+2 -2
View File
@@ -41,7 +41,7 @@ void getWarpMatrixAffine(
Matrix2d& A_cur_ref)
{
// Compute affine warp matrix A_ref_cur
const int halfpatch_size = 8;
const int halfpatch_size = 4;
Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(f_ref)));
@@ -206,7 +206,7 @@ bool Matcher::findMatchDirect(
cv::Mat Patch_ref = cv::Mat(patch_size_+2, patch_size_+2, CV_8UC1, patch_with_border_);
cv::Mat Patch_cur = cv::Mat(patch_size_+2, patch_size_+2, CV_8UC1, patch_with_border_cur_);
cv::Scalar res = getMSSIM(Patch_cur, Patch_ref);
if(res[0] >= 0.75)
if(res[0] >= 0.85)
success = true;
else
success = false;
GPU_version/vio/src/math_utils.cpp
+2 -2
View File
@@ -36,8 +36,8 @@ triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t,
Vector3d b1 = t.cross(R_F2_p);
double lamda0 = z.dot(b0)/pow(z.norm(), 2);
double lamda1 = z.dot(b1)/pow(z.norm(), 2);
//if (lamda0 <= 0 || lamda1 <= 0)return Vector3d(0.0,0.0,0.0);
if (max(m0_hat.dot(R_F2_p), feature1.dot(f1_p)) > M_PI_2)return Vector3d(0.0,0.0,0.0);
if (lamda0 <= 0 || lamda1 <= 0)return Vector3d(0.0,0.0,0.0);
if (max(m0_hat.dot(R_F2_p), feature1.dot(f1_p)) > 0.70*M_PI_2)return Vector3d(0.0,0.0,0.0);
if(R_F2_p.dot(f1_p) < 0.01)return Vector3d(0.0,0.0,0.0);
Vector3d xm = lamda1 * f1_p;
Vector3d xn = t + lamda0 * R_F2_p;