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

Jakobi Calculation done

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-24 15:30:25 +02:00
parent 5958adb57c
commit 1ffc4fb37f
2 changed files with 105 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

123
src/msckf_vio.cpp
View File

@@ -901,11 +901,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
// And its observation with the stereo cameras.
const Vector3d& p_w = feature.position;
//observation
const Vector4d& z = feature.observations.find(cam_state_id)->second;
//photometric observation
std::vector<float> photo_z;
std::vector<double> photo_z;
// individual Jacobians
Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
@@ -921,17 +918,17 @@ void MsckfVio::PhotometricMeasurementJacobian(
Eigen::Matrix<double, 1, 6> H_pAj;
// combined Jacobians
Eigen::MatrixXd H_rho(N*N, 3);
Eigen::MatrixXd H_rho(N*N, 1);
Eigen::MatrixXd H_pl(N*N, 6);
Eigen::MatrixXd H_pA(N*N, 6);
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
int count = 0;
float dx, dy;
double dx, dy;
for (auto point : feature.anchorPatch_3d)
{
Eigen::Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
//add observation
@@ -947,8 +944,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
//dh / d{}^Cp_{ij}
dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix<double, 2, 2>::Identity();
dh_dCpij(0, 2) = -(point(0))/(point(2)*point(2));
dh_dCpij(1, 2) = -(point(1))/(point(2)*point(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));
dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose();
//dh / d X_{pl}
@@ -957,9 +954,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
//d{}^Gp_P{ij} / \rho_i
double rho = feature.anchor_rho;
dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(p_in_c0.x/(rho*rho), p_in_c0.y/(rho*rho), 1/(rho*rho));
dGpi_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));
dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(p_in_c0.x/(rho), p_in_c0.y/(rho), 1/(rho)));
dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho)));
dGpi_XpAj.block<3, 3>(3, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
@@ -974,25 +971,69 @@ void MsckfVio::PhotometricMeasurementJacobian(
count++;
}
// calculate residual
// visu -residual
//printf("-----\n");
//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);
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);
std::vector<double> photo_r;
//calculate photom. residual
for(int i = 0; i < photo_z.size(); i++)
photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
// visu- residual
//for(int i = 0; i < photo_z.size(); i++)
// printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
//Final Jakobians
// cout << "------------------------" << endl;
// cout << "rho" << H_rho.rows() << "x" << H_rho.cols() << "\n" << H_rho << endl;
// cout << "l" << H_pl.rows() << "x" << H_pl.cols() << "\n" << H_pl << endl;
// cout << "A" << H_pA.rows() << "x" << H_pA.cols() << "\n" << H_pA << endl;
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+2);
auto cam_state_iter = state_server.cam_states.find(feature.observations.begin()->first);
int cam_state_cntr = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_state_id));
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
// set anchor Jakobi
H_xl.block<N*N, 6>(0,21+cam_state_cntr*7) = -H_pA;
//H_yl
cam_state_iter = state_server.cam_states.find(cam_state_id);
cam_state_cntr = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_state_id));
// get position of anchor in cam states
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA;
// set frame Jakobi
H_xl.block(N*N, 6, 0, 21+cam_state_cntr*7) = -H_pl;
//get position of current frame in cam states
auto cam_state_iter = state_server.cam_states.find(cam_state_id);
int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
// set jakobi of state
H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl;
H_xl.block(N*N, 1, 0, 21+cam_state_cntr*7) = Eigen::ArrayXd::Ones(N*N);
// set ones for irradiance bias
H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
// set irradiance error Block
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++)
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;
// Original calculation
// Convert the feature position from the world frame to
// the cam0 and cam1 frame.
@@ -1003,21 +1044,33 @@ void MsckfVio::PhotometricMeasurementJacobian(
r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
// visu -residual
//printf("-----\n");
//estimate photometric measurement
std::vector<float> estimate_photo_z;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_photo_z);
std::vector<float> photo_r;
//calculate photom. residual
for(int i = 0; i < photo_z.size(); i++)
photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
// Compute the Jacobians.
Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
dz_dpc0(0, 0) = 1 / p_c0(2);
dz_dpc0(1, 1) = 1 / p_c0(2);
dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2));
// visu- residual
//for(int i = 0; i < photo_z.size(); i++)
// printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
dz_dpc1(2, 0) = 1 / p_c1(2);
dz_dpc1(3, 1) = 1 / p_c1(2);
dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2));
dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));
Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
dpc0_dxc.rightCols(3) = -R_w_c0;
Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0);
dpc1_dxc.rightCols(3) = -R_w_c1;
Matrix3d dpc0_dpg = R_w_c0;
Matrix3d dpc1_dpg = R_w_c1;
H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;
photo_z.clear();
return;
@@ -1051,7 +1104,7 @@ void MsckfVio::PhotometricFeatureJacobian(
// visu - residual
//printf("_____FEATURE:_____\n");
printf("_____FEATURE:_____\n");
// visu - feature
//cam0.featureVisu.release();