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

deactivated most to find reason for slowdown

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-17 16:16:45 +02:00
parent 6ae83f0db7
commit f4a17f8512
2 changed files with 123 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
src/msckf_vio.cpp
View File

@@ -344,7 +344,7 @@ void MsckfVio::imageCallback(
// Add new images to moving window
start_time = ros::Time::now();
manageMovingWindow(cam0_img, cam1_img, feature_msg);
//manageMovingWindow(cam0_img, cam1_img, feature_msg);
double manage_moving_window_time = (
ros::Time::now()-start_time).toSec();
@@ -373,20 +373,20 @@ void MsckfVio::imageCallback(
processing_end_time - processing_start_time;
if (processing_time > 1.0/frame_rate) {
++critical_time_cntr;
//ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
// processing_time, critical_time_cntr);
//printf("IMU processing time: %f/%f\n",
// imu_processing_time, imu_processing_time/processing_time);
//printf("State augmentation time: %f/%f\n",
// state_augmentation_time, state_augmentation_time/processing_time);
//printf("Add observations time: %f/%f\n",
// add_observations_time, add_observations_time/processing_time);
//printf("Remove lost features time: %f/%f\n",
// remove_lost_features_time, remove_lost_features_time/processing_time);
//printf("Remove camera states time: %f/%f\n",
// prune_cam_states_time, prune_cam_states_time/processing_time);
//printf("Publish time: %f/%f\n",
// publish_time, publish_time/processing_time);
ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
processing_time, critical_time_cntr);
printf("IMU processing time: %f/%f\n",
imu_processing_time, imu_processing_time/processing_time);
printf("State augmentation time: %f/%f\n",
state_augmentation_time, state_augmentation_time/processing_time);
printf("Add observations time: %f/%f\n",
add_observations_time, add_observations_time/processing_time);
printf("Remove lost features time: %f/%f\n",
remove_lost_features_time, remove_lost_features_time/processing_time);
printf("Remove camera states time: %f/%f\n",
prune_cam_states_time, prune_cam_states_time/processing_time);
printf("Publish time: %f/%f\n",
publish_time, publish_time/processing_time);
}
return;
@@ -400,7 +400,15 @@ void MsckfVio::manageMovingWindow(
//save exposure Time into moving window
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000;
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000;
printf("exposure: %f\n", cam0_moving_window[state_server.imu_state.id].exposureTime_ms);
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 500)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 500;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 500)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 500;
// Get the current image.
cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img,
sensor_msgs::image_encodings::MONO8);
@@ -908,8 +916,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
const Vector4d& z = feature.observations.find(cam_state_id)->second;
//photometric observation
std::vector<uint8_t> photo_z;
feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z);
std::vector<float> photo_z;
//feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z);
// Convert the feature position from the world frame to
// the cam0 and cam1 frame.
@@ -973,6 +981,20 @@ void MsckfVio::PhotometricMeasurementJacobian(
r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
//estimate photometric measurement
std::vector<float> estimate_photo_z;
//feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
std::vector<float> photo_r;
//calculate photom. residual
//for(int i = 0; i < photo_z.size(); i++)
// photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
// printf("-----\n");
// for(int i = 0; i < photo_z.size(); i++)
// printf("%.4f - %.4f\n", photo_z[i], estimate_photo_z[i]);
photo_z.clear();
return;
}
@@ -1002,6 +1024,7 @@ void MsckfVio::PhotometricFeatureJacobian(
VectorXd r_j = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
for (const auto& cam_id : valid_cam_state_ids) {
Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
@@ -1318,12 +1341,11 @@ void MsckfVio::removeLostFeatures() {
continue;
}
}
}
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
invalid_feature_ids.push_back(feature.id);
continue;
/*if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
invalid_feature_ids.push_back(feature.id);
continue;
}*/
}
jacobian_row_size += 4*feature.observations.size() - 3;
@@ -1472,13 +1494,12 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
}
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
continue;
/*if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
continue;
}*/
}
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
@@ -1491,7 +1512,7 @@ void MsckfVio::pruneCamStateBuffer() {
21+6*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
ros::Time start_time = ros::Time::now();
for (auto& item : map_server) {
auto& feature = item.second;
// Check how many camera states to be removed are associated
@@ -1507,8 +1528,8 @@ void MsckfVio::pruneCamStateBuffer() {
MatrixXd H_xj;
VectorXd r_j;
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
if (gatingTest(H_xj, r_j, 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;
@@ -1518,13 +1539,15 @@ void MsckfVio::pruneCamStateBuffer() {
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
double anchorPrune_processing_time = (ros::Time::now()-start_time).toSec();
printf("FeatureJacobian Time: %f\n", anchorPrune_processing_time);
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
// Perform measurement update.
measurementUpdate(H_x, r);
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));