added scaling for images
This commit is contained in:
@ -451,13 +451,8 @@ void MsckfVio::imageCallback(
|
||||
//cout << "1" << endl;
|
||||
// Augment the state vector.
|
||||
start_time = ros::Time::now();
|
||||
if(PHOTOMETRIC)
|
||||
{
|
||||
truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
|
||||
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
|
||||
}
|
||||
else
|
||||
stateAugmentation(feature_msg->header.stamp.toSec());
|
||||
//truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
|
||||
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
|
||||
double state_augmentation_time = (
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
@ -488,7 +483,7 @@ void MsckfVio::imageCallback(
|
||||
|
||||
//cout << "5" << endl;
|
||||
start_time = ros::Time::now();
|
||||
pruneCamStateBuffer();
|
||||
pruneLastCamStateBuffer();
|
||||
double prune_cam_states_time = (
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
@ -1306,10 +1301,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
cv::Point2f residualVector(0,0);
|
||||
double res_sum = 0;
|
||||
|
||||
|
||||
ofstream myfile;
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log_jacobi.txt");
|
||||
|
||||
for (auto point : feature.anchorPatch_3d)
|
||||
{
|
||||
//cout << "____feature-measurement_____\n" << endl;
|
||||
@ -1324,9 +1315,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
r_photo(count) = photo_z[count] - estimate_photo_z[count];
|
||||
//cout << "residual: " << photo_r.back() << endl;
|
||||
|
||||
// add jacobians
|
||||
cv::Point2f pixelDistance = feature.pixelDistanceAt(anchor_state, anchor_state_id, cam0, point);
|
||||
|
||||
// calculate derivation for anchor frame, use position for derivation calculation
|
||||
// frame derivative calculated convoluting with kernel [-1, 0, 1]
|
||||
|
||||
@ -1371,16 +1359,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
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
|
||||
myfile << " --------- \n" << endl;
|
||||
myfile << "H_rhoj\n" << H_rhoj << endl;
|
||||
myfile << "H_plj\n" << H_plj << endl;
|
||||
myfile << "H_pAj\n" << H_pAj << endl;
|
||||
myfile << "\n" << endl;
|
||||
myfile << "dI_dhj\n" << dI_dhj << endl;
|
||||
myfile << "dh_dGpij\n" << dh_dGpij << endl;
|
||||
myfile << "dGpj_XpAj\n" << dGpj_XpAj << endl;
|
||||
|
||||
// myfile << "pixel pos change based on residual:\n" << dI_dhj.colPivHouseholderQr().solve(r_photo(count)) << endl;
|
||||
|
||||
H_rho.block<1, 1>(count, 0) = H_rhoj;
|
||||
H_pl.block<1, 6>(count, 0) = H_plj;
|
||||
@ -1389,8 +1367,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
count++;
|
||||
}
|
||||
|
||||
myfile.close();
|
||||
|
||||
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);
|
||||
|
||||
@ -1639,7 +1615,7 @@ void MsckfVio::featureJacobian(
|
||||
jacobian_row_size = 4 * valid_cam_state_ids.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);
|
||||
VectorXd r_j = VectorXd::Zero(jacobian_row_size);
|
||||
int stack_cntr = 0;
|
||||
@ -1656,7 +1632,7 @@ void MsckfVio::featureJacobian(
|
||||
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_xj.block<4, 6>(stack_cntr, 21+7*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;
|
||||
@ -1706,10 +1682,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
// complexity as in Equation (28), (29).
|
||||
MatrixXd H_thin;
|
||||
VectorXd r_thin;
|
||||
int augmentationSize = 6;
|
||||
if(PHOTOMETRIC)
|
||||
augmentationSize = 7;
|
||||
|
||||
|
||||
// QR decomposition to make stuff faster
|
||||
if (H.rows() > H.cols()) {
|
||||
// Convert H to a sparse matrix.
|
||||
@ -1725,14 +1698,13 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
(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()*augmentationSize);
|
||||
r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize);
|
||||
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;
|
||||
@ -1740,13 +1712,16 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
Feature::observation_noise*MatrixXd::Identity(
|
||||
H_thin.rows(), H_thin.rows());
|
||||
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
|
||||
cout << "inverting: " << S.rows() << ":" << S.cols() << endl;
|
||||
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 << "update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
|
||||
cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
|
||||
cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
|
||||
// Update the IMU state.
|
||||
|
||||
if(PHOTOMETRIC) return;
|
||||
|
||||
const VectorXd& delta_x_imu = delta_x.head<21>();
|
||||
|
||||
if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
|
||||
@ -1779,13 +1754,12 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
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*augmentationSize, 6);
|
||||
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>();
|
||||
if(PHOTOMETRIC)
|
||||
cam_state_iter->second.illumination.frame_bias += delta_x(21+i*augmentationSize+6);
|
||||
cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6);
|
||||
}
|
||||
|
||||
// Update state covariance.
|
||||
@ -1802,6 +1776,110 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
void MsckfVio::photometricMeasurementUpdate(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 << "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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
|
||||
MatrixXd P1 = H * state_server.state_cov * H.transpose();
|
||||
|
||||
@ -1811,8 +1889,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
|
||||
|
||||
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
|
||||
|
||||
cout << dof << " " << gamma << " " <<
|
||||
chi_squared_test_table[dof] << endl;
|
||||
//cout << "gate: " << dof << " " << gamma << " " <<
|
||||
//chi_squared_test_table[dof] << endl;
|
||||
|
||||
if (chi_squared_test_table[dof] == 0)
|
||||
return false;
|
||||
@ -1829,10 +1907,7 @@ void MsckfVio::removeLostFeatures() {
|
||||
// Remove the features that lost track.
|
||||
// BTW, find the size the final Jacobian matrix and residual vector.
|
||||
int jacobian_row_size = 0;
|
||||
int augmentationSize = 6;
|
||||
if(PHOTOMETRIC)
|
||||
augmentationSize = 7;
|
||||
|
||||
int pjacobian_row_size = 0;
|
||||
|
||||
vector<FeatureIDType> invalid_feature_ids(0);
|
||||
vector<FeatureIDType> processed_feature_ids(0);
|
||||
@ -1872,11 +1947,8 @@ void MsckfVio::removeLostFeatures() {
|
||||
}
|
||||
}
|
||||
|
||||
if(PHOTOMETRIC)
|
||||
//just use max. size, as gets shrunken down after anyway
|
||||
jacobian_row_size += N*N*feature.observations.size();
|
||||
else
|
||||
jacobian_row_size += 4*feature.observations.size() - 3;
|
||||
pjacobian_row_size += N*N*feature.observations.size();
|
||||
jacobian_row_size += 4*feature.observations.size() - 3;
|
||||
processed_feature_ids.push_back(feature.id);
|
||||
}
|
||||
|
||||
@ -1893,10 +1965,15 @@ void MsckfVio::removeLostFeatures() {
|
||||
if (processed_feature_ids.size() == 0) return;
|
||||
|
||||
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);
|
||||
int stack_cntr = 0;
|
||||
|
||||
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;
|
||||
|
||||
// Process the features which lose track.
|
||||
for (const auto& feature_id : processed_feature_ids) {
|
||||
auto& feature = map_server[feature_id];
|
||||
@ -1907,30 +1984,36 @@ void MsckfVio::removeLostFeatures() {
|
||||
|
||||
MatrixXd H_xj;
|
||||
VectorXd r_j;
|
||||
MatrixXd pH_xj;
|
||||
VectorXd pr_j;
|
||||
|
||||
if(PHOTOMETRIC)
|
||||
PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
else
|
||||
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
|
||||
PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
|
||||
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
|
||||
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;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
}
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
|
||||
pstack_cntr += pH_xj.rows();
|
||||
}
|
||||
|
||||
// Put an upper bound on the row size of measurement Jacobian,
|
||||
// which helps guarantee the executation time.
|
||||
if (stack_cntr > 1500) break;
|
||||
//if (stack_cntr > 1500) break;
|
||||
}
|
||||
|
||||
cout << "processed features: " << processed_feature_ids.size() << 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);
|
||||
|
||||
// Perform the measurement update step.
|
||||
measurementUpdate(H_x, r);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
|
||||
// Remove all processed features from the map.
|
||||
for (const auto& feature_id : processed_feature_ids)
|
||||
@ -1988,14 +2071,13 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
|
||||
if (state_server.cam_states.size() < max_cam_state_size)
|
||||
return;
|
||||
|
||||
|
||||
auto rm_cam_state_id = state_server.cam_states.begin()->first;
|
||||
|
||||
// Set the size of the Jacobian matrix.
|
||||
int jacobian_row_size = 0;
|
||||
int augmentationSize = 6;
|
||||
if(PHOTOMETRIC)
|
||||
augmentationSize = 7;
|
||||
int pjacobian_row_size = 0;
|
||||
|
||||
|
||||
//initialize all the features which are going to be removed
|
||||
for (auto& item : map_server) {
|
||||
@ -2031,20 +2113,23 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
}
|
||||
}
|
||||
|
||||
if(PHOTOMETRIC)
|
||||
//just use max. size, as gets shrunken down after anyway
|
||||
jacobian_row_size += N*N*feature.observations.size();
|
||||
else
|
||||
jacobian_row_size += 4*feature.observations.size() - 3;
|
||||
pjacobian_row_size += N*N*feature.observations.size();
|
||||
jacobian_row_size += 4*feature.observations.size() - 3;
|
||||
|
||||
}
|
||||
|
||||
MatrixXd H_xj;
|
||||
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);
|
||||
MatrixXd pH_xj;
|
||||
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);
|
||||
int stack_cntr = 0;
|
||||
int pruned_cntr = 0;
|
||||
int pstack_cntr = 0;
|
||||
|
||||
for (auto& item : map_server) {
|
||||
auto& feature = item.second;
|
||||
|
||||
@ -2058,38 +2143,41 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
for (const auto& cam_state : state_server.cam_states)
|
||||
involved_cam_state_ids.push_back(cam_state.first);
|
||||
|
||||
if(PHOTOMETRIC)
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
else
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
|
||||
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;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
pruned_cntr++;
|
||||
}
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
|
||||
pstack_cntr += pH_xj.rows();
|
||||
}
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
feature.observations.erase(cam_id);
|
||||
}
|
||||
|
||||
if(pruned_cntr != 0)
|
||||
{
|
||||
cout << "pruned features: " << pruned_cntr << endl;
|
||||
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
|
||||
// Perform measurement update.
|
||||
measurementUpdate(H_x, r);
|
||||
}
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
|
||||
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
|
||||
pr.conservativeResize(pstack_cntr);
|
||||
// Perform measurement update.
|
||||
|
||||
|
||||
measurementUpdate(H_x, r);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
|
||||
//reduction
|
||||
int cam_sequence = std::distance(state_server.cam_states.begin(),
|
||||
state_server.cam_states.find(rm_cam_state_id));
|
||||
int cam_state_start = 21 + augmentationSize*cam_sequence;
|
||||
int cam_state_end = cam_state_start + augmentationSize;
|
||||
|
||||
int cam_state_start = 21 + 7*cam_sequence;
|
||||
int cam_state_end = cam_state_start + 7;
|
||||
|
||||
// Remove the corresponding rows and columns in the state
|
||||
// covariance matrix.
|
||||
@ -2109,12 +2197,13 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
state_server.state_cov.cols()-cam_state_end);
|
||||
|
||||
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 {
|
||||
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.
|
||||
state_server.cam_states.erase(rm_cam_state_id);
|
||||
cam0.moving_window.erase(rm_cam_state_id);
|
||||
@ -2125,7 +2214,6 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
|
||||
void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
|
||||
if (state_server.cam_states.size() < max_cam_state_size)
|
||||
return;
|
||||
|
||||
@ -2135,9 +2223,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
// Find the size of the Jacobian matrix.
|
||||
int jacobian_row_size = 0;
|
||||
int augmentationSize = 6;
|
||||
if(PHOTOMETRIC)
|
||||
augmentationSize = 7;
|
||||
int pjacobian_row_size = 0;
|
||||
|
||||
for (auto& item : map_server) {
|
||||
auto& feature = item.second;
|
||||
@ -2181,10 +2267,9 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
if(PHOTOMETRIC)
|
||||
jacobian_row_size += N*N*involved_cam_state_ids.size();
|
||||
else
|
||||
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
|
||||
|
||||
pjacobian_row_size += N*N*involved_cam_state_ids.size();
|
||||
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
|
||||
}
|
||||
|
||||
//cout << "jacobian row #: " << jacobian_row_size << endl;
|
||||
@ -2192,9 +2277,14 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
// Compute the Jacobian and residual.
|
||||
MatrixXd H_xj;
|
||||
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);
|
||||
int stack_cntr = 0;
|
||||
MatrixXd pH_xj;
|
||||
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);
|
||||
int pstack_cntr = 0;
|
||||
for (auto& item : map_server) {
|
||||
auto& feature = item.second;
|
||||
// Check how many camera states to be removed are associated
|
||||
@ -2207,33 +2297,42 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
}
|
||||
|
||||
if (involved_cam_state_ids.size() == 0) continue;
|
||||
|
||||
if(PHOTOMETRIC)
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
else
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
|
||||
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_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;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
}
|
||||
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
|
||||
pstack_cntr += pH_xj.rows();
|
||||
}
|
||||
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
feature.observations.erase(cam_id);
|
||||
}
|
||||
|
||||
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
|
||||
pr.conservativeResize(pstack_cntr);
|
||||
|
||||
// Perform measurement update.
|
||||
measurementUpdate(H_x, r);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
|
||||
//reduction
|
||||
for (const auto& cam_id : rm_cam_state_ids) {
|
||||
int cam_sequence = std::distance(state_server.cam_states.begin(),
|
||||
state_server.cam_states.find(cam_id));
|
||||
int cam_state_start = 21 + augmentationSize*cam_sequence;
|
||||
int cam_state_end = cam_state_start + augmentationSize;
|
||||
int cam_state_start = 21 + 7*cam_sequence;
|
||||
int cam_state_end = cam_state_start + 7;
|
||||
|
||||
|
||||
// Remove the corresponding rows and columns in the state
|
||||
@ -2254,10 +2353,10 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
state_server.state_cov.cols()-cam_state_end);
|
||||
|
||||
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 {
|
||||
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.
|
||||
|
Reference in New Issue
Block a user