added ocvis output for multiple filter
This commit is contained in:
parent
3ae7bdb13a
commit
53e2a5d524
@ -7,7 +7,7 @@ cam0:
|
||||
camera_model: pinhole
|
||||
distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
|
||||
-0.001662284667857643]
|
||||
distortion_model: equidistant
|
||||
distortion_model: pre-equidistant
|
||||
intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506]
|
||||
resolution: [1024, 1024]
|
||||
rostopic: /cam0/image_raw
|
||||
@ -25,7 +25,7 @@ cam1:
|
||||
camera_model: pinhole
|
||||
distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
|
||||
-0.002347858896562788]
|
||||
distortion_model: equidistant
|
||||
distortion_model: pre-equidistant
|
||||
intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407]
|
||||
resolution: [1024, 1024]
|
||||
rostopic: /cam1/image_raw
|
||||
|
@ -220,9 +220,7 @@ bool VisualizeKernel(
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0,
|
||||
const Eigen::VectorXd& photo_r,
|
||||
std::stringstream& ss,
|
||||
cv::Point2f gradientVector,
|
||||
cv::Point2f residualVector) const;
|
||||
std::stringstream& ss) const;
|
||||
/*
|
||||
* @brief AnchorPixelToPosition uses the calcualted pixels
|
||||
* of the anchor patch to generate 3D positions of all of em
|
||||
@ -720,9 +718,7 @@ bool Feature::VisualizePatch(
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0,
|
||||
const Eigen::VectorXd& photo_r,
|
||||
std::stringstream& ss,
|
||||
cv::Point2f gradientVector,
|
||||
cv::Point2f residualVector) const
|
||||
std::stringstream& ss) const
|
||||
{
|
||||
|
||||
double rescale = 1;
|
||||
|
@ -18,11 +18,11 @@
|
||||
output="screen">
|
||||
|
||||
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
|
||||
<param name="FILTER" value="2"/>
|
||||
<param name="FILTER" value="0"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="StreamPause" value="true"/>
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="5"/>
|
||||
|
@ -1528,13 +1528,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
int count = 0;
|
||||
double dx, dy;
|
||||
|
||||
// gradient visualization parameters
|
||||
cv::Point2f gradientVector(0,0);
|
||||
|
||||
// residual change visualization
|
||||
cv::Point2f residualVector(0,0);
|
||||
double res_sum = 0;
|
||||
|
||||
for (auto point : feature.anchorPatch_3d)
|
||||
{
|
||||
//cout << "____feature-measurement_____\n" << endl;
|
||||
@ -1633,7 +1626,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
H_x = H_xl;
|
||||
H_y = H_yl;
|
||||
|
||||
//TODO make this more fluent as well
|
||||
r = r_photo;
|
||||
|
||||
std::stringstream ss;
|
||||
@ -1641,7 +1633,7 @@ 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);
|
||||
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
|
||||
}
|
||||
|
||||
@ -1747,13 +1739,18 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
<< "# 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 << "# name: r\n"
|
||||
<< "# type: A\n"
|
||||
<< "# rows: " << A_null_space.rows() << "\n"
|
||||
<< "# columns: " << A_null_space.cols() << "\n"
|
||||
<< A_null_space << endl;
|
||||
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
cvWaitKey(0);
|
||||
@ -1902,7 +1899,34 @@ void MsckfVio::featureJacobian(
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
|
||||
myfile.open("/home/raphael/dev/octave/org2octave");
|
||||
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_xj.rows() << "\n"
|
||||
<< "# columns: " << H_xj.cols() << "\n"
|
||||
<< H_xj << endl;
|
||||
|
||||
myfile << "# name: Hy\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << H_fj.rows() << "\n"
|
||||
<< "# columns: " << H_fj.cols() << "\n"
|
||||
<< H_fj << endl;
|
||||
|
||||
myfile << "# name: r\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << r_j.rows() << "\n"
|
||||
<< "# columns: " << 1 << "\n"
|
||||
<< r_j << endl;
|
||||
|
||||
myfile << "# name: r\n"
|
||||
<< "# type: A\n"
|
||||
<< "# rows: " << A.rows() << "\n"
|
||||
<< "# columns: " << A.cols() << "\n"
|
||||
<< A << endl;
|
||||
|
||||
myfile.close();
|
||||
}
|
||||
|
||||
return;
|
||||
@ -2019,8 +2043,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
// complexity as in Equation (28), (29).
|
||||
MatrixXd H_thin;
|
||||
VectorXd r_thin;
|
||||
|
||||
// QR decomposition to make stuff faster
|
||||
/*
|
||||
if (H.rows() > H.cols()) {
|
||||
// Convert H to a sparse matrix.
|
||||
SparseMatrix<double> H_sparse = H.sparseView();
|
||||
@ -2038,10 +2061,17 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
H_thin = H_temp.topRows(21+state_server.cam_states.size()*7);
|
||||
r_thin = r_temp.head(21+state_server.cam_states.size()*7);
|
||||
|
||||
} else {
|
||||
//HouseholderQR<MatrixXd> qr_helper(H);
|
||||
//MatrixXd Q = qr_helper.householderQ();
|
||||
//MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6);
|
||||
|
||||
//H_thin = Q1.transpose() * H;
|
||||
//r_thin = Q1.transpose() * r;
|
||||
} else {*/
|
||||
H_thin = H;
|
||||
r_thin = r;
|
||||
}
|
||||
//}
|
||||
|
||||
|
||||
|
||||
// Compute the Kalman gain.
|
||||
@ -2518,10 +2548,15 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
for (const auto& cam_state : state_server.cam_states)
|
||||
involved_cam_state_ids.push_back(cam_state.first);
|
||||
|
||||
cout << "measuring" << endl;
|
||||
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
cout << "norm" << endl;
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
cout << "two" << endl;
|
||||
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
|
||||
|
||||
cout << "gating" << endl;
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) {// 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;
|
||||
@ -2689,7 +2724,15 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
}
|
||||
|
||||
if (involved_cam_state_ids.size() == 0) continue;
|
||||
cout << "measuring" << endl;
|
||||
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
cout << "norm" << endl;
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
cout << "two" << endl;
|
||||
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
|
||||
|
||||
cout << "gating" << endl;
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
|
||||
|
Loading…
Reference in New Issue
Block a user