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

19
include/msckf_vio/feature.hpp
View File

@@ -165,7 +165,8 @@ struct Feature {
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
CameraCalibration& cam0, CameraCalibration& cam0,
std::vector<float>& anchorPatch_estimate) const; std::vector<double>& anchorPatch_estimate,
IlluminationParameter& estimatedIllumination) const;
bool VisualizePatch( bool VisualizePatch(
const CAMState& cam_state, const CAMState& cam_state,
@@ -202,6 +203,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
// NxN Patch of Anchor Image // NxN Patch of Anchor Image
std::vector<double> anchorPatch; std::vector<double> anchorPatch;
std::vector<cv::Point2f> anchorPatch_ideal;
// Position of NxN Patch in 3D space // Position of NxN Patch in 3D space
std::vector<Eigen::Vector3d> anchorPatch_3d; std::vector<Eigen::Vector3d> anchorPatch_3d;
@@ -365,7 +367,8 @@ bool Feature::estimate_FrameIrradiance(
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
CameraCalibration& cam0, CameraCalibration& cam0,
std::vector<float>& anchorPatch_estimate) const std::vector<double>& anchorPatch_estimate,
IlluminationParameter& estimated_illumination) const
{ {
// get irradiance of patch in anchor frame // get irradiance of patch in anchor frame
// subtract estimated b and divide by a of anchor frame // subtract estimated b and divide by a of anchor frame
@@ -384,12 +387,16 @@ bool Feature::estimate_FrameIrradiance(
double a_l = frameExposureTime_ms; double a_l = frameExposureTime_ms;
double b_l = 0; double b_l = 0;
estimated_illumination.frame_gain = a_l;
estimated_illumination.frame_bias = b_l;
estimated_illumination.feature_gain = 1;
estimated_illumination.feature_bias = 0;
//printf("frames: %lld, %lld\n", anchor->first, cam_state_id); //printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
//printf("exposure: %f, %f\n", a_A, a_l); //printf("exposure: %f, %f\n", a_A, a_l);
for (double anchorPixel : anchorPatch) for (double anchorPixel : anchorPatch)
{ {
float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l; float irradiance = (anchorPixel - b_A) / a_A ;
anchorPatch_estimate.push_back(irradiance); anchorPatch_estimate.push_back(irradiance);
} }
@@ -536,6 +543,8 @@ bool Feature::initializeAnchor(
// save anchor position for later visualisaztion // save anchor position for later visualisaztion
anchor_center_pos = vec[4]; anchor_center_pos = vec[4];
// save true pixel Patch position
for(auto point : vec) for(auto point : vec)
{ {
if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
@@ -546,8 +555,10 @@ bool Feature::initializeAnchor(
// project patch pixel to 3D space // project patch pixel to 3D space
for(auto point : und_v) for(auto point : und_v)
{
anchorPatch_ideal.push_back(point);
anchorPatch_3d.push_back(projectPixelToPosition(point, cam)); anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
}
is_anchored = true; is_anchored = true;
return true; return true;
} }

121
src/msckf_vio.cpp
View File

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