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

added tum launch files, removed anchor procedure being called multiple times through a flag

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-18 11:06:45 +02:00
parent cfecefe29f
commit d91ff7ca9d
6 changed files with 170 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/msckf_vio.cpp
View File

@ -306,7 +306,6 @@ void MsckfVio::imageCallback(
const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg)
{
// Return if the gravity vector has not been set.
if (!is_gravity_set) return;
@ -344,7 +343,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();
@ -398,16 +397,16 @@ void MsckfVio::manageMovingWindow(
const CameraMeasurementConstPtr& feature_msg) {
//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;
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000;
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000;
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;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
// Get the current image.
cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img,
@ -981,6 +980,8 @@ 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));
printf("-----\n");
//estimate photometric measurement
std::vector<float> estimate_photo_z;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
@ -990,9 +991,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
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]);
for(int i = 0; i < photo_z.size(); i++)
printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
photo_z.clear();
return;
@ -1341,6 +1342,9 @@ void MsckfVio::removeLostFeatures() {
continue;
}
}
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
invalid_feature_ids.push_back(feature.id);
@ -1477,7 +1481,6 @@ void MsckfVio::pruneCamStateBuffer() {
feature.observations.erase(involved_cam_state_ids[0]);
continue;
}
if (!feature.is_initialized) {
// Check if the feature can be initialize.
if (!feature.checkMotion(state_server.cam_states)) {
@ -1494,6 +1497,9 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
for (const auto& cam_id : involved_cam_state_ids)
@ -1501,7 +1507,6 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
}
@ -1512,7 +1517,6 @@ 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
@ -1539,8 +1543,6 @@ 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);