added kernel visualization
This commit is contained in:
parent
44fffa19a2
commit
acca4ab018
@ -206,6 +206,11 @@ bool MarkerGeneration(
|
||||
ros::Publisher& marker_pub,
|
||||
const CamStateServer& cam_states) const;
|
||||
|
||||
bool VisualizeKernel(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0) const;
|
||||
|
||||
bool VisualizePatch(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
@ -480,16 +485,16 @@ double Feature::Kernel(
|
||||
{
|
||||
Eigen::Matrix<double, 3, 3> kernel = Eigen::Matrix<double, 3, 3>::Zero();
|
||||
if(type == "Sobel_x")
|
||||
kernel << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.;
|
||||
kernel << -3., 0., 3.,-10., 0., 10. , -3., 0., 3.;
|
||||
else if(type == "Sobel_y")
|
||||
kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.;
|
||||
kernel << -3., -10., -3., 0., 0., 0., 3., 10., 3.;
|
||||
|
||||
double delta = 0;
|
||||
int offs = (int)(kernel.rows()-1)/2;
|
||||
|
||||
for(int i = 0; i < kernel.rows(); i++)
|
||||
for(int j = 0; j < kernel.cols(); j++)
|
||||
delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255 * (float)kernel(j,i);
|
||||
delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255. * (float)kernel(j,i);
|
||||
|
||||
return delta;
|
||||
}
|
||||
@ -644,6 +649,41 @@ bool Feature::MarkerGeneration(
|
||||
marker_pub.publish(ma);
|
||||
}
|
||||
|
||||
|
||||
bool Feature::VisualizeKernel(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0) const
|
||||
{
|
||||
auto anchor = observations.begin();
|
||||
cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
|
||||
|
||||
cv::Mat xderImage;
|
||||
cv::Mat yderImage;
|
||||
|
||||
cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3);
|
||||
cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
|
||||
|
||||
|
||||
cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, yderImage.type());
|
||||
cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, yderImage.type());
|
||||
|
||||
cv::imshow("xder", xderImage);
|
||||
//cv::imshow("yder", yderImage);
|
||||
|
||||
for(int i = 1; i < anchorImage.rows-1; i++)
|
||||
for(int j = 1; j < anchorImage.cols-1; j++)
|
||||
xderImage2.at<uint8_t>(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_x");
|
||||
|
||||
for(int i = 1; i < anchorImage.rows-1; i++)
|
||||
for(int j = 1; j < anchorImage.cols-1; j++)
|
||||
yderImage2.at<uint8_t>(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_y");
|
||||
cv::imshow("anchor", anchorImage);
|
||||
cv::imshow("xder2", xderImage2);
|
||||
//cv::imshow("yder2", yderImage2);
|
||||
|
||||
cvWaitKey(0);
|
||||
}
|
||||
bool Feature::VisualizePatch(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
@ -848,7 +888,7 @@ bool Feature::VisualizePatch(
|
||||
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
|
||||
{
|
||||
|
||||
return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
|
||||
return ((float)image.at<uint8_t>(pose.y, pose.x))/255.;
|
||||
}
|
||||
|
||||
cv::Point2f Feature::pixelDistanceAt(
|
||||
|
||||
@ -22,10 +22,10 @@
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="7"/>
|
||||
<param name="patch_size_n" value="3"/>
|
||||
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
@ -21,7 +21,7 @@
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="3"/>
|
||||
|
||||
73
log
Normal file
73
log
Normal file
@ -0,0 +1,73 @@
|
||||
# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>
|
||||
# name: Hx
|
||||
# type: matrix
|
||||
# rows: 18
|
||||
# columns: 49
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 313.795 -58.7912 139.778 -46.7616 144.055 86.9644 0 -314.123 55.6434 -140.648 46.7616 -144.055 -86.9644 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 441.06 -94.50069999999999 174.424 -53.7653 204.822 120.248 0 -441.685 90.1101 -175.657 53.7653 -204.822 -120.248 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 225.35 -54.5629 77.60599999999999 -21.1425 105.886 60.3706 0 -225.756 52.3373 -78.2406 21.1425 -105.886 -60.3706 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 175.128 20.6203 175.127 -79.63939999999999 73.245 62.1868 0 -174.573 -22.5235 -175.576 79.63939999999999 -73.245 -62.1868 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 296.962 43.5469 311.307 -143.667 123.399 108.355 0 -295.905 -46.7952 -312.063 143.667 -123.399 -108.355 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 126.283 117.889 311.864 -161.264 38.8521 71.8019 0 -124.464 -119.541 -312.118 161.264 -38.8521 -71.8019 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 49.2502 63.7166 155.071 -80.81950000000001 12.7732 32.1826 0 -48.2934 -64.4113 -155.157 80.81950000000001 -12.7732 -32.1826 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 69.59699999999999 154.579 335.384 -179.355 9.212580000000001 62.0364 0 -67.35599999999999 -155.735 -335.462 179.355 -9.212580000000001 -62.0364 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -66.6965 304.947 500.218 -285.589 -71.31010000000001 55.5058 0 70.8009 -305.077 -499.831 285.589 71.31010000000001 -55.5058 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 323.404 -62.2043 141.092 -46.6015 148.737 89.1108 0 0 0 0 0 0 0 0 -324.336 57.8552 -141.991 46.6015 -148.737 -89.1108 1 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 454.208 -99.3095 175.986 -53.4094 211.276 123.174 0 0 0 0 0 0 0 0 -455.779 93.0992 -177.158 53.4094 -211.276 -123.174 1 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 231.884 -57.0266 78.2559 -20.8926 109.118 61.8183 0 0 0 0 0 0 0 0 -232.824 53.8025 -78.80719999999999 20.8926 -109.118 -61.8183 1 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 181.715 18.8525 177.045 -80.08499999999999 76.3716 63.8254 0 0 0 0 0 0 0 0 -181.07 -20.839 -177.959 80.08499999999999 -76.3716 -63.8254 1 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 308.249 40.5812 314.711 -144.494 128.766 111.207 0 0 0 0 0 0 0 0 -306.972 -43.8825 -316.328 144.494 -128.766 -111.207 1 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 133.309 116.865 315.474 -162.598 42.0763 73.8353 0 0 0 0 0 0 0 0 -130.603 -117.454 -316.931 162.598 -42.0763 -73.8353 1 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 52.4426 63.3607 156.902 -81.5288 14.2139 33.1222 0 0 0 0 0 0 0 0 -50.9976 -63.4393 -157.607 81.5288 -14.2139 -33.1222 1 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 75.5508 154.234 339.369 -180.995 11.859 63.8956 0 0 0 0 0 0 0 0 -72.1041 -153.816 -340.865 180.995 -11.859 -63.8956 1 0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -62.0551 306.357 506.351 -288.542 -69.50409999999999 57.4562 0 0 0 0 0 0 0 0 68.6262 -303.028 -508.423 288.542 69.50409999999999 -57.4562 1 0 0 0 0 0 0 0
|
||||
|
||||
|
||||
# name: Hy
|
||||
# type: matrix
|
||||
# rows: 18
|
||||
# columns: 14
|
||||
1.56267 0 0 0 0 0 0 0 0 0 0.252873 0 0 0.110387
|
||||
0 1.56267 0 0 0 0 0 0 0 0 0.453168 0 0 0.162961
|
||||
0 0 1.56267 0 0 0 0 0 0 0 0.638441 0 0 0.0873758
|
||||
0 0 0 1.56267 0 0 0 0 0 0 0.21031 0 0 0.0321517
|
||||
0 0 0 0 1.56267 0 0 0 0 0 0.375554 0 0 0.0505151
|
||||
0 0 0 0 0 1.56267 0 0 0 0 0.638441 0 0 -0.0336034
|
||||
0 0 0 0 0 0 1.56267 0 0 0 0.157733 0 0 -0.0232704
|
||||
0 0 0 0 0 0 0 1.56267 0 0 0.212814 0 0 -0.0688343
|
||||
0 0 0 0 0 0 0 0 1.56267 0 0.360532 0 0 -0.187745
|
||||
1.56267 0 0 0 0 0 0 0 0 0 0 0.252873 0 0.322811
|
||||
0 1.56267 0 0 0 0 0 0 0 0 0 0.453168 0 0.467319
|
||||
0 0 1.56267 0 0 0 0 0 0 0 0 0.638441 0 0.245907
|
||||
0 0 0 1.56267 0 0 0 0 0 0 0 0.21031 0 0.135725
|
||||
0 0 0 0 1.56267 0 0 0 0 0 0 0.375554 0 0.225277
|
||||
0 0 0 0 0 1.56267 0 0 0 0 0 0.638441 0 0.012926
|
||||
0 0 0 0 0 0 1.56267 0 0 0 0 0.157733 0 -0.0108962
|
||||
0 0 0 0 0 0 0 1.56267 0 0 0 0.212814 0 -0.06974909999999999
|
||||
0 0 0 0 0 0 0 0 1.56267 0 0 0.360532 0 -0.318846
|
||||
|
||||
|
||||
# name: r
|
||||
# type: matrix
|
||||
# rows: 18
|
||||
# columns: 1
|
||||
0.0354809
|
||||
0.0153183
|
||||
0.0570191
|
||||
-0.0372801
|
||||
0.0878601
|
||||
0.06811780000000001
|
||||
-0.00426164
|
||||
5.162985999041026e-321
|
||||
6.927779999999998e-310
|
||||
0
|
||||
2.121999999910509e-314
|
||||
0
|
||||
0
|
||||
0
|
||||
3.6073900000086e-313
|
||||
4.446590812571219e-323
|
||||
3.952525166729972e-323
|
||||
3.952525166729972e-323
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user