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

added two filter, not working yet - compare with htest

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-27 19:22:08 +02:00
parent 9f528c1ea1
commit 715ca6a6b4
4 changed files with 411 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
include/msckf_vio/msckf_vio.h
View File

@ -207,6 +207,11 @@ class MsckfVio {
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
void twodotMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
void PhotometricMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
@ -214,6 +219,11 @@ class MsckfVio {
Eigen::MatrixXd& H_y,
Eigen::VectorXd& r);
void twodotFeatureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
void PhotometricFeatureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
@ -222,6 +232,8 @@ class MsckfVio {
void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
void measurementUpdate(const Eigen::MatrixXd& H,
const Eigen::VectorXd& r);
void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
bool gatingTest(const Eigen::MatrixXd& H,
const Eigen::VectorXd&r, const int& dof);
void removeLostFeatures();
@ -234,7 +246,7 @@ class MsckfVio {
void onlineReset();
// Photometry flag
bool PHOTOMETRIC;
int FILTER;
// debug flag
bool STREAMPAUSE;

5
launch/msckf_vio_euroc.launch
View File

@ -17,9 +17,8 @@
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="false"/>
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="FILTER" value="2"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>

4
launch/msckf_vio_tinytum.launch
View File

@ -17,8 +17,8 @@
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="false"/>
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="FILTER" value="1"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>

406
src/msckf_vio.cpp
View File

@ -67,7 +67,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
bool MsckfVio::loadParameters() {
//Photometry Flag
nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
nh.param<int>("FILTER", FILTER, 0);
nh.param<bool>("PrintImages", PRINTIMAGES, false);
nh.param<bool>("StreamPause", STREAMPAUSE, false);
nh.param<bool>("GroundTruth", GROUNDTRUTH, false);
@ -405,8 +405,7 @@ void MsckfVio::imageCallback(
const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg)
{
cout << "hello" << endl;
// stop playing bagfile if printing images
// stop playing bagfile if printing images
if(STREAMPAUSE)
nh.setParam("/play_bag", false);
// Return if the gravity vector has not been set.
@ -455,7 +454,7 @@ void MsckfVio::imageCallback(
double imu_processing_time = (
ros::Time::now()-start_time).toSec();
//cout << "1" << endl;
cout << "1" << endl;
// Augment the state vector.
start_time = ros::Time::now();
//truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
@ -464,7 +463,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec();
//cout << "2" << endl;
cout << "2" << endl;
// Add new observations for existing features or new
// features in the map server.
start_time = ros::Time::now();
@ -473,7 +472,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec();
//cout << "3" << endl;
cout << "3" << endl;
// Add new images to moving window
start_time = ros::Time::now();
manageMovingWindow(cam0_img, cam1_img, feature_msg);
@ -481,20 +480,20 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec();
//cout << "4" << endl;
cout << "4" << endl;
// Perform measurement update if necessary.
start_time = ros::Time::now();
removeLostFeatures();
double remove_lost_features_time = (
ros::Time::now()-start_time).toSec();
//cout << "5" << endl;
cout << "5" << endl;
start_time = ros::Time::now();
pruneLastCamStateBuffer();
double prune_cam_states_time = (
ros::Time::now()-start_time).toSec();
//cout << "6" << endl;
cout << "6" << endl;
// Publish the odometry.
start_time = ros::Time::now();
publish(feature_msg->header.stamp);
@ -1243,6 +1242,223 @@ void MsckfVio::addFeatureObservations(
return;
}
void MsckfVio::twodotMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r)
{
// Prepare all the required data.
const CAMState& cam_state = state_server.cam_states[cam_state_id];
const Feature& feature = map_server[feature_id];
// Cam0 pose.
Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Vector3d& t_c0_w = cam_state.position;
//photometric observation
std::vector<double> photo_z;
// individual Jacobians
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
Eigen::Matrix<double, 2, 1> H_rho;
Eigen::Matrix<double, 2, 6> H_plj;
Eigen::Matrix<double, 2, 6> H_pAj;
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
int count = 0;
auto point = feature.anchorPatch_3d[0];
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
// add jacobian
//dh / d{}^Cp_{ij}
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
dh_dGpij = dh_dCpij * dCpij_dGpij;
//dh / d X_{pl}
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
double rho = feature.anchor_rho;
// 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));
// alternative derivation towards feature
Matrix3d dCpc0_dpg = R_w_c0;
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
feature.anchorPatch_ideal[count].y/(rho),
1/(rho)));
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
H_rho = dh_dGpij * dGpj_drhoj; // 2 x 1
H_plj = dh_dXplj; // 2 x 6
H_pAj = dh_dGpij * dGpj_XpAj; // 2 x 6
// calculate residual
//observation
const Vector4d& total_z = feature.observations.find(cam_state_id)->second;
const Vector2d z = Vector2d(total_z[0], total_z[1]);
VectorXd r_i = VectorXd::Zero(2);
//calculate residual
r_i[0] = z[0] - p_c0(0)/p_c0(2);
r_i[1] = z[1] - p_c0(1)/p_c0(2);
MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
// 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, 2, 6) = H_pAj;
// 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, 2, 6) = H_plj;
H_x = H_xl;
H_y = H_rho;
r = r_i;
//TODO make this more fluent as well
if(PRINTIMAGES)
{
std::stringstream ss;
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
feature.MarkerGeneration(marker_pub, state_server.cam_states);
//feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
}
return;
}
void MsckfVio::twodotFeatureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
MatrixXd& H_x, VectorXd& r)
{
const auto& feature = map_server[feature_id];
// Check how many camera states in the provided camera
// id camera has actually seen this feature.
vector<StateIDType> valid_cam_state_ids(0);
for (const auto& cam_id : cam_state_ids) {
if (feature.observations.find(cam_id) ==
feature.observations.end()) continue;
if (feature.observations.find(cam_id) ==
feature.observations.begin()) continue;
valid_cam_state_ids.push_back(cam_id);
}
int jacobian_row_size = 0;
jacobian_row_size = 2 * valid_cam_state_ids.size();
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1);
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
for (const auto& cam_id : valid_cam_state_ids) {
MatrixXd H_xl;
MatrixXd H_yl;
Eigen::VectorXd r_l = VectorXd::Zero(2);
twodotMeasurementJacobian(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_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, 2) = r_l;
stack_cntr += 2;
}
// Project the residual and Jacobians onto the nullspace
// of H_yj.
// get Nullspace
FullPivLU<MatrixXd> lu(H_yi.transpose());
MatrixXd A_null_space = lu.kernel();
H_x = A_null_space.transpose() * H_xi;
r = A_null_space.transpose() * r_i;
if(PRINTIMAGES)
{
ofstream myfile;
myfile.open("/home/raphael/dev/octave/log2octave");
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
<< "# name: Hx\n"
<< "# type: matrix\n"
<< "# rows: " << H_xi.rows() << "\n"
<< "# columns: " << H_xi.cols() << "\n"
<< H_xi << endl;
myfile << "# name: Hy\n"
<< "# type: matrix\n"
<< "# rows: " << H_yi.rows() << "\n"
<< "# columns: " << H_yi.cols() << "\n"
<< H_yi << endl;
myfile << "# name: r\n"
<< "# type: matrix\n"
<< "# rows: " << r_i.rows() << "\n"
<< "# columns: " << 1 << "\n"
<< r_i << endl;
myfile.close();
std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true);
}
return;
}
void MsckfVio::PhotometricMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
@ -1431,6 +1647,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
return;
}
void MsckfVio::PhotometricFeatureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
@ -1737,7 +1954,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state.
if(PHOTOMETRIC) return;
if(FILTER != 0) return;
const VectorXd& delta_x_imu = delta_x.head<21>();
@ -1793,6 +2010,108 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
return;
}
void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0)
return;
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;
VectorXd r_thin;
// QR decomposition to make stuff faster
if (H.rows() > H.cols()) {
// Convert H to a sparse matrix.
SparseMatrix<double> H_sparse = H.sparseView();
// Perform QR decompostion on H_sparse.
SPQR<SparseMatrix<double> > spqr_helper;
spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
spqr_helper.compute(H_sparse);
MatrixXd H_temp;
VectorXd r_temp;
(spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
(spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);
H_thin = H_temp.topRows(21+state_server.cam_states.size()*7);
r_thin = r_temp.head(21+state_server.cam_states.size()*7);
} else {
H_thin = H;
r_thin = r;
}
// Compute the Kalman gain.
const MatrixXd& P = state_server.state_cov;
MatrixXd S = H_thin*P*H_thin.transpose() +
Feature::observation_noise*MatrixXd::Identity(
H_thin.rows(), H_thin.rows());
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
MatrixXd K = K_transpose.transpose();
// Compute the error of the state.
VectorXd delta_x = K * r;
cout << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state.
if (FILTER != 2) return;
const VectorXd& delta_x_imu = delta_x.head<21>();
if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
//delta_x_imu.segment<3>(3).norm() > 0.15 ||
delta_x_imu.segment<3>(6).norm() > 0.5 ||
//delta_x_imu.segment<3>(9).norm() > 0.5 ||
delta_x_imu.segment<3>(12).norm() > 1.0) {
printf("delta velocity: %f\n", delta_x_imu.segment<3>(6).norm());
printf("delta position: %f\n", delta_x_imu.segment<3>(12).norm());
ROS_WARN("Update change is too large.");
//return;
}
const Vector4d dq_imu =
smallAngleQuaternion(delta_x_imu.head<3>());
state_server.imu_state.orientation = quaternionMultiplication(
dq_imu, state_server.imu_state.orientation);
state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3);
state_server.imu_state.velocity += delta_x_imu.segment<3>(6);
state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9);
state_server.imu_state.position += delta_x_imu.segment<3>(12);
const Vector4d dq_extrinsic =
smallAngleQuaternion(delta_x_imu.segment<3>(15));
state_server.imu_state.R_imu_cam0 = quaternionToRotation(
dq_extrinsic) * state_server.imu_state.R_imu_cam0;
state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18);
// Update the camera states.
auto cam_state_iter = state_server.cam_states.begin();
for (int i = 0; i < state_server.cam_states.size();
++i, ++cam_state_iter) {
const VectorXd& delta_x_cam = delta_x.segment(21+i*7, 6);
const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
cam_state_iter->second.orientation = quaternionMultiplication(
dq_cam, cam_state_iter->second.orientation);
cam_state_iter->second.position += delta_x_cam.tail<3>();
cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6);
}
// Update state covariance.
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
//state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
// K*K.transpose()*Feature::observation_noise;
state_server.state_cov = I_KH*state_server.state_cov;
// 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::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
@ -1841,7 +2160,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state.
if (not PHOTOMETRIC) return;
if (FILTER != 1) return;
const VectorXd& delta_x_imu = delta_x.head<21>();
@ -1925,6 +2244,7 @@ void MsckfVio::removeLostFeatures() {
// BTW, find the size the final Jacobian matrix and residual vector.
int jacobian_row_size = 0;
int pjacobian_row_size = 0;
int twojacobian_row_size = 0;
vector<FeatureIDType> invalid_feature_ids(0);
vector<FeatureIDType> processed_feature_ids(0);
@ -1965,7 +2285,9 @@ void MsckfVio::removeLostFeatures() {
}
pjacobian_row_size += N*N*feature.observations.size();
twojacobian_row_size += 2*feature.observations.size();
jacobian_row_size += 4*feature.observations.size() - 3;
processed_feature_ids.push_back(feature.id);
}
@ -1974,6 +2296,9 @@ void MsckfVio::removeLostFeatures() {
// processed_feature_ids.size() << endl;
//cout << "jacobian row #: " << jacobian_row_size << endl;
cout << "sizing" << endl;
// Remove the features that do not have enough measurements.
for (const auto& feature_id : invalid_feature_ids)
map_server.erase(feature_id);
@ -1991,6 +2316,12 @@ void MsckfVio::removeLostFeatures() {
VectorXd pr = VectorXd::Zero(pjacobian_row_size);
int pstack_cntr = 0;
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size,
21+7*state_server.cam_states.size());
VectorXd twor = VectorXd::Zero(twojacobian_row_size);
int twostack_cntr = 0;
// Process the features which lose track.
for (const auto& feature_id : processed_feature_ids) {
auto& feature = map_server[feature_id];
@ -2003,9 +2334,17 @@ void MsckfVio::removeLostFeatures() {
VectorXd r_j;
MatrixXd pH_xj;
VectorXd pr_j;
MatrixXd twoH_xj;
VectorXd twor_j;
cout << "measuring" << endl;
PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
cout << "gating" << endl;
if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
@ -2017,20 +2356,29 @@ void MsckfVio::removeLostFeatures() {
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows();
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
// Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time.
//if (stack_cntr > 1500) break;
}
cout << "resizing" << endl;
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
pr.conservativeResize(pstack_cntr);
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
// Perform the measurement update step.
measurementUpdate(H_x, r);
photometricMeasurementUpdate(pH_x, pr);
twoMeasurementUpdate(twoH_x, twor);
// Remove all processed features from the map.
for (const auto& feature_id : processed_feature_ids)
@ -2094,6 +2442,7 @@ void MsckfVio::pruneLastCamStateBuffer()
// Set the size of the Jacobian matrix.
int jacobian_row_size = 0;
int pjacobian_row_size = 0;
int twojacobian_row_size = 0;
//initialize all the features which are going to be removed
@ -2132,6 +2481,7 @@ void MsckfVio::pruneLastCamStateBuffer()
pjacobian_row_size += N*N*feature.observations.size();
jacobian_row_size += 4*feature.observations.size() - 3;
twojacobian_row_size += 2*feature.observations.size();
}
@ -2143,9 +2493,14 @@ void MsckfVio::pruneLastCamStateBuffer()
VectorXd pr_j;
MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd pr = VectorXd::Zero(pjacobian_row_size);
MatrixXd twoH_xj;
VectorXd twor_j;
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd twor = VectorXd::Zero(twojacobian_row_size);
int stack_cntr = 0;
int pruned_cntr = 0;
int pstack_cntr = 0;
int twostack_cntr = 0;
for (auto& item : map_server) {
auto& feature = item.second;
@ -2162,6 +2517,7 @@ void MsckfVio::pruneLastCamStateBuffer()
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
@ -2174,6 +2530,11 @@ void MsckfVio::pruneLastCamStateBuffer()
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows();
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
@ -2186,10 +2547,14 @@ void MsckfVio::pruneLastCamStateBuffer()
pr.conservativeResize(pstack_cntr);
// Perform measurement update.
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
// Perform measurement update.
measurementUpdate(H_x, r);
photometricMeasurementUpdate(pH_x, pr);
twoMeasurementUpdate(twoH_x, twor);
//reduction
int cam_sequence = std::distance(state_server.cam_states.begin(),
state_server.cam_states.find(rm_cam_state_id));
@ -2241,6 +2606,7 @@ void MsckfVio::pruneCamStateBuffer() {
// Find the size of the Jacobian matrix.
int jacobian_row_size = 0;
int pjacobian_row_size = 0;
int twojacobian_row_size = 0;
for (auto& item : map_server) {
auto& feature = item.second;
@ -2285,6 +2651,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
}
twojacobian_row_size += 2*involved_cam_state_ids.size();
pjacobian_row_size += N*N*involved_cam_state_ids.size();
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
}
@ -2302,6 +2669,11 @@ void MsckfVio::pruneCamStateBuffer() {
MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd pr = VectorXd::Zero(pjacobian_row_size);
int pstack_cntr = 0;
MatrixXd twoH_xj;
VectorXd twor_j;
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd twor = VectorXd::Zero(twojacobian_row_size);
int twostack_cntr = 0;
for (auto& item : map_server) {
auto& feature = item.second;
// Check how many camera states to be removed are associated
@ -2317,6 +2689,7 @@ void MsckfVio::pruneCamStateBuffer() {
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
@ -2329,6 +2702,11 @@ void MsckfVio::pruneCamStateBuffer() {
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows();
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
@ -2339,11 +2717,13 @@ void MsckfVio::pruneCamStateBuffer() {
r.conservativeResize(stack_cntr);
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
pr.conservativeResize(pstack_cntr);
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
// Perform measurement update.
measurementUpdate(H_x, r);
photometricMeasurementUpdate(pH_x, pr);
twoMeasurementUpdate(twoH_x, twor);
//reduction
for (const auto& cam_id : rm_cam_state_ids) {
int cam_sequence = std::distance(state_server.cam_states.begin(),