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

added structure for stereo photometry - diverging

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-03 17:48:54 +02:00
parent 6ee756941c
commit 3873c978dd
5 changed files with 161 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/msckf_vio/cam_state.h
View File

@ -41,6 +41,7 @@ struct CameraCalibration{
cv::Vec4d distortion_coeffs;
movingWindow moving_window;
cv::Mat featureVisu;
int id;
};

40
include/msckf_vio/feature.hpp
View File

@ -507,6 +507,7 @@ double Feature::CompleteCvKernel(
delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at<short>(pose.y, pose.x))/255.;
else if (type == "Sobel_y")
delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at<short>(pose.y, pose.x))/255.;
return delta;
}
@ -990,28 +991,39 @@ cv::Point2f Feature::projectPositionToCamera(
cv::Point2f out_p;
cv::Point2f my_p;
// transfrom position to camera frame
// cam0 position
Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Eigen::Vector3d& t_c0_w = cam_state.position;
Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
// if(cam_state_id == observations.begin()->first)
//printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y);
// project point according to model
if(cam.id == 0)
{
Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
}
// if camera is one, calcualte the cam1 position from cam0 position first
else if(cam.id == 1)
{
// cam1 position
Eigen::Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
Eigen::Matrix3d R_w_c1 = R_c0_c1 * R_w_c0;
Eigen::Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
Eigen::Vector3d p_c1 = R_w_c1 * (in_p-t_c1_w);
out_p = cv::Point2f(p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
}
// undistort point according to camera model
if (cam.distortion_model.substr(0,3) == "pre-")
my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]);
else
my_p = image_handler::distortPoint(out_p,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
// printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z());
// printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2));
// printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y);
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
return my_p;
}

8
include/msckf_vio/msckf_vio.h
View File

@ -233,10 +233,10 @@ class MsckfVio {
const Feature& feature,
Eigen::Vector3d point,
int count,
Eigen::Matrix<double, 1, 1>& H_rhoj,
Eigen::Matrix<double, 1, 6>& H_plj,
Eigen::Matrix<double, 1, 6>& H_pAj,
Eigen::Matrix<double, 1, 2>& dI_dhj);
Eigen::Matrix<double, 2, 1>& H_rhoj,
Eigen::Matrix<double, 2, 6>& H_plj,
Eigen::Matrix<double, 2, 6>& H_pAj,
Eigen::Matrix<double, 2, 4>& dI_dhj);
bool PhotometricMeasurementJacobian(
const StateIDType& cam_state_id,

4
launch/msckf_vio_tinytum.launch
View File

@ -18,14 +18,14 @@
output="screen">
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="FILTER" value="2"/>
<param name="FILTER" value="1"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
<param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="1"/>
<param name="patch_size_n" value="5"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>

222
src/msckf_vio.cpp
View File

@ -184,6 +184,8 @@ bool MsckfVio::loadParameters() {
cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
cam0.id = 0;
cam1.id = 1;
state_server.state_cov = MatrixXd::Zero(21, 21);
@ -551,8 +553,8 @@ void MsckfVio::manageMovingWindow(
cv::Mat xder;
cv::Mat yder;
cv::Mat deeper_frame;
cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
cam0_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
xder/=8.;
@ -561,6 +563,15 @@ void MsckfVio::manageMovingWindow(
cam0.moving_window[state_server.imu_state.id].dximage = xder.clone();
cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone();
cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
xder/=8.;
yder/=8.;
cam1.moving_window[state_server.imu_state.id].dximage = xder.clone();
cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone();
//TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
@ -1540,51 +1551,63 @@ bool MsckfVio::PhotometricPatchPointResidual(
VectorXd& r)
{
VectorXd r_photo = VectorXd::Zero(N*N);
VectorXd r_photo = VectorXd::Zero(2*N*N);
int count = 0;
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
const CAMState& cam_state = state_server.cam_states[cam_state_id];
//estimate photometric measurement
std::vector<double> estimate_irradiance;
std::vector<double> estimate_photo_z;
std::vector<double> photo_z;
std::vector<double> photo_z_c0, photo_z_c1;
// estimate irradiance based on anchor frame
/*
IlluminationParameter estimated_illumination;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
for (auto& estimate_irradiance_j : estimate_irradiance)
estimate_photo_z.push_back (estimate_irradiance_j);// *
//estimated_illumination.frame_gain * estimated_illumination.feature_gain +
//estimated_illumination.frame_bias + estimated_illumination.feature_bias);
*/
// irradiance measurement around feature point in c0 and c1
std::vector<double> true_irradiance_c0, true_irradiance_c1;
cv::Point2f p_f_c0(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1));
cv::Point2f p_f_c1(feature.observations.find(cam_state_id)->second(2), feature.observations.find(cam_state_id)->second(3));
// irradiance measurement around feature point
std::vector<double> true_irradiance;
cv::Point2f p_f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1));
p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
for(int i = 0; i<N; i++)
for(int j = 0; j<N ; j++)
true_irradiance.push_back(feature.PixelIrradiance(cv::Point2f(p_f.x + (i-(N-1)/2), p_f.y + (j-(N-1)/2)), current_image));
p_f_c0 = image_handler::distortPoint(p_f_c0, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
p_f_c1 = image_handler::distortPoint(p_f_c1, cam1.intrinsics, cam1.distortion_model, cam1.distortion_coeffs);
cv::Mat current_image_c0 = cam0.moving_window.find(cam_state_id)->second.image;
cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image;
for(int i = 0; i<N; i++) {
for(int j = 0; j<N ; j++) {
true_irradiance_c0.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c0.x + (i-(N-1)/2), p_f_c0.y + (j-(N-1)/2)), current_image_c0));
true_irradiance_c1.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c1.x + (i-(N-1)/2), p_f_c1.y + (j-(N-1)/2)), current_image_c1));
}
}
// get residual
for(auto point : feature.anchorPatch_3d)
{
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
cv::Point2f p_in_c1 = feature.projectPositionToCamera(cam_state, cam_state_id, cam1, point);
// test if projection is inside frame
if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1)
if(p_in_c0.x < 0 or p_in_c0.x > current_image_c0.cols-1 or p_in_c0.y < 0 or p_in_c0.y > current_image_c0.rows-1)
return false;
if(p_in_c1.x < 0 or p_in_c1.x > current_image_c1.cols-1 or p_in_c1.y < 0 or p_in_c1.y > current_image_c1.rows-1)
return false;
// add observation
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
// add observation
photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0));
photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1));
// calculate photom. residual acc. to paper
// r_photo(count) = photo_z[count] - estimate_photo_z[count];
// calculate alternate photom. residual
r_photo(count) = true_irradiance[count] - photo_z[count];
// calculate photom. residual alternating between frames
r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count];
r_photo(count*2+1) = true_irradiance_c1[count] - photo_z_c1[count];
count++;
}
r = r_photo;
@ -1598,10 +1621,10 @@ bool MsckfVio::PhotometricPatchPointJacobian(
const Feature& feature,
Eigen::Vector3d point,
int count,
Eigen::Matrix<double, 1, 1>& H_rhoj,
Eigen::Matrix<double, 1, 6>& H_plj,
Eigen::Matrix<double, 1, 6>& H_pAj,
Matrix<double, 1, 2>& dI_dhj)
Eigen::Matrix<double, 2, 1>& H_rhoj,
Eigen::Matrix<double, 2, 6>& H_plj,
Eigen::Matrix<double, 2, 6>& H_pAj,
Matrix<double, 2, 4>& dI_dhj)
{
const StateIDType anchor_state_id = feature.observations.begin()->first;
@ -1618,67 +1641,82 @@ bool MsckfVio::PhotometricPatchPointJacobian(
// individual Jacobians
/*Matrix<double, 1, 2> */dI_dhj = Matrix<double, 1, 2>::Zero();
Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
/*Matrix<double, 2, 4> */dI_dhj = Matrix<double, 2, 4>::Zero();
Matrix<double, 4, 3> dh_dC0pij = Matrix<double, 4, 3>::Zero();
Matrix<double, 4, 3> dh_dC1pij = Matrix<double, 4, 3>::Zero();
Matrix<double, 4, 3> dh_dGpij = Matrix<double, 4, 3>::Zero();
Matrix<double, 4, 6> dh_dXplj = Matrix<double, 4, 6>::Zero();
Matrix<double, 3, 1> dGpj_drhoj = Matrix<double, 3, 1>::Zero();
Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
Matrix<double, 3, 3> dCpij_dGpij = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dC0pij_dGpij = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dC1pij_dGpij = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 6> dC0pij_dXplj = Matrix<double, 3, 6>::Zero();
Matrix<double, 3, 6> dC1pij_dXplj = Matrix<double, 3, 6>::Zero();
double dx, dy;
// irradiance jacobian
double dx_c0, dy_c0, dx_c1, dy_c1;
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w);
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
cv::Point2f p_in_c1 = feature.projectPositionToCamera(cam_state, cam_state_id, cam1, point);
cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point);
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
// calculate derivation for anchor frame, use position for derivation calculation
// frame derivative calculated convoluting with kernel [-1, 0, 1]
dx = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x");
dy = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y");
//cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl;
dx_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x");
dy_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y");
dI_dhj(0, 0) = dx * cam0.intrinsics[0];
dI_dhj(0, 1) = dy * cam0.intrinsics[1];
dx_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_x");
dy_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_y");
dI_dhj(0, 0) = dx_c0 * cam0.intrinsics[0];
dI_dhj(0, 1) = dy_c0 * cam0.intrinsics[1];
dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0];
dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1];
cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
// add jacobian
//dh / d{}^Cp_{ij}
dh_dC0pij(0, 0) = 1. / p_c0(2);
dh_dC0pij(1, 1) = 1. / p_c0(2);
dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
//dh / d{}^Cp_{ij}
dh_dCpij(0, 0) = 1 / p_c0(2);
dh_dCpij(1, 1) = 1 / p_c0(2);
dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
dh_dC1pij(2, 0) = 1. / p_c1(2);
dh_dC1pij(3, 1) = 1. / p_c1(2);
dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2));
dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2));
dCpij_dGpij = quaternionToRotation(cam_state.orientation);
dC0pij_dGpij = R_w_c0;
dC1pij_dGpij = R_c0_c1 * R_w_c0;
//orientation takes camera frame to world frame, we wa
dh_dGpij = dh_dCpij * dCpij_dGpij;
dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0);
dC0pij_dXplj.rightCols(3) = -R_w_c0;
//dh / d X_{pl}
dCpij_dCGtheta = skewSymmetric(p_c0);
dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
// d{}^Gp_P{ij} / \rho_i
double rho = feature.anchor_rho;
// Isometry T_anchor_w takes a vector in anchor frame to world frame
dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho));
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
feature.anchorPatch_ideal[count].y/(rho),
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho),
feature.anchorPatch_ideal[(N*N-1)/2].y/(rho),
1/(rho)));
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
H_plj = dI_dhj * dh_dXplj; // 1 x 6
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
H_rhoj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1
H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6
H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6
// check if point nullspaceable
if (H_rhoj(0, 0) != 0)
@ -1701,26 +1739,25 @@ bool MsckfVio::PhotometricMeasurementJacobian(
VectorXd r_photo = VectorXd::Zero(N*N);
// one line of the NxN Jacobians
Eigen::Matrix<double, 1, 1> H_rhoj;
Eigen::Matrix<double, 1, 6> H_plj;
Eigen::Matrix<double, 1, 6> H_pAj;
Eigen::Matrix<double, 2, 1> H_rhoj;
Eigen::Matrix<double, 2, 6> H_plj;
Eigen::Matrix<double, 2, 6> H_pAj;
Eigen::MatrixXd dI_dh(N*N, 2);
Eigen::MatrixXd dI_dh(2* N*N, 4);
// combined Jacobians
Eigen::MatrixXd H_rho(N*N, 1);
Eigen::MatrixXd H_pl(N*N, 6);
Eigen::MatrixXd H_pA(N*N, 6);
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
Eigen::MatrixXd H_rho(2 * N*N, 1);
Eigen::MatrixXd H_pl(2 * N*N, 6);
Eigen::MatrixXd H_pA(2 * N*N, 6);
// calcualte residual of patch
PhotometricPatchPointResidual(cam_state_id, feature, r_photo);
cout << "r\n" << r_photo << endl;
// calculate jacobian for patch
int count = 0;
bool valid = false;
Matrix<double, 1, 2> dI_dhj;// = Matrix<double, 1, 2>::Zero();
Matrix<double, 2, 4> dI_dhj;// = Matrix<double, 1, 2>::Zero();
for (auto point : feature.anchorPatch_3d)
{
// get jacobi of single point in patch
@ -1728,11 +1765,11 @@ bool MsckfVio::PhotometricMeasurementJacobian(
valid = true;
// stack point into entire jacobi
H_rho.block<1, 1>(count, 0) = H_rhoj;
H_pl.block<1, 6>(count, 0) = H_plj;
H_pA.block<1, 6>(count, 0) = H_pAj;
H_rho.block<2, 1>(count*2, 0) = H_rhoj;
H_pl.block<2, 6>(count*2, 0) = H_plj;
H_pA.block<2, 6>(count*2, 0) = H_pAj;
dI_dh.block<1, 2>(count, 0) = dI_dhj;
dI_dh.block<2, 4>(count*2, 0) = dI_dhj;
count++;
}
@ -1741,8 +1778,8 @@ bool MsckfVio::PhotometricMeasurementJacobian(
//cout << "h photo: \n" << h_photo << endl;
// construct the jacobian structure needed for nullspacing
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
MatrixXd H_yl = MatrixXd::Zero(N*N, 1);
MatrixXd H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7);
MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1);
ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
@ -1788,7 +1825,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
// set anchor Jakobi
H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = H_pA;
H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*N*N, 6) = H_pA;
//get position of current frame in cam states
auto cam_state_iter = state_server.cam_states.find(cam_state_id);
@ -1796,7 +1833,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
// set jakobi of state
H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = H_pl;
H_xl.block(0, 21+cam_state_cntr*7, 2*N*N, 6) = H_pl;
// set ones for irradiance bias
// H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
@ -1819,7 +1856,6 @@ bool MsckfVio::PhotometricFeatureJacobian(
MatrixXd& H_x, VectorXd& r)
{
return false;
const auto& feature = map_server[feature_id];
// Check how many camera states in the provided camera
@ -1835,7 +1871,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
}
int jacobian_row_size = 0;
jacobian_row_size = N * N * valid_cam_state_ids.size();
jacobian_row_size = 2 * N * N * valid_cam_state_ids.size();
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7);
@ -1847,7 +1883,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
MatrixXd H_xl;
MatrixXd H_yl;
Eigen::VectorXd r_l = VectorXd::Zero(N*N);
Eigen::VectorXd r_l = VectorXd::Zero(2*N*N);
if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
continue;
@ -1855,15 +1891,16 @@ bool MsckfVio::PhotometricFeatureJacobian(
auto cam_state_iter = state_server.cam_states.find(cam_id);
int cam_state_cntr = std::distance(
state_server.cam_states.begin(), cam_state_iter);
// Stack the Jacobians.
H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
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;
r_i.segment(stack_cntr, 2*N*N) = r_l;
stack_cntr += 2*N*N;
}
// if not enough to propper nullspace (in paper implementation)
if(stack_cntr < N*N)
if(stack_cntr < 2*N*N)
return false;
// Project the residual and Jacobians onto the nullspace
@ -2263,10 +2300,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0)
{
cout << "zero" << endl;
return;
}
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;
@ -2316,7 +2350,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
// Update the IMU state.
if (FILTER != 2) return;
cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
//cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]);
delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]);
@ -2377,8 +2411,11 @@ void MsckfVio::twoMeasurementUpdate(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)
{
cout << "zero" << endl;
return;
}
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;
@ -2423,7 +2460,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
VectorXd delta_x = K * r;
// Update the IMU state.
// cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
if (FILTER != 1) return;
@ -2504,7 +2541,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
//return true;
return true;
MatrixXd P1 = H * state_server.state_cov * H.transpose();
MatrixXd P2 = Feature::observation_noise *
@ -2571,7 +2608,7 @@ void MsckfVio::removeLostFeatures() {
}
}
pjacobian_row_size += N*N*feature.observations.size();
pjacobian_row_size += 2*N*N*feature.observations.size();
twojacobian_row_size += 4*feature.observations.size();
jacobian_row_size += 4*feature.observations.size() - 3;
@ -2640,7 +2677,6 @@ void MsckfVio::removeLostFeatures() {
stack_cntr += H_xj.rows();
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
cout << "passed" << endl;
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
@ -2769,7 +2805,7 @@ void MsckfVio::pruneLastCamStateBuffer()
}
}
pjacobian_row_size += N*N*feature.observations.size();
pjacobian_row_size += 2*N*N*feature.observations.size();
jacobian_row_size += 4*feature.observations.size() - 3;
twojacobian_row_size += 4*feature.observations.size();
@ -2827,7 +2863,6 @@ void MsckfVio::pruneLastCamStateBuffer()
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
cout << "passed" << endl;
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
@ -2954,7 +2989,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
twojacobian_row_size += 4*involved_cam_state_ids.size();
pjacobian_row_size += N*N*involved_cam_state_ids.size();
pjacobian_row_size += 2*N*N*involved_cam_state_ids.size();
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
}
@ -3010,7 +3045,6 @@ void MsckfVio::pruneCamStateBuffer() {
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
cout << "passed" << endl;
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();