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

added scaling for images

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-19 18:32:44 +02:00
parent c79fc173b3
commit 02b9e0bc78
7 changed files with 439 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

319
src/msckf_vio.cpp
View File

@@ -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.

75
src/shrinkImage.py Executable file
View File

@@ -0,0 +1,75 @@
#!/usr/bin/env python
from __future__ import print_function
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
class image_converter:
def __init__(self):
self.image0_pub = rospy.Publisher("/cam0/new_image_raw",Image, queue_size=10)
self.image1_pub = rospy.Publisher("/cam1/new_image_raw",Image, queue_size=10)
self.bridge = CvBridge()
self.image0_sub = rospy.Subscriber("/cam0/image_raw",Image,self.callback_cam0)
self.image1_sub = rospy.Subscriber("/cam1/image_raw",Image,self.callback_cam1)
def callback_cam0(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
imgScale = 0.25
newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
newimg = cv2.resize(cv_image,(int(newX),int(newY)))
newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
newdata = data
newdata.height = newpub.height
newdata.width = newpub.width
newdata.step = newpub.step
newdata.data = newpub.data
try:
self.image0_pub.publish(newdata)
except CvBridgeError as e:
print(e)
def callback_cam1(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
imgScale = 0.25
newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
newimg = cv2.resize(cv_image,(int(newX),int(newY)))
newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
newdata = data
newdata.height = newpub.height
newdata.width = newpub.width
newdata.step = newpub.step
newdata.data = newpub.data
try:
self.image1_pub.publish(newdata)
except CvBridgeError as e:
print(e)
def main(args):
ic = image_converter()
rospy.init_node('image_converter', anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)