Skip to content

Commit

Permalink
fix MIN_PARRALAX bug, but also have accuracy problwm
Browse files Browse the repository at this point in the history
  • Loading branch information
solomonhe committed Sep 8, 2017
1 parent 345db04 commit 5c1145c
Showing 12 changed files with 502 additions and 99 deletions.
7 changes: 6 additions & 1 deletion camera_model/src/camera_models/PinholeCamera.cc
Original file line number Diff line number Diff line change
@@ -7,6 +7,8 @@
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
using namespace std;

#include "camodocal/gpl/gpl.h"

@@ -178,7 +180,9 @@ PinholeCamera::Parameters::readFromYamlFile(const std::string& filename)
m_fy = static_cast<double>(n["fy"]);
m_cx = static_cast<double>(n["cx"]);
m_cy = static_cast<double>(n["cy"]);


cout << "m_k1:" << m_k1 << " m_k2" << m_k2 << " m_p1:"<<m_p1 << " m_p2:" << m_p2<< endl;
cout << "m_fx:" << m_fx << " m_fy" << m_fy << " m_cx:"<<m_cx << " m_cy:" << m_cy<< endl;
return true;
}

@@ -506,6 +510,7 @@ PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) cons
}

// Obtain a projective ray
//cout<<"lift:mx_u:"<<mx_u<<",my_u:"<<my_u<<endl;
P << mx_u, my_u, 1.0;
}

4 changes: 2 additions & 2 deletions config/euroc/euroc_config.yaml
Original file line number Diff line number Diff line change
@@ -25,7 +25,7 @@ projection_parameters:
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
ex_calib_result_path: "/config/euroc/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.
ex_calib_result_path: "/src/config/euroc/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
@@ -65,7 +65,7 @@ g_norm: 9.81007 # gravity magnitude


#loop closure parameters
loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;
loop_closure: 0 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;
#also give the camera calibration file same as feature_tracker node
pattern_file: "/src/support_files/brief_pattern.yml"
voc_file: "/src/support_files/brief_k10L6.bin"
10 changes: 5 additions & 5 deletions config/euroc/euroc_config_no_extrinsic.yaml
Original file line number Diff line number Diff line change
@@ -3,7 +3,7 @@
#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
output_path: "/config/euroc/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path
output_path: "/src/config/euroc/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path

#camera calibration
model_type: PINHOLE
@@ -25,15 +25,15 @@ projection_parameters:
estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
ex_calib_result_path: "/config/euroc/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.
ex_calib_result_path: "/src/config/euroc/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.
#If you choose 0 or 1, you should write down the following matrix.

#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
show_track:0 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

@@ -53,8 +53,8 @@ g_norm: 9.81007 # gravity magnitude
#loop closure parameters
loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;
#also give the camera calibration file same as feature_tracker node
pattern_file: "/support_files/brief_pattern.yml"
voc_file: "/support_files/brief_k10L6.bin"
pattern_file: "/src/support_files/brief_pattern.yml"
voc_file: "/src/support_files/brief_k10L6.bin"
min_loop_num: 25


Loading

0 comments on commit 5c1145c

Please sign in to comment.