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

added fix to feature projection

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-18 11:03:52 +02:00
parent 1b29047451
commit dcbc69a1c4
3 changed files with 9 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/msckf_vio.cpp
View File

@@ -212,7 +212,7 @@ bool MsckfVio::loadParameters() {
utils::getTransformEigen(nh, "T_imu_body").inverse();
// Maximum number of camera states to be stored
nh.param<int>("max_cam_state_size", max_cam_state_size, 30);
nh.param<int>("max_cam_state_size", max_cam_state_size, 20);
//cam_state_size = max_cam_state_size;
ROS_INFO("===========================================");
ROS_INFO("fixed frame id: %s", fixed_frame_id.c_str());
@@ -1320,16 +1320,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
// dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame);
// dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame);
dI_dhj(0, 0) = dx;// /(pixelDistance.x);
dI_dhj(0, 1) = dy;// /(pixelDistance.y);
gradientVector.x += dx;
gradientVector.y += dy;
residualVector.x += dx * r_photo(count);
residualVector.y += dy * r_photo(count);
res_sum += r_photo(count);
dI_dhj(0, 0) = dx * cam0.intrinsics[0];
dI_dhj(0, 1) = dy * cam0.intrinsics[1];
//dh / d{}^Cp_{ij}
dh_dCpij(0, 0) = 1 / p_c0(2);
@@ -1424,7 +1416,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
{
feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
feature.VisualizeKernel(cam_state, cam_state_id, cam0);
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
}
return;
@@ -1767,7 +1759,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
// Compute the error of the state.
VectorXd delta_x = K * r;
// Update the IMU state.
const VectorXd& delta_x_imu = delta_x.head<21>();
@@ -1823,6 +1814,7 @@ void MsckfVio::measurementUpdate(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();
@@ -2085,7 +2077,7 @@ void MsckfVio::pruneLastCamStateBuffer()
else
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, involved_cam_state_ids.size())) {// 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;
stack_cntr += H_xj.rows();
@@ -2232,7 +2224,7 @@ void MsckfVio::pruneCamStateBuffer() {
else
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, involved_cam_state_ids.size())) {// 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;
stack_cntr += H_xj.rows();