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

added debug launch file, added state augmentation, added jakobi concat; resulting jakobis do not pass gating test

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-24 19:36:38 +02:00
parent 1ffc4fb37f
commit 821d9d6f71
5 changed files with 215 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

223
src/msckf_vio.cpp
View File

@ -117,6 +117,10 @@ bool MsckfVio::loadParameters() {
nh.param<double>("initial_covariance/extrinsic_translation_cov",
extrinsic_translation_cov, 1e-4);
// Get the initial irradiance covariance
nh.param<double>("initial_covariance/irradiance_frame_bias",
irradiance_frame_bias, 0.1);
// get camera information (used for back projection)
nh.param<string>("cam0/distortion_model",
cam0.distortion_model, string("radtan"));
@ -336,7 +340,7 @@ void MsckfVio::imageCallback(
// Augment the state vector.
start_time = ros::Time::now();
stateAugmentation(feature_msg->header.stamp.toSec());
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
double state_augmentation_time = (
ros::Time::now()-start_time).toSec();
@ -511,6 +515,10 @@ bool MsckfVio::resetCallback(
nh.param<double>("initial_covariance/extrinsic_translation_cov",
extrinsic_translation_cov, 1e-4);
// Reset the irradiance covariance
nh.param<double>("initial_covariance/irradiance_frame_bias",
irradiance_frame_bias, 0.1);
state_server.state_cov = MatrixXd::Zero(21, 21);
for (int i = 3; i < 6; ++i)
state_server.state_cov(i, i) = gyro_bias_cov;
@ -820,6 +828,8 @@ void MsckfVio::stateAugmentation(const double& time) {
// Resize the state covariance matrix.
size_t old_rows = state_server.state_cov.rows();
size_t old_cols = state_server.state_cov.cols();
// add 7 for camera state + irradiance bias eta = b_l
state_server.state_cov.conservativeResize(old_rows+6, old_cols+6);
// Rename some matrix blocks for convenience.
@ -839,10 +849,78 @@ void MsckfVio::stateAugmentation(const double& time) {
MatrixXd state_cov_fixed = (state_server.state_cov +
state_server.state_cov.transpose()) / 2.0;
state_server.state_cov = state_cov_fixed;
return;
}
void MsckfVio::PhotometricStateAugmentation(const double& time) {
const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0;
const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu;
// Add a new camera state to the state server.
Matrix3d R_w_i = quaternionToRotation(
state_server.imu_state.orientation);
Matrix3d R_w_c = R_i_c * R_w_i;
Vector3d t_c_w = state_server.imu_state.position +
R_w_i.transpose()*t_c_i;
state_server.cam_states[state_server.imu_state.id] =
CAMState(state_server.imu_state.id);
CAMState& cam_state = state_server.cam_states[
state_server.imu_state.id];
cam_state.time = time;
cam_state.orientation = rotationToQuaternion(R_w_c);
cam_state.position = t_c_w;
cam_state.orientation_null = cam_state.orientation;
cam_state.position_null = cam_state.position;
// Update the covariance matrix of the state.
// To simplify computation, the matrix J below is the nontrivial block
// in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision
// -aided Inertial Navigation".
Matrix<double, 6, 21> J = Matrix<double, 6, 21>::Zero();
J.block<3, 3>(0, 0) = R_i_c;
J.block<3, 3>(0, 15) = Matrix3d::Identity();
J.block<3, 3>(3, 0) = skewSymmetric(R_w_i.transpose()*t_c_i);
//J.block<3, 3>(3, 0) = -R_w_i.transpose()*skewSymmetric(t_c_i);
J.block<3, 3>(3, 12) = Matrix3d::Identity();
J.block<3, 3>(3, 18) = Matrix3d::Identity();
// Resize the state covariance matrix.
size_t old_rows = state_server.state_cov.rows();
size_t old_cols = state_server.state_cov.cols();
// add 7 for camera state + irradiance bias eta = b_l
state_server.state_cov.conservativeResize(old_rows+7, old_cols+7);
// Rename some matrix blocks for convenience.
const Matrix<double, 21, 21>& P11 =
state_server.state_cov.block<21, 21>(0, 0);
const MatrixXd& P12 =
state_server.state_cov.block(0, 21, 21, old_cols-21);
// Fill in the augmented state covariance.
state_server.state_cov.block(old_rows, 0, 6, old_cols) << J*P11, J*P12;
state_server.state_cov.block(0, old_cols, old_rows, 6) =
state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose();
state_server.state_cov.block<6, 6>(old_rows, old_cols) =
J * P11 * J.transpose();
// Add photometry P_eta and surrounding Zeros
state_server.state_cov.block<1, 12>(old_rows+6, 0) = Matrix<double, 1, 12>::Zero();
state_server.state_cov.block<12, 1>(0, old_cols+6) = Matrix<double, 12, 1>::Zero();
state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias;
// Fix the covariance to be symmetric
MatrixXd state_cov_fixed = (state_server.state_cov +
state_server.state_cov.transpose()) / 2.0;
state_server.state_cov = state_cov_fixed;
return;
}
void MsckfVio::addFeatureObservations(
const CameraMeasurementConstPtr& msg) {
@ -879,7 +957,7 @@ void MsckfVio::addFeatureObservations(
void MsckfVio::PhotometricMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) {
// Prepare all the required data.
const CAMState& cam_state = state_server.cam_states[cam_state_id];
@ -890,7 +968,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
const Vector3d& t_c0_w = cam_state.position;
//temp N
const int N = 3;
const int N = 13;
// Cam1 pose.
Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
@ -940,7 +1018,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
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(1, 0) = dy;
dI_dhj(0, 1) = dy;
//dh / d{}^Cp_{ij}
dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix<double, 2, 2>::Identity();
@ -949,15 +1027,15 @@ void MsckfVio::PhotometricMeasurementJacobian(
dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose();
//dh / d X_{pl}
dh_dXplj.block<2, 3>(3, 0) = dh_dCpij * skewSymmetric(point);
dh_dXplj.block<2, 3>(3, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose();
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();
//d{}^Gp_P{ij} / \rho_i
double rho = feature.anchor_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(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>(0, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho)));
dGpi_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
H_rhoj = dI_dhj * dh_dGpij * dGpi_drhoj; // 1 x 3
@ -973,9 +1051,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
// calculate residual
// visu -residual
//printf("-----\n");
//observation
const Vector4d& z = feature.observations.find(cam_state_id)->second;
@ -985,7 +1060,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
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 *
estimate_photo_z.push_back (estimate_irradiance_j *
estimated_illumination.frame_gain * estimated_illumination.feature_gain +
estimated_illumination.frame_bias + estimated_illumination.feature_bias);
@ -995,16 +1070,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
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+state_server.cam_states.size()+1);
@ -1033,46 +1098,14 @@ void MsckfVio::PhotometricMeasurementJacobian(
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
H_x = H_xl;
H_y = H_yl;
// Convert the feature position from the world frame to
// the cam0 and cam1 frame.
Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w);
//TODO make this more fluent as well
count = 0;
for(auto data : photo_r)
r[count++] = data;
// Compute the residual.
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));
// 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));
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;
}
@ -1083,6 +1116,7 @@ void MsckfVio::PhotometricFeatureJacobian(
const auto& feature = map_server[feature_id];
int N = 13;
// Check how many camera states in the provided camera
// id camera has actually seen this feature.
vector<StateIDType> valid_cam_state_ids(0);
@ -1094,36 +1128,38 @@ void MsckfVio::PhotometricFeatureJacobian(
}
int jacobian_row_size = 0;
jacobian_row_size = 4 * valid_cam_state_ids.size();
jacobian_row_size = N * N * valid_cam_state_ids.size();
MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*6);
MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3);
VectorXd r_j = VectorXd::Zero(jacobian_row_size);
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1);
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
// visu - residual
printf("_____FEATURE:_____\n");
//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();
Vector4d r_i = Vector4d::Zero();
PhotometricMeasurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);
//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);
PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
auto cam_state_iter = state_server.cam_states.find(cam_id);
int cam_state_cntr = std::distance(
state_server.cam_states.begin(), cam_state_iter);
// Stack the Jacobians.
H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi;
H_fj.block<4, 3>(stack_cntr, 0) = H_fi;
r_j.segment<4>(stack_cntr) = r_i;
stack_cntr += 4;
H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
r_i.segment(stack_cntr, N*N) = r_l;
stack_cntr += N*N;
}
// visu - feature
/*
@ -1141,13 +1177,14 @@ void MsckfVio::PhotometricFeatureJacobian(
*/
// Project the residual and Jacobians onto the nullspace
// of H_fj.
JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
// of H_yj.
JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
MatrixXd A = svd_helper.matrixU().rightCols(
jacobian_row_size - 3);
H_x = A.transpose() * H_xj;
r = A.transpose() * r_j;
H_x = A.transpose() * H_xi;
r = A.transpose() * r_i;
return;
}
@ -1290,6 +1327,7 @@ void MsckfVio::measurementUpdate(
// complexity as in Equation (28), (29).
MatrixXd H_thin;
VectorXd r_thin;
cout << " measurement update ..." << endl;
if (H.rows() > H.cols()) {
// Convert H to a sparse matrix.
@ -1480,12 +1518,18 @@ void MsckfVio::removeLostFeatures() {
MatrixXd H_xj;
VectorXd r_j;
PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j);
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;
r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows();
cout << "made gating test" << endl;
}
else
{
cout << "failed gating test" << endl;
}
cout << " stacked features up" << endl;
// Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time.
@ -1628,18 +1672,27 @@ void MsckfVio::pruneCamStateBuffer() {
MatrixXd H_xj;
VectorXd r_j;
cout << "getting featureJacobian...";
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
cout << "done" << endl;
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;
r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows();
cout << "made gating test" << endl;
}
else
{
cout << "failed gating test" << endl;
}
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
cout << " stacked features up" << endl;
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@ -1728,6 +1781,10 @@ void MsckfVio::onlineReset() {
nh.param<double>("initial_covariance/extrinsic_translation_cov",
extrinsic_translation_cov, 1e-4);
// Reset the irradiance covariance
nh.param<double>("initial_covariance/irradiance_frame_bias",
irradiance_frame_bias, 0.1);
state_server.state_cov = MatrixXd::Zero(21, 21);
for (int i = 3; i < 6; ++i)
state_server.state_cov(i, i) = gyro_bias_cov;