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

added some feature calculations to this branch

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-11 16:45:17 +02:00
parent 8cfbe06945
commit a0577dfb9d
3 changed files with 388 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/msckf_vio.cpp
View File

@ -1254,7 +1254,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
int count = 0;
double dx, dy;
auto point = feature.anchorPatch_3d[0];
@ -1367,23 +1366,6 @@ void MsckfVio::PhotometricFeatureJacobian(
nh.setParam("/play_bag", false);
}
// Errstate
calcErrorState();
/*
std::cout << "--- error state: ---\n " << std::endl;
std::cout << "imu orientation:\n " << err_state_server.imu_state.orientation << std::endl;
std::cout << "imu position\n" << err_state_server.imu_state.position << std::endl;
int count = 0;
for(auto cam_state : err_state_server.cam_states)
{
std::cout << " - cam " << count++ << " - \n" << std::endl;
std::cout << "orientation: " << cam_state.second.orientation(0) << cam_state.second.orientation(1) << " " << cam_state.second.orientation(2) << " " << std::endl;
std::cout << "position:" << cam_state.second.position(0) << " " << cam_state.second.position(1) << " " << cam_state.second.position(2) << std::endl;
}
*/
const auto& feature = map_server[feature_id];
@ -1430,22 +1412,10 @@ void MsckfVio::PhotometricFeatureJacobian(
// get Nullspace
FullPivLU<MatrixXd> lu(H_yi.transpose());
MatrixXd A_null_space = lu.kernel();
/*
JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
int sv_size = 0;
Eigen::VectorXd singularValues = svd_helper.singularValues();
for(int i = 0; i < singularValues.size(); i++)
if(singularValues[i] > 1e-12)
sv_size++;
MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size());
*/
H_x = A_null_space.transpose() * H_xi;
r = A_null_space.transpose() * r_i;
if(PRINTIMAGES)
{
std::cout << "resume playback" << std::endl;