Skip to content

Commit

Permalink
add relocalize: relocalize happens immediately; pose graph optimize e…
Browse files Browse the repository at this point in the history 
…very few seconds

finish multi loop constraints

resample according detection time

make thread safe; fix the fisrt pose of loop constraints
  • Loading branch information
@qintonguav
qintonguav committed Jun 11, 2017
1 parent 6567249 commit d01a808
Show file tree
Hide file tree
Showing 12 changed files with 379 additions and 404 deletions.
2 changes: 1 addition & 1 deletion camera_model/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ catkin_package(
INCLUDE_DIRS include
LIBRARIES camera_model
CATKIN_DEPENDS roscpp std_msgs
DEPENDS system_lib
# DEPENDS system_lib
)

include_directories(
Expand Down
67 changes: 27 additions & 40 deletions config/A3/A3_config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,58 +6,45 @@ image_topic: "/djiros/image"
output_path: "/config/A3/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path

#camera calibration
# model_type: MEI
# camera_name: camera
# image_width: 752
# image_height: 480
# mirror_parameters:
# xi: 2.5826025650890592e+00
# distortion_parameters:
# k1: 3.9389417936108251e-01
# k2: 2.2073276594959599e+00
# p1: -1.9253757033376501e-03
# p2: -6.1503758445210599e-04
# projection_parameters:
# gamma1: 1.3068921176079280e+03
# gamma2: 1.3065892394640514e+03
# u0: 3.6959543908262725e+02
# v0: 2.1891731132428231e+02
model_type: PINHOLE
model_type: MEI
camera_name: camera
image_width: 752
image_height: 480
mirror_parameters:
xi: 2.5826025650890592e+00
distortion_parameters:
k1: -2.727e-01
k2: 6.375e-02
p1: -5.850e-05
p2: -9.779e-05
k1: 3.9389417936108251e-01
k2: 2.2073276594959599e+00
p1: -1.9253757033376501e-03
p2: -6.1503758445210599e-04
projection_parameters:
fx: 3.651e+02
fy: 3.652e+02
cx: 3.695e+02
cy: 2.182e+02
gamma1: 1.3068921176079280e+03
gamma2: 1.3065892394640514e+03
u0: 3.6959543908262725e+02
v0: 2.1891731132428231e+02


# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
ex_calib_result_path: "/config/A3/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
# extrinsicRotation: !!opencv-matrix
# rows: 3
# cols: 3
# dt: d
# data: [ 0, -1, 0,
# 1, 0, 0,
# 0, 0, 1]
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 0, -1, 0,
1, 0, 0,
0, 0, 1]

# #Translation from camera frame to imu frame, imu^T_cam
# extrinsicTranslation: !!opencv-matrix
# rows: 3
# cols: 1
# dt: d
# data: [ 0, 0.05, 0]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [ 0, 0.05, 0]


#feature traker paprameters
Expand Down Expand Up @@ -88,4 +75,4 @@ loop_closure: 1 #if you want to use loop closure to minimize the drift, set lo
#also give the camera calibration file same as feature_tracker node
pattern_file: "/support_files/brief_pattern.yml"
voc_file: "/support_files/brief_k10L6.bin"
min_loop_num: 25
min_loop_num: 30
144 changes: 78 additions & 66 deletions vins_estimator/src/estimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,8 @@ void Estimator::clearState()
solver_flag = INITIAL;
initial_timestamp = 0;
all_image_frame.clear();
relocalize = false;
retrive_data_vector.clear();

if (tmp_pre_integration != nullptr)
delete tmp_pre_integration;
Expand Down Expand Up @@ -566,6 +568,21 @@ void Estimator::double2vector()
dep(i) = para_Feature[i][0];
f_manager.setDepth(dep);

if (LOOP_CLOSURE && relocalize && retrive_data_vector[0].relative_pose)
{

Matrix3d vio_loop_r;
Vector3d vio_loop_t;
vio_loop_r = rot_diff * Quaterniond(retrive_data_vector[0].loop_pose[6], retrive_data_vector[0].loop_pose[3], retrive_data_vector[0].loop_pose[4], retrive_data_vector[0].loop_pose[5]).normalized().toRotationMatrix();
vio_loop_t = rot_diff * Vector3d(retrive_data_vector[0].loop_pose[0] - para_Pose[0][0],
retrive_data_vector[0].loop_pose[1] - para_Pose[0][1],
retrive_data_vector[0].loop_pose[2] - para_Pose[0][2]) + origin_P0;
double relocalize_yaw;
relocalize_yaw = Utility::R2ypr(retrive_data_vector[0].R_old).x() - Utility::R2ypr(vio_loop_r).x();
relocalize_r = Utility::ypr2R(Vector3d(relocalize_yaw, 0, 0));
relocalize_t = retrive_data_vector[0].P_old- relocalize_r * vio_loop_t;
}

}

bool Estimator::failureDetection()
Expand Down Expand Up @@ -688,71 +705,57 @@ void Estimator::optimization()
f_m_cnt++;
}
}
relocalize = false;
//loop close factor
if(LOOP_CLOSURE)
{
//loop close factor
//front_pose.measurements.clear();
if(front_pose.header != retrive_pose_data.header)
{
front_pose = retrive_pose_data;
}
if(!front_pose.measurements.empty())
{
//the retrive pose is in the current window
if(front_pose.header >= Headers[0].stamp.toSec())
int loop_constraint_num = 0;
for (int k = 0; k < (int)retrive_data_vector.size(); k++)
{
for(int i = 0; i < WINDOW_SIZE; i++)
{
//tmp_retrive_pose_buf.push(front_pose);
for(int i = 0; i < WINDOW_SIZE; i++)
if(retrive_data_vector[k].header == Headers[i].stamp.toSec())
{
if(front_pose.header == Headers[i].stamp.toSec())
relocalize = true;
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(retrive_data_vector[k].loop_pose, SIZE_POSE, local_parameterization);
loop_window_index = i;
loop_constraint_num++;
int retrive_feature_index = 0;
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)
{
for (int k = 0; k < 7; k++)
front_pose.loop_pose[k] = para_Pose[i][k];
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(front_pose.loop_pose, SIZE_POSE, local_parameterization);
loop_window_index = i;

int retrive_feature_index = 0;
int feature_index = -1;
int loop_factor_cnt = 0;
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;

++feature_index;
int start = it_per_id.start_frame;
//feature has been obeserved in ith frame
int end = (start + it_per_id.feature_per_frame.size() - i - 1);
if(start <= i && end >=0)
{
while(front_pose.features_ids[retrive_feature_index] < it_per_id.feature_id)
{
retrive_feature_index++;
}

if(front_pose.features_ids[retrive_feature_index] == it_per_id.feature_id)
{
Vector3d pts_j = Vector3d(front_pose.measurements[retrive_feature_index].x, front_pose.measurements[retrive_feature_index].y, 1.0);
Vector3d pts_i = it_per_id.feature_per_frame[0].point;

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[start], front_pose.loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);

retrive_feature_index++;
loop_factor_cnt++;
}

it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;

++feature_index;
int start = it_per_id.start_frame;
if(start <= i)
{
while(retrive_data_vector[k].features_ids[retrive_feature_index] < it_per_id.feature_id)
{
retrive_feature_index++;
}
}

if(retrive_data_vector[k].features_ids[retrive_feature_index] == it_per_id.feature_id)
{
Vector3d pts_j = Vector3d(retrive_data_vector[k].measurements[retrive_feature_index].x, retrive_data_vector[k].measurements[retrive_feature_index].y, 1.0);
Vector3d pts_i = it_per_id.feature_per_frame[0].point;

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[start], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);

retrive_feature_index++;
}
}
}

}
}
}
ROS_DEBUG("loop constraint num: %d", loop_constraint_num);
}

ROS_DEBUG("visual measurement count: %d", f_m_cnt);
ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());

Expand All @@ -776,22 +779,31 @@ void Estimator::optimization()
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
ROS_DEBUG("solver costs: %f", t_solver.toc());

if(LOOP_CLOSURE)
// relative info between two loop frame
if(LOOP_CLOSURE && relocalize)
{
for(int i = 0; i< WINDOW_SIZE; i++)
for (int k = 0; k < (int)retrive_data_vector.size(); k++)
{
if(front_pose.header == Headers[i].stamp.toSec())
for(int i = 0; i< WINDOW_SIZE; i++)
{
Matrix3d Rs_i = Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();
Vector3d Ps_i = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]);
Matrix3d Rs_loop = Quaterniond(front_pose.loop_pose[6], front_pose.loop_pose[3], front_pose.loop_pose[4], front_pose.loop_pose[5]).normalized().toRotationMatrix();
Vector3d Ps_loop = Vector3d( front_pose.loop_pose[0], front_pose.loop_pose[1], front_pose.loop_pose[2]);

front_pose.relative_t = Rs_loop.transpose() * (Ps_i - Ps_loop);
front_pose.relative_q = Rs_loop.transpose() * Rs_i;
front_pose.relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs_i).x() - Utility::R2ypr(Rs_loop).x());
}
}
if(retrive_data_vector[k].header == Headers[i].stamp.toSec())
{
retrive_data_vector[k].relative_pose = true;
Matrix3d Rs_i = Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();
Vector3d Ps_i = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]);
Matrix3d Rs_loop = Quaterniond(retrive_data_vector[k].loop_pose[6], retrive_data_vector[k].loop_pose[3], retrive_data_vector[k].loop_pose[4], retrive_data_vector[k].loop_pose[5]).normalized().toRotationMatrix();
Vector3d Ps_loop = Vector3d( retrive_data_vector[k].loop_pose[0], retrive_data_vector[k].loop_pose[1], retrive_data_vector[k].loop_pose[2]);

retrive_data_vector[k].relative_t = Rs_loop.transpose() * (Ps_i - Ps_loop);
retrive_data_vector[k].relative_q = Rs_loop.transpose() * Rs_i;
retrive_data_vector[k].relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs_i).x() - Utility::R2ypr(Rs_loop).x());
cout << "loop after cere " << Ps_loop.transpose() << endl;
if (abs(retrive_data_vector[k].relative_yaw) > 30.0 || retrive_data_vector[k].relative_t.norm() > 20.0)
retrive_data_vector[k].relative_pose = false;

}
}
}
}

double2vector();
Expand Down
10 changes: 6 additions & 4 deletions vins_estimator/src/estimator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,12 +28,10 @@ struct RetriveData
int cur_index;
double header;
Vector3d P_old;
Quaterniond Q_old;
Vector3d P_cur;
Quaterniond Q_cur;
Matrix3d R_old;
vector<cv::Point2f> measurements;
vector<int> features_ids;
bool use;
bool relative_pose;
Vector3d relative_t;
Quaterniond relative_q;
double relative_yaw;
Expand Down Expand Up @@ -128,7 +126,11 @@ class Estimator
double para_Retrive_Pose[SIZE_POSE];

RetriveData retrive_pose_data, front_pose;
vector<RetriveData> retrive_data_vector;
int loop_window_index;
bool relocalize;
Vector3d relocalize_t;
Matrix3d relocalize_r;

MarginalizationInfo *last_marginalization_info;
vector<double *> last_marginalization_parameter_blocks;
Expand Down
Loading

0 comments on commit d01a808

Please sign in to comment.