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

added visualization with a ros flag, which shows feature with projection and residual (the features apparent movement)

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-30 17:02:22 +02:00
parent 750d8ecfb1
commit cf40ce8afb
4 changed files with 106 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/msckf_vio.cpp
View File

@ -63,7 +63,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
bool MsckfVio::loadParameters() {
//Photometry Flag
nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
nh.param<bool>("PrintImages", PRINTIMAGES, false);
// Frame id
nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
@ -1027,11 +1027,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
//add observation
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
//add jacobian
// add jacobian
// 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);
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;
@ -1043,7 +1043,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//dh / d X_{pl}
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point);
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose();
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation);//.transpose();
//d{}^Gp_P{ij} / \rho_i
double rho = feature.anchor_rho;
@ -1074,6 +1074,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
std::vector<double> estimate_photo_z;
IlluminationParameter estimated_illumination;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
for (auto& estimate_irradiance_j : estimate_irradiance)
estimate_photo_z.push_back (estimate_irradiance_j *
estimated_illumination.frame_gain * estimated_illumination.feature_gain +
@ -1121,6 +1122,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
for(auto data : photo_r)
r[count++] = data;
if(PRINTIMAGES)
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r);
return;
}
@ -1151,15 +1155,8 @@ void MsckfVio::PhotometricFeatureJacobian(
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
// visu - residual
//printf("_____FEATURE:_____\n");
// visu - feature
//cam0.featureVisu.release();
for (const auto& cam_id : valid_cam_state_ids) {
//Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
//Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
MatrixXd H_xl;
MatrixXd H_yl;
Eigen::VectorXd r_l = VectorXd::Zero(N*N);
@ -1176,27 +1173,11 @@ void MsckfVio::PhotometricFeatureJacobian(
r_i.segment(stack_cntr, N*N) = r_l;
stack_cntr += N*N;
}
// visu - feature
/*
if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000)
imshow("feature", cam0.featureVisu);
cvWaitKey(1);
if((ros::Time::now() - image_save_time).toSec() > 1)
{
std::stringstream ss;
ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
imwrite(ss.str(), cam0.featureVisu);
image_save_time = ros::Time::now();
}
*/
// Project the residual and Jacobians onto the nullspace
// of H_yj.
// get Nullspace
JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
int sv_size = 0;
Eigen::VectorXd singularValues = svd_helper.singularValues();
@ -1339,8 +1320,6 @@ void MsckfVio::featureJacobian(
H_x = A.transpose() * H_xj;
r = A.transpose() * r_j;
cout << "A: \n" << A.transpose() << endl;
return;
}
@ -1348,8 +1327,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0) return;
cout << "Updating...";
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;