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

removed merge conflicts

Browse Source
This commit is contained in:
Raphael Maenle
2019-05-22 11:34:00 +02:00
parent 976108bffe 2aef2089aa
commit 8ff0e9d816
6 changed files with 234 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
src/msckf_vio.cpp
View File

@ -1238,10 +1238,31 @@ void MsckfVio::PhotometricMeasurementJacobian(
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
//observation
const Vector4d& z = feature.observations.find(cam_state_id)->second;
//estimate photometric measurement
std::vector<double> estimate_irradiance;
std::vector<double> estimate_photo_z;
IlluminationParameter estimated_illumination;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
// calculated here, because we need true 'estimate_irradiance' later for jacobi
for (auto& estimate_irradiance_j : estimate_irradiance)
estimate_photo_z.push_back (estimate_irradiance_j *
estimated_illumination.frame_gain * estimated_illumination.feature_gain +
estimated_illumination.frame_bias + estimated_illumination.feature_bias);
int count = 0;
double dx, dy;
std::vector<float> z_irr_est;
// 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)
{
@ -1251,6 +1272,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
z_irr_est.push_back(feature.PixelIrradiance(p_in_c0, frame));
Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
//calculate photom. residual
photo_r.push_back(photo_z[count] - estimate_photo_z[count]);
// add jacobians
// 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);
@ -1276,6 +1301,41 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_xi = dI_dhj*dz_dpc0*dpc0_dxc;
H_fi = dI_dhj*dz_dpc0*dpc0_dpg;
gradientVector.x += dx;
gradientVector.y += dy;
residualVector.x += dx * photo_r[count];
residualVector.y += dy * photo_r[count];
res_sum += photo_r[count];
//dh / d{}^Cp_{ij}
dh_dCpij(0, 0) = 1 / p_c0(2);
dh_dCpij(1, 1) = 1 / p_c0(2);
dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
dCpij_dGpij = quaternionToRotation(cam_state.orientation);
//orientation takes camera frame to world frame, we wa
dh_dGpij = dh_dCpij * dCpij_dGpij;
//dh / d X_{pl}
dCpij_dCGtheta = skewSymmetric(p_c0);
dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
//d{}^Gp_P{ij} / \rho_i
double rho = feature.anchor_rho;
// Isometry T_anchor_w takes a vector in anchor frame to world frame
dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
feature.anchorPatch_ideal[count].y/(rho),
1/(rho)));
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
H_xl.block<1, 6>(count, 0) = H_xi;
H_yl.block<1, 3>(count, 0) = H_fi;
@ -1296,7 +1356,23 @@ void MsckfVio::PhotometricMeasurementJacobian(
r = r_l;
H_x = H_xl;
H_y = H_yl;
<<<<<<< HEAD
// calculate residual
=======
//TODO make this more fluent as well
count = 0;
for(auto data : photo_r)
r[count++] = data;
std::stringstream ss;
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
if(PRINTIMAGES)
{
feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector);
}
>>>>>>> photometry-jakobi
return;
}
@ -1392,13 +1468,11 @@ void MsckfVio::PhotometricFeatureJacobian(
H_x = A_null_space.transpose() * H_xi;
r = A_null_space.transpose() * r_i;
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl;
myfile.close();
cout << "---------- LOGGED -------- " << endl;
if(PRINTIMAGES)
{
@ -1551,13 +1625,11 @@ void MsckfVio::featureJacobian(
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.ldlt().solve(r) << endl;
myfile.close();
cout << "---------- LOGGED -------- " << endl;
nh.setParam("/play_bag", false);
return;
}
void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
@ -1568,7 +1640,11 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
// complexity as in Equation (28), (29).
MatrixXd H_thin;
VectorXd r_thin;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
/*
if (H.rows() > H.cols()) {
// Convert H to a sparse matrix.
SparseMatrix<double> H_sparse = H.sparseView();
@ -1583,8 +1659,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
(spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
(spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);
H_thin = H_temp.topRows(21+state_server.cam_states.size()*6);
r_thin = r_temp.head(21+state_server.cam_states.size()*6);
H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize);
r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize);
//HouseholderQR<MatrixXd> qr_helper(H);
//MatrixXd Q = qr_helper.householderQ();
@ -1596,18 +1672,19 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
H_thin = H;
r_thin = r;
}
*/
// Compute the Kalman gain.
const MatrixXd& P = state_server.state_cov;
MatrixXd S = H_thin*P*H_thin.transpose() +
MatrixXd S = H*P*H.transpose() +
Feature::observation_noise*MatrixXd::Identity(
H_thin.rows(), H_thin.rows());
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P);
MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
H.rows(), H.rows());
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
MatrixXd K_transpose = S.ldlt().solve(H*P);
MatrixXd K = K_transpose.transpose();
// Compute the error of the state.
VectorXd delta_x = K * r_thin;
VectorXd delta_x = K * r;
// Update the IMU state.
const VectorXd& delta_x_imu = delta_x.head<21>();
@ -1642,7 +1719,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
auto cam_state_iter = state_server.cam_states.begin();
for (int i = 0; i < state_server.cam_states.size();
++i, ++cam_state_iter) {
const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*6);
const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, augmentationSize);
const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
cam_state_iter->second.orientation = quaternionMultiplication(
dq_cam, cam_state_iter->second.orientation);
@ -1650,7 +1727,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
}
// Update state covariance.
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H;
//state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
// K*K.transpose()*Feature::observation_noise;
state_server.state_cov = I_KH*state_server.state_cov;
@ -1664,7 +1741,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
}
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return true;
MatrixXd P1 = H * state_server.state_cov * H.transpose();