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

fixed Irradiance jacobain calculation, now division by pixel distance

Browse Source
This commit is contained in:
Raphael Maenle
2019-05-23 18:34:57 +02:00
parent 0be7047928
commit 82cd2c6771
4 changed files with 102 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
src/msckf_vio.cpp
View File

@ -1232,29 +1232,12 @@ 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;
//observation
const Vector4d& z = feature.observations.find(cam_state_id)->second;
//estimate photometric measurement
std::vector<double> estimate_irradiance;
std::vector<double> estimate_photo_z;
IlluminationParameter estimated_illumination;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
// calculated here, because we need true 'estimate_irradiance' later for jacobi
for (auto& estimate_irradiance_j : estimate_irradiance)
estimate_photo_z.push_back (estimate_irradiance_j *
estimated_illumination.frame_gain * estimated_illumination.feature_gain +
estimated_illumination.frame_bias + estimated_illumination.feature_bias);
int count = 0;
double dx, dy;
@ -1271,19 +1254,17 @@ void MsckfVio::PhotometricMeasurementJacobian(
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
z_irr_est.push_back(feature.PixelIrradiance(p_in_c0, frame));
Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
//calculate photom. residual
photo_r.push_back(photo_z[count] - estimate_photo_z[count]);
// add jacobians
cv::Point2f pixelDistance = feature.pixelDistanceAt(cam_state, cam_state_id, cam0, point);
// frame derivative calculated convoluting with kernel [-1, 0, 1]
dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
dI_dhj(0, 0) = dx;
dI_dhj(0, 1) = dy;
dI_dhj(0, 0) = dx/pixelDistance.x;
dI_dhj(0, 1) = dy/pixelDistance.y;
// Compute the Jacobians.
Matrix<double, 2, 3> dz_dpc0 = Matrix<double, 2, 3>::Zero();
@ -1298,17 +1279,16 @@ void MsckfVio::PhotometricMeasurementJacobian(
dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
dpc0_dxc.rightCols(3) = -R_w_c0;
Matrix3d dpc0_dpg = R_w_c0;
H_xi = dI_dhj*dz_dpc0*dpc0_dxc;
H_fi = dI_dhj*dz_dpc0*dpc0_dpg;
dCpij_dGpij = quaternionToRotation(cam_state.orientation);
Matrix3d dCpij_dGpij = quaternionToRotation(cam_state.orientation);
//orientation takes camera frame to world frame, we wa
dh_dGpij = dz_dpc0 * dCpij_dGpij;
Matrix<double, 2, 3> dh_dGpij = dz_dpc0 * dCpij_dGpij;
double rho = feature.rh
double rho = feature.anchor_rho;
Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
@ -1316,42 +1296,43 @@ 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_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++;
}
// Compute the residual.
std::vector<float> z_irr;
cv::Point2f z = cv::Point2f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1));
feature.PatchAroundPurePixel(z, N, cam0, cam_state_id, z_irr);
// calculate projected irradiance
std::vector<double> projectionPatch;
for(auto point : feature.anchorPatch_3d)
{
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
projectionPatch.push_back(feature.PixelIrradiance(p_in_c0, frame));
}
Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count);
Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count-1);
std::vector<double> residual_v;
for(int i = 0; i < r_l.size(); i++)
r_l(i) = z_irr[i]- feature.anchorPatch[i];
{
residual_v.push_back(projectionPatch[i]- feature.anchorPatch[i]);
r_l(i) = projectionPatch[i] - feature.anchorPatch[i];
}
r = r_l;
H_x = H_xl;
H_y = H_xAl;
//TODO make this more fluent as well
count = 0;
for(auto data : photo_r)
r[count++] = data;
std::stringstream ss;
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
ss << "INFO:"; // << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
if(PRINTIMAGES)
{
feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector);
feature.VisualizePatch(cam_state, cam_state_id, cam0, residual_v, ss);
}
return;
@ -1426,9 +1407,8 @@ void MsckfVio::PhotometricFeatureJacobian(
// 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_xi.block(stack_cntr, 21+cam_state_cntr_anchor*7, H_yl.rows(), H_yl.cols()) = H_yl;
H_xi.block(stack_cntr, 21+cam_state_cntr*7, H_xl.rows(), H_xl.cols()) = -H_xl;
r_i.segment(stack_cntr, N*N) = r_l;
stack_cntr += N*N;
@ -1439,7 +1419,7 @@ void MsckfVio::PhotometricFeatureJacobian(
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl;
myfile << "Hx\n" << H_x << "r\n" << r << "\n x\n" << H_x.colPivHouseholderQr().solve(r) << endl;
myfile.close();
cout << "---------- LOGGED -------- " << endl;
@ -1710,7 +1690,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
}
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return true;
MatrixXd P1 = H * state_server.state_cov * H.transpose();
@ -1819,13 +1798,8 @@ void MsckfVio::removeLostFeatures() {
else
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
cout << "\n" << endl;
cout << "H_xj: \n" << H_xj << endl;
cout << "res: \n" << endl;
cout << r_j << endl;
if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
if (gatingTest(H_xj, r_j, r_j.size()-1)){ //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;
stack_cntr += H_xj.rows();
@ -1986,14 +1960,8 @@ void MsckfVio::pruneCamStateBuffer() {
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
else
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
cout << "\n" << endl;
cout << "H_xj: \n" << H_xj << endl;
cout << "res: \n" << endl;
cout << r_j << endl;
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
if (gatingTest(H_xj, r_j, r_j.size()-1)) { //involved_cam_state_ids.size())) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows();