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

reformulated irradiance to be: irradiance arround measured feature - irradiance at projection

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-17 15:00:56 +02:00
parent 9c7f67d2fd
commit 05d277c4f4
2 changed files with 91 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
include/msckf_vio/feature.hpp
View File

@ -183,6 +183,17 @@ bool MarkerGeneration(
CameraCalibration& cam0, CameraCalibration& cam0,
const std::vector<double> photo_r, const std::vector<double> photo_r,
std::stringstream& ss) const; std::stringstream& ss) const;
/* @brief takes a pure pixel position (1m from image)
* converts to actual pixel value and returns patch irradiance
* around this pixel
*/
void PatchAroundPurePixel(cv::Point2f p,
int N,
const CameraCalibration& cam,
const StateIDType& cam_state_id,
std::vector<float>& return_i) const;
/* /*
* @brief projectPixelToPosition uses the calcualted pixels * @brief projectPixelToPosition uses the calcualted pixels
* of the anchor patch to generate 3D positions of all of em * of the anchor patch to generate 3D positions of all of em
@ -665,6 +676,25 @@ bool Feature::VisualizePatch(
cvWaitKey(0); cvWaitKey(0);
} }
void Feature::PatchAroundPurePixel(cv::Point2f p,
int N,
const CameraCalibration& cam,
const StateIDType& cam_state_id,
std::vector<float>& return_i) const
{
int n = (int)(N-1)/2;
cv::Mat image = cam.moving_window.find(cam_state_id)->second.image;
cv::Point2f img_p = image_handler::distortPoint(p,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
for(double u_run = -n; u_run <= n; u_run++)
for(double v_run = -n; v_run <= n; v_run++)
return_i.push_back(PixelIrradiance(cv::Point2f(img_p.x+u_run, img_p.y+v_run), image));
}
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
{ {

184
src/msckf_vio.cpp
View File

@ -1229,51 +1229,28 @@ void MsckfVio::PhotometricMeasurementJacobian(
Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Vector3d& t_c0_w = cam_state.position; const Vector3d& t_c0_w = cam_state.position;
// Cam1 pose.
Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
//photometric observation
std::vector<double> photo_z;
// individual Jacobians
Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
Matrix<double, 3, 1> dGpj_drhoj = Matrix<double, 3, 1>::Zero();
Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
Matrix<double, 3, 3> dCpij_dGpij = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
// one line of the NxN Jacobians // one line of the NxN Jacobians
Eigen::Matrix<double, 1, 1> H_rhoj; Matrix<double, 1, 6> H_xi;
Eigen::Matrix<double, 1, 6> H_plj; Matrix<double, 1, 3> H_fi;
Eigen::Matrix<double, 1, 6> H_pAj;
// combined Jacobians MatrixXd H_xl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
Eigen::MatrixXd H_rho(N*N, 1); MatrixXd H_yl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 3);
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; auto frame = cam0.moving_window.find(cam_state_id)->second.image;
int count = 0; int count = 0;
double dx, dy; double dx, dy;
std::vector<float> z_irr_est;
for (auto point : feature.anchorPatch_3d) for (auto point : feature.anchorPatch_3d)
{ {
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); Eigen::Vector3d p_c0 = R_w_c0 * (point-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 z_irr_est.push_back(feature.PixelIrradiance(p_in_c0, frame));
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
// add jacobian Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
// frame derivative calculated convoluting with kernel [-1, 0, 1] // 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); 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);
@ -1281,111 +1258,45 @@ void MsckfVio::PhotometricMeasurementJacobian(
dI_dhj(0, 0) = dx; dI_dhj(0, 0) = dx;
dI_dhj(0, 1) = dy; dI_dhj(0, 1) = dy;
//dh / d{}^Cp_{ij} // Compute the Jacobians.
dh_dCpij(0, 0) = 1 / p_c0(2); Matrix<double, 2, 3> dz_dpc0 = Matrix<double, 2, 3>::Zero();
dh_dCpij(1, 1) = 1 / p_c0(2); dz_dpc0(0, 0) = 1 / p_c0(2);
dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); dz_dpc0(1, 1) = 1 / p_c0(2);
dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*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));
dCpij_dGpij = quaternionToRotation(cam_state.orientation); Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
//orientation takes camera frame to world frame, we wa // original jacobi
dh_dGpij = dh_dCpij * dCpij_dGpij; dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
dpc0_dxc.rightCols(3) = -R_w_c0;
//dh / d X_{pl} Matrix3d dpc0_dpg = R_w_c0;
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 H_xi = dI_dhj*dz_dpc0*dpc0_dxc;
double rho = feature.anchor_rho; H_fi = dI_dhj*dz_dpc0*dpc0_dpg;
// 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() H_xl.block<1, 6>(count, 0) = H_xi;
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), H_yl.block<1, 3>(count, 0) = H_fi;
feature.anchorPatch_ideal[count].y/(rho),
1/(rho)));
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
H_plj = dI_dhj * dh_dXplj; // 1 x 6
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
H_rho.block<1, 1>(count, 0) = H_rhoj;
H_pl.block<1, 6>(count, 0) = H_plj;
H_pA.block<1, 6>(count, 0) = H_pAj;
count++; count++;
} }
// calculate residual
//observation // Compute the residual.
const Vector4d& z = feature.observations.find(cam_state_id)->second; std::vector<float> z_irr;
cv::Point2f z = cv::Point2f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1));
feature.PatchAroundPurePixel(z, N, cam0, cam_state_id, z_irr);
//estimate photometric measurement Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count-1);
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(int i = 0; i < r_l.size(); i++)
for (auto& estimate_irradiance_j : estimate_irradiance) r_l(i) = z_irr[i]- z_irr_est[i];
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]);
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
// set anchor Jakobi
// 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
//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;
// 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;
r = r_l;
H_x = H_xl; H_x = H_xl;
H_y = H_yl; H_y = H_yl;
// 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);
}
return; return;
} }
@ -1455,7 +1366,7 @@ void MsckfVio::PhotometricFeatureJacobian(
state_server.cam_states.begin(), cam_state_iter); state_server.cam_states.begin(), cam_state_iter);
// Stack the Jacobians. // Stack the Jacobians.
H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; H_xi.block(stack_cntr, 21+cam_state_cntr*7, H_xl.rows(), H_xl.cols()) = H_xl;
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
r_i.segment(stack_cntr, N*N) = r_l; r_i.segment(stack_cntr, N*N) = r_l;
stack_cntr += N*N; stack_cntr += N*N;
@ -1500,8 +1411,7 @@ void MsckfVio::PhotometricFeatureJacobian(
void MsckfVio::measurementJacobian( void MsckfVio::measurementJacobian(
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {
{
// Prepare all the required data. // Prepare all the required data.
const CAMState& cam_state = state_server.cam_states[cam_state_id]; const CAMState& cam_state = state_server.cam_states[cam_state_id];
@ -1540,8 +1450,6 @@ void MsckfVio::measurementJacobian(
dz_dpc1(3, 2) = -p_c1(1) / (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(); Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
// original jacobi
dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
dpc0_dxc.rightCols(3) = -R_w_c0; dpc0_dxc.rightCols(3) = -R_w_c0;
@ -1555,6 +1463,17 @@ void MsckfVio::measurementJacobian(
H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;
// Modifty the measurement Jacobian to ensure
// observability constrain.
Matrix<double, 4, 6> A = H_x;
Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();
u.block<3, 1>(0, 0) = quaternionToRotation(
cam_state.orientation_null) * IMUState::gravity;
u.block<3, 1>(3, 0) = skewSymmetric(
p_w-cam_state.position_null) * IMUState::gravity;
H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose();
H_f = -H_x.block<4, 3>(0, 3);
// Compute the residual. // Compute the residual.
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));
@ -1562,6 +1481,7 @@ void MsckfVio::measurementJacobian(
return; return;
} }
void MsckfVio::featureJacobian( void MsckfVio::featureJacobian(
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids, const std::vector<StateIDType>& cam_state_ids,
@ -1853,6 +1773,12 @@ void MsckfVio::removeLostFeatures() {
else else
featureJacobian(feature.id, cam_state_ids, H_xj, r_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
cout << "\n" << endl;
cout << "H_xj: \n" << H_xj << endl;
cout << "res: \n" << endl;
cout << r_j << endl;
if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;
@ -2015,6 +1941,12 @@ void MsckfVio::pruneCamStateBuffer() {
else else
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
cout << "\n" << endl;
cout << "H_xj: \n" << H_xj << endl;
cout << "res: \n" << endl;
cout << r_j << endl;
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;