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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/msckf_vio/feature.hpp
View File

@ -874,8 +874,14 @@ cv::Point2f Feature::pixelDistanceAt(
cam.distortion_coeffs, cam.distortion_coeffs,
pure); pure);
// returns the absolute pixel distance at pixels one metres away // transfrom position to camera frame
cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y)); // to get distance multiplier
Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Eigen::Vector3d& t_c0_w = cam_state.position;
Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
// returns the distance between the pixel points in space
cv::Point2f distance(fabs(pure[0].x - pure[1].x)*p_c0[2], fabs(pure[2].y - pure[3].y)*p_c0[2]);
return distance; return distance;
} }

2
launch/msckf_vio_tum.launch
View File

@ -18,7 +18,7 @@
output="screen"> output="screen">
<!-- Photometry Flag--> <!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="false"/> <param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="PrintImages" value="false"/> <param name="PrintImages" value="false"/>

32
src/msckf_vio.cpp
View File

@ -1427,23 +1427,6 @@ void MsckfVio::PhotometricFeatureJacobian(
nh.setParam("/play_bag", false); 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]; const auto& feature = map_server[feature_id];
@ -1493,21 +1476,12 @@ void MsckfVio::PhotometricFeatureJacobian(
// get Nullspace // get Nullspace
FullPivLU<MatrixXd> lu(H_yi.transpose()); FullPivLU<MatrixXd> lu(H_yi.transpose());
MatrixXd A_null_space = lu.kernel(); 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; H_x = A_null_space.transpose() * H_xi;
r = A_null_space.transpose() * r_i; r = A_null_space.transpose() * r_i;
cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
if(PRINTIMAGES) if(PRINTIMAGES)
{ {
ofstream myfile; ofstream myfile;
@ -1665,6 +1639,8 @@ void MsckfVio::featureJacobian(
r = A.transpose() * r_j; r = A.transpose() * r_j;
cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
// stop playing bagfile if printing images // stop playing bagfile if printing images
if(PRINTIMAGES) if(PRINTIMAGES)
{ {