raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

removed resizing not correcting for photometric info, added N as global variable

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-26 09:44:19 +02:00
parent e2e936ff01
commit e8489dbd06
4 changed files with 33 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/msckf_vio/feature.hpp
View File

@ -126,7 +126,7 @@ struct Feature {
*/
bool initializeAnchor(
const CameraCalibration& cam);
const CameraCalibration& cam, int N);
/*
@ -491,12 +491,10 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
//@test center projection must always be initial feature projection
bool Feature::initializeAnchor(
const CameraCalibration& cam)
const CameraCalibration& cam, int N)
{
//initialize patch Size
//TODO make N size a ros parameter
int N = 3;
int n = (int)(N-1)/2;
auto anchor = observations.begin();

3
include/msckf_vio/msckf_vio.h
View File

@ -206,6 +206,9 @@ class MsckfVio {
bool nan_flag;
// Patch size for Photometry
int N;
// Chi squared test table.
static std::map<int, double> chi_squared_test_table;

1
launch/msckf_vio_tum.launch
View File

@ -20,6 +20,7 @@
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/>
<param name="patch_size_n" value="5"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>

69
src/msckf_vio.cpp
View File

@ -127,6 +127,10 @@ bool MsckfVio::loadParameters() {
nh.param<double>("initial_covariance/irradiance_frame_bias",
irradiance_frame_bias, 0.1);
// Get the photometric patch size
nh.param<int>("patch_size_n",
N, 3);
// get camera information (used for back projection)
nh.param<string>("cam0/distortion_model",
cam0.distortion_model, string("radtan"));
@ -288,7 +292,7 @@ bool MsckfVio::initialize() {
for (int i = 1; i < 100; ++i) {
boost::math::chi_squared chi_squared_dist(i);
chi_squared_test_table[i] =
boost::math::quantile(chi_squared_dist, 0.05);
boost::math::quantile(chi_squared_dist, 0.2);
}
if (!createRosIO()) return false;
@ -979,9 +983,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Vector3d& t_c0_w = cam_state.position;
//temp N
const int N = 3;
// Cam1 pose.
Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
@ -1128,7 +1129,6 @@ void MsckfVio::PhotometricFeatureJacobian(
const auto& feature = map_server[feature_id];
int N = 3;
// Check how many camera states in the provided camera
// id camera has actually seen this feature.
vector<StateIDType> valid_cam_state_ids(0);
@ -1346,7 +1346,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0) return;
cout << "decomposition...";
cout << "Updating...";
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;
@ -1379,8 +1379,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
H_thin = H;
r_thin = r;
}
cout << "done" << endl;
cout << "computing K...";
// Compute the Kalman gain.
const MatrixXd& P = state_server.state_cov;
MatrixXd S = H_thin*P*H_thin.transpose() +
@ -1390,8 +1389,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
MatrixXd K = K_transpose.transpose();
cout << "done" << endl;
// Compute the error of the state.
VectorXd delta_x = K * r_thin;
@ -1457,15 +1454,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
MatrixXd P2 = Feature::observation_noise *
MatrixXd::Identity(H.rows(), H.rows());
//cout << "H: \n" << H << endl;
//cout << "cov: \n" << state_server.state_cov << endl;
//cout << "P1: \n" << P1 << endl;
//cout << "solv: \n" << (P1+P2).ldlt().solve(r) << endl;
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
cout << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << " ";
chi_squared_test_table[dof] << endl;
if (gamma < chi_squared_test_table[dof]) {
//cout << "passed" << endl;
@ -1480,11 +1472,14 @@ void MsckfVio::removeLostFeatures() {
// Remove the features that lost track.
// BTW, find the size the final Jacobian matrix and residual vector.
int jacobian_row_size = 0;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
vector<FeatureIDType> invalid_feature_ids(0);
vector<FeatureIDType> processed_feature_ids(0);
int N = 3;
for (auto iter = map_server.begin();
iter != map_server.end(); ++iter) {
// Rename the feature to be checked.
@ -1513,7 +1508,7 @@ void MsckfVio::removeLostFeatures() {
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0))
if(!feature.initializeAnchor(cam0, N))
{
invalid_feature_ids.push_back(feature.id);
continue;
@ -1540,10 +1535,6 @@ void MsckfVio::removeLostFeatures() {
// Return if there is no lost feature to be processed.
if (processed_feature_ids.size() == 0) return;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
21+augmentationSize*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size);
@ -1569,11 +1560,11 @@ void MsckfVio::removeLostFeatures() {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows();
cout << "approved chi" << endl;
cout << "passed" << endl;
}
else
{
cout << "rejected chi" << endl;
cout << "rejected" << endl;
}
// Put an upper bound on the row size of measurement Jacobian,
@ -1647,10 +1638,12 @@ void MsckfVio::pruneCamStateBuffer() {
vector<StateIDType> rm_cam_state_ids(0);
findRedundantCamStates(rm_cam_state_ids);
int N = 3;
// Find the size of the Jacobian matrix.
int jacobian_row_size = 0;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
for (auto& item : map_server) {
auto& feature = item.second;
// Check how many camera states to be removed are associated
@ -1686,7 +1679,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0))
if(!feature.initializeAnchor(cam0, N))
{
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
@ -1702,8 +1695,9 @@ void MsckfVio::pruneCamStateBuffer() {
//cout << "jacobian row #: " << jacobian_row_size << endl;
// Compute the Jacobian and residual.
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
21+6*state_server.cam_states.size());
MatrixXd H_xj;
VectorXd r_j;
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
for (auto& item : map_server) {
@ -1719,9 +1713,6 @@ void MsckfVio::pruneCamStateBuffer() {
if (involved_cam_state_ids.size() == 0) continue;
MatrixXd H_xj;
VectorXd r_j;
if(PHOTOMETRIC)
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
else
@ -1731,29 +1722,23 @@ void MsckfVio::pruneCamStateBuffer() {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows();
cout << "made gating test" << endl;
cout << "passed" << endl;
}
else
{
cout << "failed gating test" << endl;
cout << "rejected" << endl;
}
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
cout << "resize" << endl;
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
cout << "done" << endl;
// Perform measurement update.
measurementUpdate(H_x, r);
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
//reduction
for (const auto& cam_id : rm_cam_state_ids) {
int cam_sequence = std::distance(state_server.cam_states.begin(),
state_server.cam_states.find(cam_id));