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

added kernel calculation visualization; changed sobel filter to cv implementation, added octave export

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-13 16:20:37 +02:00
parent acca4ab018
commit 07f4927128
2 changed files with 98 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
src/msckf_vio.cpp
View File

@ -1314,14 +1314,14 @@ void MsckfVio::PhotometricMeasurementJacobian(
// calculate derivation for anchor frame, use position for derivation calculation
// frame derivative calculated convoluting with kernel [-1, 0, 1]
dx = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_x");
dy = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_y");
dx = feature.cvKernel(p_in_anchor, "Sobel_x");
dy = feature.cvKernel(p_in_anchor, "Sobel_y");
// 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;
@ -1423,8 +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.VisualizeKernel(cam_state, cam_state_id, cam0);
feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
// feature.VisualizeKernel(cam_state, cam_state_id, cam0);
}
return;
@ -1496,7 +1496,7 @@ void MsckfVio::PhotometricFeatureJacobian(
H_x = A_null_space.transpose() * H_xi;
r = A_null_space.transpose() * r_i;
cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
//cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
if(PRINTIMAGES)
{
@ -1519,8 +1519,34 @@ void MsckfVio::PhotometricFeatureJacobian(
myfile << A_null_space.block(i, 0, 1, A_null_space.cols()) << ";";
myfile.close();
//octave
myfile.open("/home/raphael/dev/octave/log2octave");
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
<< "# name: Hx\n"
<< "# type: matrix\n"
<< "# rows: " << H_xi.rows() << "\n"
<< "# columns: " << H_xi.cols() << "\n"
<< H_xi << endl;
myfile << "# name: Hy\n"
<< "# type: matrix\n"
<< "# rows: " << H_yi.rows() << "\n"
<< "# columns: " << H_yi.cols() << "\n"
<< H_yi << endl;
myfile << "# name: r\n"
<< "# type: matrix\n"
<< "# rows: " << r_i.rows() << "\n"
<< "# columns: " << 1 << "\n"
<< r_i << endl;
myfile.close();
cout << "---------- LOGGED -------- " << endl;
cvWaitKey(0);
}
if(PRINTIMAGES)
{
@ -1808,6 +1834,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
cout << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
if (chi_squared_test_table[dof] == 0)
return true;
if (gamma < chi_squared_test_table[dof]) {
//cout << "passed" << endl;
return true;