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

36
config/camchain-imucam-tum-scaled.yaml Normal file
View File

@@ -0,0 +1,36 @@
cam0:
T_cam_imu:
[-0.9995378259923383, 0.02917807204183088, -0.008530798463872679, 0.047094247958417004,
0.007526588843243184, -0.03435493139706542, -0.9993813532126198, -0.04788273017221637,
-0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238,
0.0, 0.0, 0.0, 1.0]
camera_model: pinhole
distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
-0.001662284667857643]
distortion_model: equidistant
intrinsics: [95.2026071784, 95.2029854486, 127.573663262, 128.582615763]
resolution: [256, 256]
rostopic: /cam0/image_raw
cam1:
T_cam_imu:
[-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335,
0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889,
-0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645,
0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
[0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335,
0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977,
0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572,
0.0, 0.0, 0.0, 1.0]
camera_model: pinhole
distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
-0.002347858896562788]
distortion_model: equidistant
intrinsics: [94.8217471066, 94.8164593555, 126.391667581, 127.571024044]
resolution: [256, 256]
rostopic: /cam1/image_raw
T_imu_body:
[1.0000, 0.0000, 0.0000, 0.0000,
0.0000, 1.0000, 0.0000, 0.0000,
0.0000, 0.0000, 1.0000, 0.0000,
0.0000, 0.0000, 0.0000, 1.0000]

2
include/msckf_vio/msckf_vio.h
View File

@@ -219,6 +219,7 @@ class MsckfVio {
const std::vector<StateIDType>& cam_state_ids, const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r); Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
void measurementUpdate(const Eigen::MatrixXd& H, void measurementUpdate(const Eigen::MatrixXd& H,
const Eigen::VectorXd& r); const Eigen::VectorXd& r);
bool gatingTest(const Eigen::MatrixXd& H, bool gatingTest(const Eigen::MatrixXd& H,
@@ -252,6 +253,7 @@ class MsckfVio {
// State vector // State vector
StateServer state_server; StateServer state_server;
StateServer photometric_state_server;
// Ground truth state vector // Ground truth state vector
StateServer true_state_server; StateServer true_state_server;

34
launch/image_processor_tinytum.launch Normal file
View File

@@ -0,0 +1,34 @@
<launch>
<arg name="robot" default="firefly_sbx"/>
<arg name="calibration_file"
default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
<!-- Image Processor Nodelet -->
<group ns="$(arg robot)">
<node pkg="nodelet" type="nodelet" name="image_processor"
args="standalone msckf_vio/ImageProcessorNodelet"
output="screen"
>
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="grid_row" value="4"/>
<param name="grid_col" value="4"/>
<param name="grid_min_feature_num" value="3"/>
<param name="grid_max_feature_num" value="4"/>
<param name="pyramid_levels" value="3"/>
<param name="patch_size" value="15"/>
<param name="fast_threshold" value="10"/>
<param name="max_iteration" value="30"/>
<param name="track_precision" value="0.01"/>
<param name="ransac_threshold" value="3"/>
<param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/imu0"/>
<remap from="~cam0_image" to="/cam0/new_image_raw"/>
<remap from="~cam1_image" to="/cam1/new_image_raw"/>
</node>
</group>
</launch>

78
launch/msckf_vio_tinytum.launch Normal file
View File

@@ -0,0 +1,78 @@
<launch>
<arg name="robot" default="firefly_sbx"/>
<arg name="fixed_frame_id" default="world"/>
<arg name="calibration_file"
default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
<!-- Image Processor Nodelet -->
<include file="$(find msckf_vio)/launch/image_processor_tum.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="calibration_file" value="$(arg calibration_file)"/>
</include>
<!-- Msckf Vio Nodelet -->
<group ns="$(arg robot)">
<node pkg="nodelet" type="nodelet" name="vio"
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
<param name="PrintImages" value="true"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="publish_tf" value="true"/>
<param name="frame_rate" value="20"/>
<param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
<param name="child_frame_id" value="odom"/>
<param name="max_cam_state_size" value="20"/>
<param name="position_std_threshold" value="8.0"/>
<param name="rotation_threshold" value="0.2618"/>
<param name="translation_threshold" value="0.4"/>
<param name="tracking_rate_threshold" value="0.5"/>
<!-- Feature optimization config -->
<param name="feature/config/translation_threshold" value="-1.0"/>
<!-- These values should be standard deviation -->
<param name="noise/gyro" value="0.005"/>
<param name="noise/acc" value="0.05"/>
<param name="noise/gyro_bias" value="0.001"/>
<param name="noise/acc_bias" value="0.01"/>
<param name="noise/feature" value="0.035"/>
<param name="initial_state/velocity/x" value="0.0"/>
<param name="initial_state/velocity/y" value="0.0"/>
<param name="initial_state/velocity/z" value="0.0"/>
<!-- These values should be covariance -->
<param name="initial_covariance/velocity" value="0.25"/>
<param name="initial_covariance/gyro_bias" value="0.01"/>
<param name="initial_covariance/acc_bias" value="0.01"/>
<param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
<param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
<param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
<remap from="~imu" to="/imu0"/>
<remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
<remap from="~cam0_image" to="/cam0/new_image_raw"/>
<remap from="~cam1_image" to="/cam1/new_image_raw"/>
<remap from="~features" to="image_processor/features"/>
</node>
</group>
<node name="player" pkg="bagcontrol" type="control.py" />
<node name="scaler" pkg="msckf_vio" type="shrinkImage.py"/>
</launch>

10
launch/msckf_vio_tum.launch
View File

@@ -18,14 +18,14 @@
output="screen"> output="screen">
<!-- Photometry Flag--> <!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="false"/> <param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="StreamPause" value="false"/> <param name="StreamPause" value="true"/>
<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="3"/> <param name="patch_size_n" value="5"/>
<!-- Calibration parameters --> <!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/> <rosparam command="load" file="$(arg calibration_file)"/>
@@ -72,6 +72,6 @@
</node> </node>
</group> </group>
<!--node name="player" pkg="bagcontrol" type="control.py" /--> <node name="player" pkg="bagcontrol" type="control.py" />
</launch> </launch>

303
src/msckf_vio.cpp
View File

@@ -451,13 +451,8 @@ void MsckfVio::imageCallback(
//cout << "1" << endl; //cout << "1" << endl;
// Augment the state vector. // Augment the state vector.
start_time = ros::Time::now(); start_time = ros::Time::now();
if(PHOTOMETRIC) //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
{ PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
}
else
stateAugmentation(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();
@@ -488,7 +483,7 @@ void MsckfVio::imageCallback(
//cout << "5" << endl; //cout << "5" << endl;
start_time = ros::Time::now(); start_time = ros::Time::now();
pruneCamStateBuffer(); pruneLastCamStateBuffer();
double prune_cam_states_time = ( double prune_cam_states_time = (
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
@@ -1306,10 +1301,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
cv::Point2f residualVector(0,0); cv::Point2f residualVector(0,0);
double res_sum = 0; double res_sum = 0;
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log_jacobi.txt");
for (auto point : feature.anchorPatch_3d) for (auto point : feature.anchorPatch_3d)
{ {
//cout << "____feature-measurement_____\n" << endl; //cout << "____feature-measurement_____\n" << endl;
@@ -1324,9 +1315,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
r_photo(count) = photo_z[count] - estimate_photo_z[count]; r_photo(count) = photo_z[count] - estimate_photo_z[count];
//cout << "residual: " << photo_r.back() << endl; //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 // calculate derivation for anchor frame, use position for derivation calculation
// frame derivative calculated convoluting with kernel [-1, 0, 1] // 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_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_plj = dI_dhj * dh_dXplj; // 1 x 6
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 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_rho.block<1, 1>(count, 0) = H_rhoj;
H_pl.block<1, 6>(count, 0) = H_plj; H_pl.block<1, 6>(count, 0) = H_plj;
@@ -1389,8 +1367,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
count++; count++;
} }
myfile.close();
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); 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); 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(); jacobian_row_size = 4 * 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;
@@ -1656,7 +1632,7 @@ 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<4, 6>(stack_cntr, 21+7*cam_state_cntr) = H_xi;
H_fj.block<4, 3>(stack_cntr, 0) = H_fi; H_fj.block<4, 3>(stack_cntr, 0) = H_fi;
r_j.segment<4>(stack_cntr) = r_i; r_j.segment<4>(stack_cntr) = r_i;
stack_cntr += 4; stack_cntr += 4;
@@ -1706,9 +1682,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
// complexity as in Equation (28), (29). // complexity as in Equation (28), (29).
MatrixXd H_thin; MatrixXd H_thin;
VectorXd r_thin; VectorXd r_thin;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
// QR decomposition to make stuff faster // QR decomposition to make stuff faster
if (H.rows() > H.cols()) { if (H.rows() > H.cols()) {
@@ -1725,28 +1698,30 @@ 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()*augmentationSize); H_thin = H_temp.topRows(21+state_server.cam_states.size()*7);
r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize); r_thin = r_temp.head(21+state_server.cam_states.size()*7);
} else { } else {
H_thin = H; H_thin = H;
r_thin = r; r_thin = r;
} }
// Compute the Kalman gain. // Compute the Kalman gain.
const MatrixXd& P = state_server.state_cov; const MatrixXd& P = state_server.state_cov;
MatrixXd S = H_thin*P*H_thin.transpose() + MatrixXd S = H_thin*P*H_thin.transpose() +
Feature::observation_noise*MatrixXd::Identity( Feature::observation_noise*MatrixXd::Identity(
H_thin.rows(), H_thin.rows()); H_thin.rows(), H_thin.rows());
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); //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_transpose = S.ldlt().solve(H_thin*P);
MatrixXd K = K_transpose.transpose(); MatrixXd K = K_transpose.transpose();
// Compute the error of the state. // Compute the error of the state.
VectorXd delta_x = K * r; 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. // Update the IMU state.
if(PHOTOMETRIC) return;
const VectorXd& delta_x_imu = delta_x.head<21>(); const VectorXd& delta_x_imu = delta_x.head<21>();
if (//delta_x_imu.segment<3>(0).norm() > 0.15 || 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(); 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(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>()); 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);
cam_state_iter->second.position += delta_x_cam.tail<3>(); cam_state_iter->second.position += delta_x_cam.tail<3>();
if(PHOTOMETRIC) cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6);
cam_state_iter->second.illumination.frame_bias += delta_x(21+i*augmentationSize+6);
} }
// Update state covariance. // Update state covariance.
@@ -1802,6 +1776,110 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
return; 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) { bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
MatrixXd P1 = H * state_server.state_cov * H.transpose(); 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); double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
cout << dof << " " << gamma << " " << //cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl; //chi_squared_test_table[dof] << endl;
if (chi_squared_test_table[dof] == 0) if (chi_squared_test_table[dof] == 0)
return false; return false;
@@ -1829,10 +1907,7 @@ void MsckfVio::removeLostFeatures() {
// Remove the features that lost track. // Remove the features that lost track.
// BTW, find the size the final Jacobian matrix and residual vector. // BTW, find the size the final Jacobian matrix and residual vector.
int jacobian_row_size = 0; int jacobian_row_size = 0;
int augmentationSize = 6; int pjacobian_row_size = 0;
if(PHOTOMETRIC)
augmentationSize = 7;
vector<FeatureIDType> invalid_feature_ids(0); vector<FeatureIDType> invalid_feature_ids(0);
vector<FeatureIDType> processed_feature_ids(0); vector<FeatureIDType> processed_feature_ids(0);
@@ -1872,11 +1947,8 @@ void MsckfVio::removeLostFeatures() {
} }
} }
if(PHOTOMETRIC) pjacobian_row_size += N*N*feature.observations.size();
//just use max. size, as gets shrunken down after anyway jacobian_row_size += 4*feature.observations.size() - 3;
jacobian_row_size += N*N*feature.observations.size();
else
jacobian_row_size += 4*feature.observations.size() - 3;
processed_feature_ids.push_back(feature.id); processed_feature_ids.push_back(feature.id);
} }
@@ -1893,10 +1965,15 @@ 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;
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. // Process the features which lose track.
for (const auto& feature_id : processed_feature_ids) { for (const auto& feature_id : processed_feature_ids) {
auto& feature = map_server[feature_id]; auto& feature = map_server[feature_id];
@@ -1907,30 +1984,36 @@ void MsckfVio::removeLostFeatures() {
MatrixXd H_xj; MatrixXd H_xj;
VectorXd r_j; VectorXd r_j;
MatrixXd pH_xj;
VectorXd pr_j;
if(PHOTOMETRIC) PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
else
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { 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; H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows(); 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, // Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time. // 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()); H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr); r.conservativeResize(stack_cntr);
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
pr.conservativeResize(pstack_cntr);
// Perform the measurement update step. // Perform the measurement update step.
measurementUpdate(H_x, r); measurementUpdate(H_x, r);
photometricMeasurementUpdate(pH_x, pr);
// Remove all processed features from the map. // Remove all processed features from the map.
for (const auto& feature_id : processed_feature_ids) for (const auto& feature_id : processed_feature_ids)
@@ -1993,9 +2076,8 @@ void MsckfVio::pruneLastCamStateBuffer()
// Set the size of the Jacobian matrix. // Set the size of the Jacobian matrix.
int jacobian_row_size = 0; int jacobian_row_size = 0;
int augmentationSize = 6; int pjacobian_row_size = 0;
if(PHOTOMETRIC)
augmentationSize = 7;
//initialize all the features which are going to be removed //initialize all the features which are going to be removed
for (auto& item : map_server) { for (auto& item : map_server) {
@@ -2031,20 +2113,23 @@ void MsckfVio::pruneLastCamStateBuffer()
} }
} }
if(PHOTOMETRIC) pjacobian_row_size += N*N*feature.observations.size();
//just use max. size, as gets shrunken down after anyway jacobian_row_size += 4*feature.observations.size() - 3;
jacobian_row_size += N*N*feature.observations.size();
else
jacobian_row_size += 4*feature.observations.size() - 3;
} }
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);
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 stack_cntr = 0;
int pruned_cntr = 0; int pruned_cntr = 0;
int pstack_cntr = 0;
for (auto& item : map_server) { for (auto& item : map_server) {
auto& feature = item.second; auto& feature = item.second;
@@ -2058,38 +2143,41 @@ void MsckfVio::pruneLastCamStateBuffer()
for (const auto& cam_state : state_server.cam_states) for (const auto& cam_state : state_server.cam_states)
involved_cam_state_ids.push_back(cam_state.first); involved_cam_state_ids.push_back(cam_state.first);
if(PHOTOMETRIC) PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
else
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_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; H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows(); stack_cntr += H_xj.rows();
pruned_cntr++; 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) for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id); feature.observations.erase(cam_id);
} }
if(pruned_cntr != 0)
{
cout << "pruned features: " << pruned_cntr << endl;
H_x.conservativeResize(stack_cntr, H_x.cols()); H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr); r.conservativeResize(stack_cntr);
// Perform measurement update. pH_x.conservativeResize(pstack_cntr, pH_x.cols());
measurementUpdate(H_x, r); pr.conservativeResize(pstack_cntr);
} // Perform measurement update.
measurementUpdate(H_x, r);
photometricMeasurementUpdate(pH_x, pr);
//reduction //reduction
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(rm_cam_state_id)); state_server.cam_states.find(rm_cam_state_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
// covariance matrix. // covariance matrix.
@@ -2109,12 +2197,13 @@ void MsckfVio::pruneLastCamStateBuffer()
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.
state_server.cam_states.erase(rm_cam_state_id); state_server.cam_states.erase(rm_cam_state_id);
cam0.moving_window.erase(rm_cam_state_id); cam0.moving_window.erase(rm_cam_state_id);
@@ -2125,7 +2214,6 @@ void MsckfVio::pruneLastCamStateBuffer()
void MsckfVio::pruneCamStateBuffer() { void MsckfVio::pruneCamStateBuffer() {
if (state_server.cam_states.size() < max_cam_state_size) if (state_server.cam_states.size() < max_cam_state_size)
return; return;
@@ -2135,9 +2223,7 @@ void MsckfVio::pruneCamStateBuffer() {
// Find the size of the Jacobian matrix. // Find the size of the Jacobian matrix.
int jacobian_row_size = 0; int jacobian_row_size = 0;
int augmentationSize = 6; int pjacobian_row_size = 0;
if(PHOTOMETRIC)
augmentationSize = 7;
for (auto& item : map_server) { for (auto& item : map_server) {
auto& feature = item.second; auto& feature = item.second;
@@ -2181,10 +2267,9 @@ void MsckfVio::pruneCamStateBuffer() {
continue; continue;
} }
} }
if(PHOTOMETRIC)
jacobian_row_size += N*N*involved_cam_state_ids.size(); pjacobian_row_size += N*N*involved_cam_state_ids.size();
else jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
} }
//cout << "jacobian row #: " << jacobian_row_size << endl; //cout << "jacobian row #: " << jacobian_row_size << endl;
@@ -2192,9 +2277,14 @@ 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;
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) { for (auto& item : map_server) {
auto& feature = item.second; auto& feature = item.second;
// Check how many camera states to be removed are associated // Check how many camera states to be removed are associated
@@ -2208,32 +2298,41 @@ void MsckfVio::pruneCamStateBuffer() {
if (involved_cam_state_ids.size() == 0) continue; if (involved_cam_state_ids.size() == 0) continue;
if(PHOTOMETRIC) PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
else
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())) { 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; H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows(); 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) for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id); feature.observations.erase(cam_id);
} }
H_x.conservativeResize(stack_cntr, H_x.cols()); H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr); r.conservativeResize(stack_cntr);
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
pr.conservativeResize(pstack_cntr);
// Perform measurement update. // Perform measurement update.
measurementUpdate(H_x, r); measurementUpdate(H_x, r);
photometricMeasurementUpdate(pH_x, pr);
//reduction //reduction
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
@@ -2254,10 +2353,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.

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)