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

added flag to switch to original, using right null space matrix for calculation now and existing eigen function, gating restult still way to high

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-25 11:16:44 +02:00
parent 821d9d6f71
commit 6ba26d782d
5 changed files with 59 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/msckf_vio/feature.hpp
View File

@ -441,7 +441,7 @@ bool Feature::VisualizePatch(
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
{
return (float)image.at<uint8_t>(pose.x, pose.y);
return (float)(image.at<uint8_t>(pose.x, pose.y));
}
cv::Point2f Feature::projectPositionToCamera(
@ -496,7 +496,7 @@ bool Feature::initializeAnchor(
//initialize patch Size
//TODO make N size a ros parameter
int N = 13;
int N = 9;
int n = (int)(N-1)/2;
auto anchor = observations.begin();

3
include/msckf_vio/msckf_vio.h
View File

@ -201,6 +201,9 @@ class MsckfVio {
// Reset the system online if the uncertainty is too large.
void onlineReset();
// Photometry flag
bool PHOTOMETRIC;
// Chi squared test table.
static std::map<int, double> chi_squared_test_table;

3
launch/msckf_vio_debug_tum.launch
View File

@ -18,6 +18,9 @@
output="screen"
launch-prefix="xterm -e gdb --args">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>

3
launch/msckf_vio_tum.launch
View File

@ -17,6 +17,9 @@
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>

72
src/msckf_vio.cpp
View File

@ -27,6 +27,7 @@
#include <msckf_vio/math_utils.hpp>
#include <msckf_vio/utils.h>
using namespace std;
using namespace Eigen;
@ -59,6 +60,10 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
}
bool MsckfVio::loadParameters() {
//Photometry Flag
nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
// Frame id
nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
nh.param<string>("child_frame_id", child_frame_id, "robot");
@ -340,7 +345,7 @@ void MsckfVio::imageCallback(
// Augment the state vector.
start_time = ros::Time::now();
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
stateAugmentation(feature_msg->header.stamp.toSec());
double state_augmentation_time = (
ros::Time::now()-start_time).toSec();
@ -968,7 +973,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
const Vector3d& t_c0_w = cam_state.position;
//temp N
const int N = 13;
const int N = 9;
// Cam1 pose.
Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
@ -1116,7 +1121,7 @@ void MsckfVio::PhotometricFeatureJacobian(
const auto& feature = map_server[feature_id];
int N = 13;
int N = 9;
// Check how many camera states in the provided camera
// id camera has actually seen this feature.
vector<StateIDType> valid_cam_state_ids(0);
@ -1179,12 +1184,25 @@ void MsckfVio::PhotometricFeatureJacobian(
// Project the residual and Jacobians onto the nullspace
// of H_yj.
JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
MatrixXd A = svd_helper.matrixU().rightCols(
jacobian_row_size - 3);
// get Nullspace
H_x = A.transpose() * H_xi;
r = A.transpose() * r_i;
FullPivLU<MatrixXd> lu(H_yi);
MatrixXd A_right = lu.kernel();
//FullPivLU<MatrixXd> lu2(A_right.transpose());
//MatrixXd A = lu2.kernel().transpose();
/*
JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size();
MatrixXd A = svd_helper.matrixU().rightCols(
jacobian_row_size-null_space_size);
*/
//H_x = A.transpose() * H_xi;
//r = A.transpose() * r_i;
H_x = H_xi * A_right;
r = r_i * A_right;
cout << "r\n" << r << endl;
cout << "Hx\n" << H_x << endl;
return;
}
@ -1327,7 +1345,6 @@ void MsckfVio::measurementUpdate(
// complexity as in Equation (28), (29).
MatrixXd H_thin;
VectorXd r_thin;
cout << " measurement update ..." << endl;
if (H.rows() > H.cols()) {
// Convert H to a sparse matrix.
@ -1431,8 +1448,8 @@ bool MsckfVio::gatingTest(
MatrixXd::Identity(H.rows(), H.rows());
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
//cout << dof << " " << gamma << " " <<
// chi_squared_test_table[dof] << " ";
cout << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << " ";
if (gamma < chi_squared_test_table[dof]) {
//cout << "passed" << endl;
@ -1517,7 +1534,12 @@ void MsckfVio::removeLostFeatures() {
MatrixXd H_xj;
VectorXd r_j;
PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j);
if(PHOTOMETRIC)
PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j);
else
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j;
@ -1529,8 +1551,6 @@ void MsckfVio::removeLostFeatures() {
cout << "failed gating test" << endl;
}
cout << " stacked features up" << endl;
// Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time.
if (stack_cntr > 1500) break;
@ -1673,9 +1693,10 @@ void MsckfVio::pruneCamStateBuffer() {
MatrixXd H_xj;
VectorXd r_j;
cout << "getting featureJacobian...";
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
cout << "done" << endl;
if(PHOTOMETRIC)
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
else
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
@ -1691,19 +1712,22 @@ void MsckfVio::pruneCamStateBuffer() {
feature.observations.erase(cam_id);
}
cout << " stacked features up" << endl;
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
// Perform measurement update.
measurementUpdate(H_x, r);
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
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));
int cam_state_start = 21 + 6*cam_sequence;
int cam_state_end = cam_state_start + 6;
int cam_state_start = 21 + augmentationSize*cam_sequence;
int cam_state_end = cam_state_start + augmentationSize;
// Remove the corresponding rows and columns in the state
// covariance matrix.
@ -1723,10 +1747,10 @@ void MsckfVio::pruneCamStateBuffer() {
state_server.state_cov.cols()-cam_state_end);
state_server.state_cov.conservativeResize(
state_server.state_cov.rows()-6, state_server.state_cov.cols()-6);
state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize);
} else {
state_server.state_cov.conservativeResize(
state_server.state_cov.rows()-6, state_server.state_cov.cols()-6);
state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize);
}
// Remove this camera state in the state vector.