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

added live toggle for photometric

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-25 19:05:53 +02:00
parent 6f7f8b7892
commit 1d6100ed13
5 changed files with 55 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/msckf_vio/feature.hpp
View File

@ -1008,8 +1008,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal
bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
{
//initialize patch Size
int n = (int)(N-1)/2;

10
launch/msckf_vio_euroc.launch
View File

@ -17,6 +17,16 @@
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="false"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="false"/>
<param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>

17
launch/msckf_vio_tinytum.launch
View File

@ -6,7 +6,7 @@
default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
<!-- Image Processor Nodelet -->
<include file="$(find msckf_vio)/launch/image_processor_tum.launch">
<include file="$(find msckf_vio)/launch/image_processor_tinytum.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="calibration_file" value="$(arg calibration_file)"/>
</include>
@ -18,14 +18,14 @@
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/>
<param name="PHOTOMETRIC" value="false"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
<param name="PrintImages" value="true"/>
<param name="StreamPause" value="false"/>
<param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/>
<param name="patch_size_n" value="7"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>
@ -64,16 +64,13 @@
<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="~cam0_image" to="/cam0/image_raw"/>
<remap from="~cam1_image" to="/cam1/image_raw"/>
<remap from="~features" to="image_processor/features"/>
</node>
</group>
<node name="scaler" pkg="msckf_vio" type="shrinkImage.py"/>
<node name="player" pkg="bagcontrol" type="control.py" />
</launch>

4
launch/msckf_vio_tum.launch
View File

@ -18,10 +18,10 @@
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="false"/>
<param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
<param name="StreamPause" value="false"/>
<param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/>

53
src/msckf_vio.cpp
View File

@ -412,6 +412,8 @@ void MsckfVio::imageCallback(
if(STREAMPAUSE)
nh.setParam("/play_bag", false);
nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
// Start the system if the first image is received.
// The frame where the first image is received will be
// the origin.
@ -448,7 +450,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());
@ -457,7 +459,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();
@ -466,7 +468,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);
@ -474,20 +476,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();
pruneCamStateBuffer();
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);
@ -1368,7 +1370,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
}
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);
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1);
// set anchor Jakobi
// get position of anchor in cam states
@ -1389,12 +1393,16 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
// set irradiance error Block
H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N);
//H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N);
// TODO make this calculation more fluent
/*
for(int i = 0; i< N*N; i++)
H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho;
*/
H_yl.block(0, 0, N*N, N*N) = Eigen::MatrixXd::Identity(N*N, N*N);
H_yl.block(0, N*N, N*N, 1) = -H_rho;
H_x = H_xl;
H_y = H_yl;
@ -1439,7 +1447,10 @@ void MsckfVio::PhotometricFeatureJacobian(
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1);
// MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1);
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
@ -1460,6 +1471,8 @@ void MsckfVio::PhotometricFeatureJacobian(
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
r_i.segment(stack_cntr, N*N) = r_l;
stack_cntr += N*N;
}
// Project the residual and Jacobians onto the nullspace
@ -1798,8 +1811,10 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0)
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;
@ -1840,11 +1855,14 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
// 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;
cout << "msc veloci: " << delta_x[6] << ", " << delta_x[7] << ", " << delta_x[8] << endl;
cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
const VectorXd& delta_x_imu = delta_x.head<21>();
if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
@ -1900,6 +1918,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return 1;
MatrixXd P1 = H * state_server.state_cov * H.transpose();
@ -1908,8 +1927,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
//cout << "gate: " << 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;
@ -2008,12 +2027,12 @@ void MsckfVio::removeLostFeatures() {
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;
@ -2180,7 +2199,6 @@ void MsckfVio::pruneLastCamStateBuffer()
feature.observations.erase(cam_id);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@ -2188,7 +2206,6 @@ void MsckfVio::pruneLastCamStateBuffer()
pr.conservativeResize(pstack_cntr);
// Perform measurement update.
measurementUpdate(H_x, r);
photometricMeasurementUpdate(pH_x, pr);