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

added kernel visualization

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-12 18:39:40 +02:00
parent 44fffa19a2
commit acca4ab018
5 changed files with 157 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
src/msckf_vio.cpp
View File

@@ -1290,6 +1290,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
cv::Point2f residualVector(0,0);
double res_sum = 0;
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log_jacobi.txt");
for (auto point : feature.anchorPatch_3d)
{
//cout << "____feature-measurement_____\n" << endl;
@@ -1316,8 +1320,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
// dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame);
// dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame);
dI_dhj(0, 0) = dx/(pixelDistance.x);
dI_dhj(0, 1) = dy/(pixelDistance.y);
dI_dhj(0, 0) = dx;// /(pixelDistance.x);
dI_dhj(0, 1) = dy;// /(pixelDistance.y);
gradientVector.x += dx;
gradientVector.y += dy;
@@ -1359,6 +1363,16 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
H_plj = dI_dhj * dh_dXplj; // 1 x 6
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
myfile << " --------- \n" << endl;
myfile << "H_rhoj\n" << H_rhoj << endl;
myfile << "H_plj\n" << H_plj << endl;
myfile << "H_pAj\n" << H_pAj << endl;
myfile << "\n" << endl;
myfile << "dI_dhj\n" << dI_dhj << endl;
myfile << "dh_dGpij\n" << dh_dGpij << endl;
myfile << "dGpj_XpAj\n" << dGpj_XpAj << endl;
// myfile << "pixel pos change based on residual:\n" << dI_dhj.colPivHouseholderQr().solve(r_photo(count)) << endl;
H_rho.block<1, 1>(count, 0) = H_rhoj;
H_pl.block<1, 6>(count, 0) = H_plj;
@@ -1367,6 +1381,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
count++;
}
myfile.close();
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
@@ -1393,7 +1408,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N);
// TODO make this calculation more fluent
for(int i = 0; i< N*N; i++)
for(int i = 0; i< N*N; i++)
H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho;
@@ -1408,7 +1423,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
if(PRINTIMAGES)
{
feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
//feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
feature.VisualizeKernel(cam_state, cam_state_id, cam0);
}
return;
@@ -1486,10 +1502,24 @@ void MsckfVio::PhotometricFeatureJacobian(
{
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl;
myfile << "kernel\n" << A_null_space << endl;
myfile << "\nHxi" << endl;
for(int i = 0; i < H_xi.rows(); i++)
myfile << H_xi.block(i, 0, 1, H_xi.cols()) << ";";
myfile << "\nr" << endl;
for(int i = 0; i < r_i.rows(); i++)
myfile << r.segment(i, 1) << ";";
myfile << "\nHyi" << endl;
for(int i = 0; i < H_yi.rows(); i++)
myfile << H_yi.block(i, 0, 1, H_yi.cols()) << ";";
myfile << "A_null_space" << endl;
for(int i = 0; i < A_null_space.rows(); i++)
myfile << A_null_space.block(i, 0, 1, A_null_space.cols()) << ";";
myfile.close();
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
cout << "---------- LOGGED -------- " << endl;
}
if(PRINTIMAGES)