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

added changed formulation, no positive result

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-16 15:53:07 +02:00
parent 2ee7c248c1
commit 6b208dbc44
4 changed files with 98 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/msckf_vio/msckf_vio.h
View File

@ -195,9 +195,9 @@ class MsckfVio {
// for a single feature observed at a single camera frame. // for a single feature observed at a single camera frame.
void measurementJacobian(const StateIDType& cam_state_id, void measurementJacobian(const StateIDType& cam_state_id,
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
Eigen::Matrix<double, 4, 6>& H_x, Eigen::Matrix<double, 2, 6>& H_x,
Eigen::Matrix<double, 4, 3>& H_f, Eigen::Matrix<double, 2, 3>& H_f,
Eigen::Vector4d& r); Eigen::Vector2d& r);
// This function computes the Jacobian of all measurements viewed // This function computes the Jacobian of all measurements viewed
// in the given camera states of this feature. // in the given camera states of this feature.
void featureJacobian(const FeatureIDType& feature_id, void featureJacobian(const FeatureIDType& feature_id,

2
launch/msckf_vio_debug_tum.launch
View File

@ -22,7 +22,7 @@
<param name="PHOTOMETRIC" value="true"/> <param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="PrintImages" value="false"/> <param name="PrintImages" value="true"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="7"/> <param name="patch_size_n" value="7"/>

4
launch/msckf_vio_tum.launch
View File

@ -18,13 +18,13 @@
output="screen"> output="screen">
<!-- Photometry Flag--> <!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/> <param name="PHOTOMETRIC" value="false"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="PrintImages" value="false"/> <param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="7"/> <param name="patch_size_n" value="1"/>
<!-- Calibration parameters --> <!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/> <rosparam command="load" file="$(arg calibration_file)"/>

227
src/msckf_vio.cpp
View File

@ -454,7 +454,7 @@ void MsckfVio::imageCallback(
PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
} }
else else
stateAugmentation(feature_msg->header.stamp.toSec()); PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
double state_augmentation_time = ( double state_augmentation_time = (
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
@ -1175,7 +1175,7 @@ void MsckfVio::PhotometricStateAugmentation(const double& time)
J * P11 * J.transpose(); J * P11 * J.transpose();
// Add photometry P_eta and surrounding Zeros // Add photometry P_eta and surrounding Zeros
state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias; state_server.state_cov(old_rows+6, old_cols+6) = 0;
// Fix the covariance to be symmetric // Fix the covariance to be symmetric
MatrixXd state_cov_fixed = (state_server.state_cov + MatrixXd state_cov_fixed = (state_server.state_cov +
@ -1242,7 +1242,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
std::vector<double> photo_z; std::vector<double> photo_z;
// individual Jacobians // 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_dCpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 3> dh_dGpij = 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, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
@ -1254,107 +1253,75 @@ void MsckfVio::PhotometricMeasurementJacobian(
Matrix<double, 3, 3> dCpij_dGpC = 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; Eigen::Matrix<double, 2, 1> H_rhoj;
Eigen::Matrix<double, 1, 6> H_plj; Eigen::Matrix<double, 2, 6> H_plj;
Eigen::Matrix<double, 1, 6> H_pAj; Eigen::Matrix<double, 2, 6> H_pAj;
// combined Jacobians
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; auto frame = cam0.moving_window.find(cam_state_id)->second.image;
int count = 0; int count = 0;
double dx, dy; double dx, dy;
for (auto point : feature.anchorPatch_3d) auto point = feature.anchorPatch_3d[0];
{
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);
//add observation Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
// add jacobian // add jacobian
// frame derivative calculated convoluting with kernel [-1, 0, 1] //dh / d{}^Cp_{ij}
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); dh_dCpij(0, 0) = 1 / p_c0(2);
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); dh_dCpij(1, 1) = 1 / p_c0(2);
dI_dhj(0, 0) = dx; dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
dI_dhj(0, 1) = dy; dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
//dh / d{}^Cp_{ij} dCpij_dGpij = quaternionToRotation(cam_state.orientation);
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;
//orientation takes camera frame to world frame, we wa //dh / d X_{pl}
dh_dGpij = dh_dCpij * dCpij_dGpij; 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;
//dh / d X_{pl} //d{}^Gp_P{ij} / \rho_i
dCpij_dCGtheta = skewSymmetric(p_c0); double rho = feature.anchor_rho;
dCpij_dGpC = -quaternionToRotation(cam_state.orientation); // Isometry T_anchor_w takes a vector in anchor frame to world frame
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; 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));
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
//d{}^Gp_P{ij} / \rho_i dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
double rho = feature.anchor_rho; * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
// Isometry T_anchor_w takes a vector in anchor frame to world frame feature.anchorPatch_ideal[count].y/(rho),
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)); 1/(rho)));
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() // Intermediate Jakobians
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), H_rhoj = dh_dGpij * dGpj_drhoj; // 1 x 1
feature.anchorPatch_ideal[count].y/(rho), H_plj = dh_dXplj; // 1 x 6
1/(rho))); H_pAj = dh_dGpij * dGpj_XpAj; // 1 x 6
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++;
}
// calculate residual // calculate residual
//observation //observation
const Vector4d& z = feature.observations.find(cam_state_id)->second; const Vector4d& total_z = feature.observations.find(cam_state_id)->second;
const Vector2d z = Vector2d(total_z[0], total_z[1]);
//estimate photometric measurement VectorXd r_i = VectorXd::Zero(2);
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 //calculate residual
for (auto& estimate_irradiance_j : estimate_irradiance) r_i[0] = z[0] - p_in_c0.x;
estimate_photo_z.push_back (estimate_irradiance_j * r_i[1] = z[1] - p_in_c0.y;
estimated_illumination.frame_gain * estimated_illumination.feature_gain +
estimated_illumination.frame_bias + estimated_illumination.feature_bias);
std::vector<double> photo_r; MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
//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 // set anchor Jakobi
// get position of anchor in cam states // get position of anchor in cam states
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); 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); 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; H_xl.block(0, 21+cam_state_cntr_anchor*7, 2, 6) = H_pAj;
// set frame Jakobi // set frame Jakobi
//get position of current frame in cam states //get position of current frame in cam states
@ -1362,32 +1329,20 @@ void MsckfVio::PhotometricMeasurementJacobian(
int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
// set jakobi of state // set jakobi of state
H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; H_xl.block(0, 21+cam_state_cntr*7, 2, 6) = H_plj;
// 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;
H_x = H_xl; H_x = H_xl;
H_y = H_yl; H_y = H_rhoj;
r = r_i;
cout << "h for patch done" << endl;
//TODO make this more fluent as well //TODO make this more fluent as well
count = 0;
for(auto data : photo_r)
r[count++] = data;
std::stringstream ss; std::stringstream ss;
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
if(PRINTIMAGES) if(PRINTIMAGES)
{ {
feature.MarkerGeneration(marker_pub, state_server.cam_states); feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
} }
return; return;
@ -1437,11 +1392,11 @@ void MsckfVio::PhotometricFeatureJacobian(
} }
int jacobian_row_size = 0; int jacobian_row_size = 0;
jacobian_row_size = N * N * valid_cam_state_ids.size(); jacobian_row_size = 2 * valid_cam_state_ids.size();
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7); 21+state_server.cam_states.size()*7);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1); MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1);
VectorXd r_i = VectorXd::Zero(jacobian_row_size); VectorXd r_i = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0; int stack_cntr = 0;
@ -1449,19 +1404,21 @@ void MsckfVio::PhotometricFeatureJacobian(
MatrixXd H_xl; MatrixXd H_xl;
MatrixXd H_yl; MatrixXd H_yl;
Eigen::VectorXd r_l = VectorXd::Zero(N*N); Eigen::VectorXd r_l = VectorXd::Zero(2);
cout << "getting jacobi" << endl;
PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
cout << "done" << endl;
auto cam_state_iter = state_server.cam_states.find(cam_id); auto cam_state_iter = state_server.cam_states.find(cam_id);
int cam_state_cntr = std::distance( int cam_state_cntr = std::distance(
state_server.cam_states.begin(), cam_state_iter); state_server.cam_states.begin(), cam_state_iter);
// Stack the Jacobians. // Stack the Jacobians.
cout << "stacking" << endl;
H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; 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; 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, 2) = r_l;
stack_cntr += N*N; stack_cntr += 2;
cout << "done" << endl;
} }
// Project the residual and Jacobians onto the nullspace // Project the residual and Jacobians onto the nullspace
@ -1495,7 +1452,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, 2, 6>& H_x, Matrix<double, 2, 3>& H_f, Vector2d& r)
{ {
// Prepare all the required data. // Prepare all the required data.
@ -1514,48 +1471,42 @@ void MsckfVio::measurementJacobian(
// 3d feature position in the world frame. // 3d feature position in the world frame.
// 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;
const Vector4d& z = feature.observations.find(cam_state_id)->second; const Vector2d& z = feature.observations.find(cam_state_id)->second.topRows(2);
// 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.
Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w); //Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w);
// Compute the Jacobians. // Compute the Jacobians.
Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero(); Matrix<double, 2, 3> dz_dpc0 = Matrix<double, 2, 3>::Zero();
dz_dpc0(0, 0) = 1 / p_c0(2); dz_dpc0(0, 0) = 1 / p_c0(2);
dz_dpc0(1, 1) = 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(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
dz_dpc0(1, 2) = -p_c0(1) / (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(); Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
dz_dpc1(2, 0) = 1 / p_c1(2); dz_dpc1(2, 0) = 1 / p_c1(2);
dz_dpc1(3, 1) = 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(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2));
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 // original jacobi
//dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
// my version of calculation
dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
//dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_c0;
dpc0_dxc.rightCols(3) = -R_w_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 dpc0_dpg = R_w_c0;
Matrix3d dpc1_dpg = R_w_c1; Matrix3d dpc1_dpg = R_w_c1;
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;
// Compute the residual. // Compute the residual.
r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), r = z - Vector2d(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));
return; return;
} }
@ -1579,19 +1530,19 @@ void MsckfVio::featureJacobian(
} }
int jacobian_row_size = 0; int jacobian_row_size = 0;
jacobian_row_size = 4 * valid_cam_state_ids.size(); jacobian_row_size = 2 * valid_cam_state_ids.size();
MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size, MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*6); 21+state_server.cam_states.size()*7);
MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3); MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3);
VectorXd r_j = VectorXd::Zero(jacobian_row_size); VectorXd r_j = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0; int stack_cntr = 0;
for (const auto& cam_id : valid_cam_state_ids) { for (const auto& cam_id : valid_cam_state_ids) {
Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero(); Matrix<double, 2, 6> H_xi = Matrix<double, 2, 6>::Zero();
Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero(); Matrix<double, 2, 3> H_fi = Matrix<double, 2, 3>::Zero();
Vector4d r_i = Vector4d::Zero(); Vector2d r_i = Vector2d::Zero();
measurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i); measurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);
auto cam_state_iter = state_server.cam_states.find(cam_id); auto cam_state_iter = state_server.cam_states.find(cam_id);
@ -1599,10 +1550,10 @@ void MsckfVio::featureJacobian(
state_server.cam_states.begin(), cam_state_iter); state_server.cam_states.begin(), cam_state_iter);
// Stack the Jacobians. // Stack the Jacobians.
H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi; H_xj.block<2, 6>(stack_cntr, 21+7*cam_state_cntr) = H_xi;
H_fj.block<4, 3>(stack_cntr, 0) = H_fi; H_fj.block<2, 3>(stack_cntr, 0) = H_fi;
r_j.segment<4>(stack_cntr) = r_i; r_j.segment<2>(stack_cntr) = r_i;
stack_cntr += 4; stack_cntr += 2;
} }
// Project the residual and Jacobians onto the nullspace // Project the residual and Jacobians onto the nullspace
@ -1661,8 +1612,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
(spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
(spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);
H_thin = H_temp.topRows(21+state_server.cam_states.size()*6); H_thin = H_temp.topRows(21+state_server.cam_states.size()*7);
r_thin = r_temp.head(21+state_server.cam_states.size()*6); r_thin = r_temp.head(21+state_server.cam_states.size()*7);
//HouseholderQR<MatrixXd> qr_helper(H); //HouseholderQR<MatrixXd> qr_helper(H);
//MatrixXd Q = qr_helper.householderQ(); //MatrixXd Q = qr_helper.householderQ();
@ -1720,7 +1671,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
auto cam_state_iter = state_server.cam_states.begin(); auto cam_state_iter = state_server.cam_states.begin();
for (int i = 0; i < state_server.cam_states.size(); for (int i = 0; i < state_server.cam_states.size();
++i, ++cam_state_iter) { ++i, ++cam_state_iter) {
const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*6); const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*7);
const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
cam_state_iter->second.orientation = quaternionMultiplication( cam_state_iter->second.orientation = quaternionMultiplication(
dq_cam, cam_state_iter->second.orientation); dq_cam, cam_state_iter->second.orientation);
@ -1812,9 +1763,9 @@ void MsckfVio::removeLostFeatures() {
if(PHOTOMETRIC) if(PHOTOMETRIC)
//just use max. size, as gets shrunken down after anyway //just use max. size, as gets shrunken down after anyway
jacobian_row_size += N*N*feature.observations.size(); jacobian_row_size += 2*feature.observations.size();
else else
jacobian_row_size += 4*feature.observations.size() - 3; jacobian_row_size += 2*feature.observations.size() - 3;
processed_feature_ids.push_back(feature.id); processed_feature_ids.push_back(feature.id);
} }
@ -1831,7 +1782,7 @@ void MsckfVio::removeLostFeatures() {
if (processed_feature_ids.size() == 0) return; if (processed_feature_ids.size() == 0) return;
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
21+augmentationSize*state_server.cam_states.size()); 21+7*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size); VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0; int stack_cntr = 0;
@ -1982,9 +1933,9 @@ void MsckfVio::pruneCamStateBuffer() {
} }
} }
if(PHOTOMETRIC) if(PHOTOMETRIC)
jacobian_row_size += N*N*involved_cam_state_ids.size(); jacobian_row_size += 2*involved_cam_state_ids.size();
else else
jacobian_row_size += 4*involved_cam_state_ids.size() - 3; jacobian_row_size += 2*involved_cam_state_ids.size() - 3;
} }
//cout << "jacobian row #: " << jacobian_row_size << endl; //cout << "jacobian row #: " << jacobian_row_size << endl;
@ -1992,7 +1943,7 @@ void MsckfVio::pruneCamStateBuffer() {
// Compute the Jacobian and residual. // Compute the Jacobian and residual.
MatrixXd H_xj; MatrixXd H_xj;
VectorXd r_j; VectorXd r_j;
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size); VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0; int stack_cntr = 0;
for (auto& item : map_server) { for (auto& item : map_server) {
@ -2037,8 +1988,8 @@ void MsckfVio::pruneCamStateBuffer() {
for (const auto& cam_id : rm_cam_state_ids) { for (const auto& cam_id : rm_cam_state_ids) {
int cam_sequence = std::distance(state_server.cam_states.begin(), int cam_sequence = std::distance(state_server.cam_states.begin(),
state_server.cam_states.find(cam_id)); state_server.cam_states.find(cam_id));
int cam_state_start = 21 + augmentationSize*cam_sequence; int cam_state_start = 21 + 7*cam_sequence;
int cam_state_end = cam_state_start + augmentationSize; int cam_state_end = cam_state_start + 7;
// Remove the corresponding rows and columns in the state // Remove the corresponding rows and columns in the state
@ -2059,10 +2010,10 @@ void MsckfVio::pruneCamStateBuffer() {
state_server.state_cov.cols()-cam_state_end); state_server.state_cov.cols()-cam_state_end);
state_server.state_cov.conservativeResize( state_server.state_cov.conservativeResize(
state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); state_server.state_cov.rows()-7, state_server.state_cov.cols()-7);
} else { } else {
state_server.state_cov.conservativeResize( state_server.state_cov.conservativeResize(
state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); state_server.state_cov.rows()-7, state_server.state_cov.cols()-7);
} }
// Remove this camera state in the state vector. // Remove this camera state in the state vector.