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

minor scale change in distance between pixels

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-11 10:59:41 +02:00
parent 9b4bf12846
commit 44fffa19a2
3 changed files with 13 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/msckf_vio.cpp
View File

@@ -1427,23 +1427,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];
@@ -1493,21 +1476,12 @@ 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;
cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
if(PRINTIMAGES)
{
ofstream myfile;
@@ -1665,6 +1639,8 @@ void MsckfVio::featureJacobian(
r = A.transpose() * r_j;
cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
// stop playing bagfile if printing images
if(PRINTIMAGES)
{