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

cache

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-23 09:17:05 +02:00
parent 2f130685c8
commit 0be7047928
2 changed files with 18 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
include/msckf_vio/feature.hpp
View File

@ -206,12 +206,6 @@ struct Feature {
const StateIDType& cam_state_id,
std::vector<float>& return_i) const;
/*
* @brief AnchorPixelToPosition uses the calcualted pixels
* of the anchor patch to generate 3D positions of all of em
*/
inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p,
const CameraCalibration& cam);
/*
* @brief Irradiance returns irradiance value of a pixel
@ -821,20 +815,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal
//printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
}
Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
{
// use undistorted position of point of interest
// project it back into 3D space using pinhole model
// save resulting NxN positions for this feature
Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho);
Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
return PositionInWorld;
//printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
}
//@test center projection must always be initial feature projection
bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
{

64
src/msckf_vio.cpp
View File

@ -1231,9 +1231,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
// one line of the NxN Jacobians
Matrix<double, 1, 6> H_xi;
Matrix<double, 1, 6> H_xA;
Matrix<double, 1, 3> H_fi;
MatrixXd H_xl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
MatrixXd H_xAl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
MatrixXd H_yl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 3);
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
@ -1301,34 +1303,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_xi = dI_dhj*dz_dpc0*dpc0_dxc;
H_fi = dI_dhj*dz_dpc0*dpc0_dpg;
gradientVector.x += dx;
gradientVector.y += dy;
residualVector.x += dx * photo_r[count];
residualVector.y += dy * photo_r[count];
res_sum += photo_r[count];
//dh / d{}^Cp_{ij}
dh_dCpij(0, 0) = 1 / p_c0(2);
dh_dCpij(1, 1) = 1 / p_c0(2);
dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
dCpij_dGpij = quaternionToRotation(cam_state.orientation);
//orientation takes camera frame to world frame, we wa
dh_dGpij = dh_dCpij * dCpij_dGpij;
dh_dGpij = dz_dpc0 * dCpij_dGpij;
//dh / d X_{pl}
dCpij_dCGtheta = skewSymmetric(p_c0);
dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
//d{}^Gp_P{ij} / \rho_i
double rho = feature.anchor_rho;
// Isometry T_anchor_w takes a vector in anchor frame to world frame
dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
double rho = feature.rh
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
@ -1336,8 +1316,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
1/(rho)));
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
H_xA = dI_dhj*dh_dGpij*dGpj_XpAj
H_xl.block<1, 6>(count, 0) = H_xi;
H_yl.block<1, 3>(count, 0) = H_fi;
H_xAl.block<1, 6>(count, 0) = H_xA;
count++;
}
@ -1350,12 +1333,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count-1);
for(int i = 0; i < r_l.size(); i++)
r_l(i) = z_irr[i]- z_irr_est[i];
r_l(i) = z_irr[i]- feature.anchorPatch[i];
r = r_l;
H_x = H_xl;
H_y = H_yl;
H_y = H_xAl;
//TODO make this more fluent as well
@ -1438,32 +1421,21 @@ void MsckfVio::PhotometricFeatureJacobian(
int cam_state_cntr = std::distance(
state_server.cam_states.begin(), cam_state_iter);
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
// Stack the Jacobians.
H_xi.block(stack_cntr, 21+cam_state_cntr_anchor*7, H_yl.rows(), H_yl.cols()) = -H_yi;
H_xi.block(stack_cntr, 21+cam_state_cntr*7, H_xl.rows(), H_xl.cols()) = H_xl;
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
r_i.segment(stack_cntr, N*N) = r_l;
stack_cntr += N*N;
}
// Project the residual and Jacobians onto the nullspace
// of H_yj.
// 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;
H_x = H_xi;
r = r_i;
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");