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

added structure to compare two patch sizes

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-11 13:57:49 +02:00
parent 1380ec426f
commit 30daf37202
5 changed files with 184 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config/camchain-imucam-tum-scaled.yaml
View File

@ -7,7 +7,7 @@ cam0:
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
0.00020293673591811182] 0.00020293673591811182]
distortion_model: equidistant distortion_model: pre-equidistant
intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
resolution: [512, 512] resolution: [512, 512]
rostopic: /cam0/image_raw rostopic: /cam0/image_raw
@ -25,7 +25,7 @@ cam1:
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
0.0003299517423931039] 0.0003299517423931039]
distortion_model: equidistant distortion_model: pre-equidistant
intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
resolution: [512, 512] resolution: [512, 512]
rostopic: /cam1/image_raw rostopic: /cam1/image_raw

4
config/camchain-imucam-tum.yaml
View File

@ -7,7 +7,7 @@ cam0:
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
-0.001662284667857643] -0.001662284667857643]
distortion_model: equidistant distortion_model: pre-equidistant
intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506] intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506]
resolution: [1024, 1024] resolution: [1024, 1024]
rostopic: /cam0/image_raw rostopic: /cam0/image_raw
@ -25,7 +25,7 @@ cam1:
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
-0.002347858896562788] -0.002347858896562788]
distortion_model: equidistant distortion_model: pre-equidistant
intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407] intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407]
resolution: [1024, 1024] resolution: [1024, 1024]
rostopic: /cam1/image_raw rostopic: /cam1/image_raw

13
include/msckf_vio/msckf_vio.h
View File

@ -220,12 +220,14 @@ class MsckfVio {
const Feature& feature, const Feature& feature,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
Eigen::MatrixXd& H_xl, Eigen::MatrixXd& H_xl,
Eigen::MatrixXd& H_yl); Eigen::MatrixXd& H_yl,
int patchsize);
bool PhotometricPatchPointResidual( bool PhotometricPatchPointResidual(
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const Feature& feature, const Feature& feature,
Eigen::VectorXd& r); Eigen::VectorXd& r,
int patchsize);
bool PhotometricPatchPointJacobian( bool PhotometricPatchPointJacobian(
const CAMState& cam_state, const CAMState& cam_state,
@ -233,6 +235,7 @@ class MsckfVio {
const Feature& feature, const Feature& feature,
Eigen::Vector3d point, Eigen::Vector3d point,
int count, int count,
int patchsize,
Eigen::Matrix<double, 2, 1>& H_rhoj, Eigen::Matrix<double, 2, 1>& H_rhoj,
Eigen::Matrix<double, 2, 6>& H_plj, Eigen::Matrix<double, 2, 6>& H_plj,
Eigen::Matrix<double, 2, 6>& H_pAj, Eigen::Matrix<double, 2, 6>& H_pAj,
@ -243,7 +246,8 @@ class MsckfVio {
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_x,
Eigen::MatrixXd& H_y, Eigen::MatrixXd& H_y,
Eigen::VectorXd& r); Eigen::VectorXd& r,
int patchsize);
void twodotFeatureJacobian( void twodotFeatureJacobian(
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
@ -253,7 +257,8 @@ class MsckfVio {
bool PhotometricFeatureJacobian( bool PhotometricFeatureJacobian(
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids, const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r); Eigen::MatrixXd& H_x, Eigen::VectorXd& r,
int patchsize);
void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
void measurementUpdate(const Eigen::MatrixXd& H, void measurementUpdate(const Eigen::MatrixXd& H,

2
launch/msckf_vio_tum.launch
View File

@ -33,7 +33,7 @@
<param name="frame_rate" value="20"/> <param name="frame_rate" value="20"/>
<param name="fixed_frame_id" value="$(arg fixed_frame_id)"/> <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
<param name="child_frame_id" value="odom"/> <param name="child_frame_id" value="odom"/>
<param name="max_cam_state_size" value="20"/> <param name="max_cam_state_size" value="12"/>
<param name="position_std_threshold" value="8.0"/> <param name="position_std_threshold" value="8.0"/>
<param name="rotation_threshold" value="0.2618"/> <param name="rotation_threshold" value="0.2618"/>

224
src/msckf_vio.cpp
View File

@ -449,7 +449,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
// cout << "2" << endl; cout << "2" << endl;
// Add new observations for existing features or new // Add new observations for existing features or new
// features in the map server. // features in the map server.
start_time = ros::Time::now(); start_time = ros::Time::now();
@ -458,7 +458,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
// cout << "3" << endl; cout << "3" << endl;
// Add new images to moving window // Add new images to moving window
start_time = ros::Time::now(); start_time = ros::Time::now();
manageMovingWindow(cam0_img, cam1_img, feature_msg); manageMovingWindow(cam0_img, cam1_img, feature_msg);
@ -466,14 +466,14 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
// cout << "4" << endl; cout << "4" << endl;
// Perform measurement update if necessary. // Perform measurement update if necessary.
start_time = ros::Time::now(); start_time = ros::Time::now();
removeLostFeatures(); removeLostFeatures();
double remove_lost_features_time = ( double remove_lost_features_time = (
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
// cout << "5" << endl; cout << "5" << endl;
start_time = ros::Time::now(); start_time = ros::Time::now();
pruneLastCamStateBuffer(); pruneLastCamStateBuffer();
double prune_cam_states_time = ( double prune_cam_states_time = (
@ -483,7 +483,7 @@ void MsckfVio::imageCallback(
batchTruthProcessing(feature_msg->header.stamp.toSec()); batchTruthProcessing(feature_msg->header.stamp.toSec());
// cout << "6" << endl; cout << "6" << endl;
// Publish the odometry. // Publish the odometry.
start_time = ros::Time::now(); start_time = ros::Time::now();
publish(feature_msg->header.stamp); publish(feature_msg->header.stamp);
@ -1561,9 +1561,9 @@ void MsckfVio::twodotFeatureJacobian(
bool MsckfVio::PhotometricPatchPointResidual( bool MsckfVio::PhotometricPatchPointResidual(
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const Feature& feature, const Feature& feature,
VectorXd& r) VectorXd& r, int patchsize)
{ {
VectorXd r_photo = VectorXd::Zero(2*N*N); VectorXd r_photo = VectorXd::Zero(2*patchsize*patchsize);
int count = 0; int count = 0;
const CAMState& cam_state = state_server.cam_states[cam_state_id]; const CAMState& cam_state = state_server.cam_states[cam_state_id];
@ -1574,18 +1574,19 @@ bool MsckfVio::PhotometricPatchPointResidual(
std::vector<double> photo_z_c0, photo_z_c1; std::vector<double> photo_z_c0, photo_z_c1;
// estimate irradiance based on anchor frame // estimate irradiance based on anchor frame
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
for (auto& estimate_irradiance_j : estimate_irradiance) for (auto& estimate_irradiance_j : estimate_irradiance)
estimate_photo_z_c0.push_back (estimate_irradiance_j);// * estimate_photo_z_c0.push_back (estimate_irradiance_j);// *
//estimated_illumination.frame_gain * estimated_illumination.feature_gain + //estimated_illumination.frame_gain * estimated_illumination.feature_gain +
//estimated_illumination.frame_bias + estimated_illumination.feature_bias); //estimated_illumination.frame_bias + estimated_illumination.feature_bias);
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination); feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination);
for (auto& estimate_irradiance_j : estimate_irradiance) for (auto& estimate_irradiance_j : estimate_irradiance)
estimate_photo_z_c1.push_back (estimate_irradiance_j);// * estimate_photo_z_c1.push_back (estimate_irradiance_j);// *
//estimated_illumination.frame_gain * estimated_illumination.feature_gain + //estimated_illumination.frame_gain * estimated_illumination.feature_gain +
//estimated_illumination.frame_bias + estimated_illumination.feature_bias); //estimated_illumination.frame_bias + estimated_illumination.feature_bias);
// irradiance measurement around feature point in c0 and c1 // irradiance measurement around feature point in c0 and c1
std::vector<double> true_irradiance_c0, true_irradiance_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_c0(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1));
@ -1596,15 +1597,32 @@ bool MsckfVio::PhotometricPatchPointResidual(
cv::Mat current_image_c0 = cam0.moving_window.find(cam_state_id)->second.image; 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; cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image;
for(int i = 0; i<N; i++) { for(int i = 0; i<patchsize; i++) {
for(int j = 0; j<N ; j++) { for(int j = 0; j<patchsize; 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_c0.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c0.x + (i-(patchsize-1)/2), p_f_c0.y + (j-(patchsize-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)); true_irradiance_c1.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c1.x + (i-(patchsize-1)/2), p_f_c1.y + (j-(patchsize-1)/2)), current_image_c1));
} }
} }
std::vector<Eigen::Vector3d> new_anchorPatch_3d;
if (patchsize == 3 and N == 5)
{
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[6]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[7]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[8]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[11]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[12]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[13]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[16]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[17]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[18]);
}
else
{
new_anchorPatch_3d = feature.anchorPatch_3d;
}
// get residual for(auto point : new_anchorPatch_3d)
for(auto point : feature.anchorPatch_3d)
{ {
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
@ -1629,6 +1647,8 @@ bool MsckfVio::PhotometricPatchPointResidual(
count++; count++;
} }
r = r_photo; r = r_photo;
cout << "returning" << endl;
return true; return true;
} }
@ -1639,6 +1659,7 @@ bool MsckfVio::PhotometricPatchPointJacobian(
const Feature& feature, const Feature& feature,
Eigen::Vector3d point, Eigen::Vector3d point,
int count, int count,
int patchsize,
Eigen::Matrix<double, 2, 1>& H_rhoj, Eigen::Matrix<double, 2, 1>& H_rhoj,
Eigen::Matrix<double, 2, 6>& H_plj, Eigen::Matrix<double, 2, 6>& H_plj,
Eigen::Matrix<double, 2, 6>& H_pAj, Eigen::Matrix<double, 2, 6>& H_pAj,
@ -1723,11 +1744,11 @@ bool MsckfVio::PhotometricPatchPointJacobian(
// d{}^Gp_P{ij} / \rho_i // d{}^Gp_P{ij} / \rho_i
double rho = feature.anchor_rho; double rho = feature.anchor_rho;
// Isometry T_anchor_w takes a vector in anchor frame to world frame // 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[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho)); dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho*rho), 1/(rho*rho));
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho),
feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho),
1/(rho))); 1/(rho)));
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity(); dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
@ -1750,41 +1771,61 @@ bool MsckfVio::PhotometricPatchPointJacobian(
bool MsckfVio::PhotometricMeasurementJacobian( bool MsckfVio::PhotometricMeasurementJacobian(
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) MatrixXd& H_x, MatrixXd& H_y, VectorXd& r, int patchsize)
{ {
// Prepare all the required data. // Prepare all the required data.
const CAMState& cam_state = state_server.cam_states[cam_state_id]; const CAMState& cam_state = state_server.cam_states[cam_state_id];
const Feature& feature = map_server[feature_id]; const Feature& feature = map_server[feature_id];
//photometric observation //photometric observation
VectorXd r_photo; VectorXd r_photo = VectorXd::Zero(2*patchsize*patchsize);
// one line of the NxN Jacobians // one line of the patchsizexpatchsize Jacobians
Eigen::Matrix<double, 2, 1> H_rhoj; Eigen::Matrix<double, 2, 1> H_rhoj;
Eigen::Matrix<double, 2, 6> H_plj; Eigen::Matrix<double, 2, 6> H_plj;
Eigen::Matrix<double, 2, 6> H_pAj; Eigen::Matrix<double, 2, 6> H_pAj;
Eigen::MatrixXd dI_dh(2* N*N, 4); Eigen::MatrixXd dI_dh(2* patchsize*patchsize, 4);
// combined Jacobians // combined Jacobians
Eigen::MatrixXd H_rho(2 * N*N, 1); Eigen::MatrixXd H_rho(2 * patchsize*patchsize, 1);
Eigen::MatrixXd H_pl(2 * N*N, 6); Eigen::MatrixXd H_pl(2 * patchsize*patchsize, 6);
Eigen::MatrixXd H_pA(2 * N*N, 6); Eigen::MatrixXd H_pA(2 * patchsize*patchsize, 6);
// calcualte residual of patch // calcualte residual of patch
if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo, patchsize))
{
return false; return false;
}
//cout << "r\n" << r_photo << endl; //cout << "r\n" << r_photo << endl;
// calculate jacobian for patch // calculate jacobian for patch
int count = 0; int count = 0;
bool valid = false; bool valid = false;
Matrix<double, 2, 4> dI_dhj;// = Matrix<double, 1, 2>::Zero(); Matrix<double, 2, 4> dI_dhj;// = Matrix<double, 1, 2>::Zero();
int valid_count = 0; int valid_count = 0;
for (auto point : feature.anchorPatch_3d)
std::vector<Eigen::Vector3d> new_anchorPatch_3d;
if (patchsize == 3 and N == 5)
{
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[6]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[7]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[8]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[11]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[12]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[13]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[16]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[17]);
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[18]);
}
else
{
new_anchorPatch_3d = feature.anchorPatch_3d;
}
for(auto point : new_anchorPatch_3d)
{ {
// get jacobi of single point in patch // get jacobi of single point in patch
if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, patchsize, H_rhoj, H_plj, H_pAj, dI_dhj))
{ {
valid_count++; valid_count++;
valid = true; valid = true;
@ -1796,7 +1837,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
H_pA.block<2, 6>(count*2, 0) = H_pAj; H_pA.block<2, 6>(count*2, 0) = H_pAj;
dI_dh.block<2, 4>(count*2, 0) = dI_dhj; dI_dh.block<2, 4>(count*2, 0) = dI_dhj;
count++; count++;
} }
// cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; // cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl;
@ -1807,7 +1847,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
MatrixXd H_xl; MatrixXd H_xl;
MatrixXd H_yl; MatrixXd H_yl;
ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl, patchsize);
// set to return values // set to return values
H_x = H_xl; H_x = H_xl;
@ -1845,16 +1885,17 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
const Feature& feature, const Feature& feature,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
Eigen::MatrixXd& H_xl, Eigen::MatrixXd& H_xl,
Eigen::MatrixXd& H_yl) Eigen::MatrixXd& H_yl,
int patchsize)
{ {
H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); H_xl = MatrixXd::Zero(2*patchsize*patchsize, 21+state_server.cam_states.size()*7);
H_yl = MatrixXd::Zero(2*N*N, 1); H_yl = MatrixXd::Zero(2*patchsize*patchsize, 1);
// get position of anchor in cam states // get position of anchor in cam states
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); 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); int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
// set anchor Jakobi // set anchor Jakobi
H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*N*N, 6) = H_pA; H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*patchsize*patchsize, 6) = H_pA;
//get position of current frame in cam states //get position of current frame in cam states
auto cam_state_iter = state_server.cam_states.find(cam_state_id); auto cam_state_iter = state_server.cam_states.find(cam_state_id);
@ -1862,7 +1903,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
// set jakobi of state // set jakobi of state
H_xl.block(0, 21+cam_state_cntr*7, 2*N*N, 6) = H_pl; H_xl.block(0, 21+cam_state_cntr*7, 2*patchsize*patchsize, 6) = H_pl;
// set ones for irradiance bias // set ones for irradiance bias
// H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
@ -1882,7 +1923,8 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
bool MsckfVio::PhotometricFeatureJacobian( bool MsckfVio::PhotometricFeatureJacobian(
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids, const std::vector<StateIDType>& cam_state_ids,
MatrixXd& H_x, VectorXd& r) MatrixXd& H_x, VectorXd& r,
int patchsize)
{ {
const auto& feature = map_server[feature_id]; const auto& feature = map_server[feature_id];
@ -1917,7 +1959,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
for (const auto& cam_id : valid_cam_state_ids) { for (const auto& cam_id : valid_cam_state_ids) {
// skip observation if measurement is not valid // skip observation if measurement is not valid
if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l, patchsize))
continue; continue;
// set size of stacking jacobians, once the returned jacobians are known // set size of stacking jacobians, once the returned jacobians are known
@ -1939,7 +1981,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
r_i.segment(stack_cntr, r_l.size()) = r_l; r_i.segment(stack_cntr, r_l.size()) = r_l;
stack_cntr += r_l.size(); stack_cntr += r_l.size();
} }
// if not enough to propper nullspace (in paper implementation) // if not enough to propper nullspace (in paper implementation)
if(stack_cntr < r_l.size()) if(stack_cntr < r_l.size())
return false; return false;
@ -2023,6 +2065,9 @@ bool MsckfVio::PhotometricFeatureJacobian(
cout << "---------- LOGGED -------- " << endl; cout << "---------- LOGGED -------- " << endl;
} }
cout << H_x.rows() << ":" << H_x.cols() << endl;
cout << r.rows() << ":" << r.cols() << endl;
if(valid) if(valid)
return true; return true;
return false; return false;
@ -2590,6 +2635,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
double gamma = r.transpose() * (P1+P2).ldlt().solve(r); double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
/*
if(gamma > 1000000) if(gamma > 1000000)
{ {
cout << " logging " << endl; cout << " logging " << endl;
@ -2615,14 +2661,16 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
<< state_server.state_cov << endl; << state_server.state_cov << endl;
myfile.close(); myfile.close();
} }
*/
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
if (chi_squared_test_table[dof] == 0) if (chi_squared_test_table[dof] == 0)
return false; return false;
if (gamma < chi_squared_test_table[dof]) { if (gamma < chi_squared_test_table[dof]) {
// cout << "passed" << endl; // cout << "passed" << endl;
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
return true; return true;
} else { } else {
// cout << "failed" << endl; // cout << "failed" << endl;
@ -2635,6 +2683,7 @@ void MsckfVio::removeLostFeatures() {
// BTW, find the size the final Jacobian matrix and residual vector. // BTW, find the size the final Jacobian matrix and residual vector.
int jacobian_row_size = 0; int jacobian_row_size = 0;
int pjacobian_row_size = 0; int pjacobian_row_size = 0;
int p2jacobian_row_size = 0;
int twojacobian_row_size = 0; int twojacobian_row_size = 0;
vector<FeatureIDType> invalid_feature_ids(0); vector<FeatureIDType> invalid_feature_ids(0);
@ -2676,6 +2725,7 @@ void MsckfVio::removeLostFeatures() {
} }
pjacobian_row_size += 2*N*N*feature.observations.size(); pjacobian_row_size += 2*N*N*feature.observations.size();
p2jacobian_row_size += 2*3*3*feature.observations.size();
twojacobian_row_size += 4*feature.observations.size(); twojacobian_row_size += 4*feature.observations.size();
jacobian_row_size += 4*feature.observations.size() - 3; jacobian_row_size += 4*feature.observations.size() - 3;
@ -2704,6 +2754,10 @@ void MsckfVio::removeLostFeatures() {
VectorXd pr = VectorXd::Zero(pjacobian_row_size); VectorXd pr = VectorXd::Zero(pjacobian_row_size);
int pstack_cntr = 0; int pstack_cntr = 0;
MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size,
21+7*state_server.cam_states.size());
VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
int p2stack_cntr = 0;
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size,
21+7*state_server.cam_states.size()); 21+7*state_server.cam_states.size());
@ -2720,12 +2774,18 @@ void MsckfVio::removeLostFeatures() {
MatrixXd H_xj; MatrixXd H_xj;
VectorXd r_j; VectorXd r_j;
MatrixXd pH_xj;
VectorXd pr_j;
MatrixXd twoH_xj; MatrixXd twoH_xj;
VectorXd twor_j; VectorXd twor_j;
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) MatrixXd pH_xj;
VectorXd pr_j;
MatrixXd p2H_xj;
VectorXd p2r_j;
cout << "5: " << endl;
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j, N))
{ {
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
@ -2734,6 +2794,20 @@ void MsckfVio::removeLostFeatures() {
} }
} }
cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3))
{
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
cout << p2H_x.rows() << ":" << p2H_x.cols() << ", " << p2H_xj.rows() << ":" << p2H_xj.cols() << endl;
p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
p2stack_cntr += p2H_xj.rows();
}
}
featureJacobian(feature.id, cam_state_ids, H_xj, r_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
@ -2831,6 +2905,7 @@ void MsckfVio::pruneLastCamStateBuffer()
// Set the size of the Jacobian matrix. // Set the size of the Jacobian matrix.
int jacobian_row_size = 0; int jacobian_row_size = 0;
int pjacobian_row_size = 0; int pjacobian_row_size = 0;
int p2jacobian_row_size = 0;
int twojacobian_row_size = 0; int twojacobian_row_size = 0;
@ -2869,6 +2944,7 @@ void MsckfVio::pruneLastCamStateBuffer()
} }
pjacobian_row_size += 2*N*N*feature.observations.size(); pjacobian_row_size += 2*N*N*feature.observations.size();
p2jacobian_row_size += 2*3*3*feature.observations.size();
jacobian_row_size += 4*feature.observations.size() - 3; jacobian_row_size += 4*feature.observations.size() - 3;
twojacobian_row_size += 4*feature.observations.size(); twojacobian_row_size += 4*feature.observations.size();
@ -2884,11 +2960,16 @@ void MsckfVio::pruneLastCamStateBuffer()
VectorXd pr = VectorXd::Zero(pjacobian_row_size); VectorXd pr = VectorXd::Zero(pjacobian_row_size);
MatrixXd twoH_xj; MatrixXd twoH_xj;
VectorXd twor_j; VectorXd twor_j;
MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
MatrixXd p2H_xj;
VectorXd p2r_j;
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd twor = VectorXd::Zero(twojacobian_row_size); VectorXd twor = VectorXd::Zero(twojacobian_row_size);
int stack_cntr = 0; int stack_cntr = 0;
int pruned_cntr = 0; int pruned_cntr = 0;
int pstack_cntr = 0; int pstack_cntr = 0;
int p2stack_cntr = 0;
int twostack_cntr = 0; int twostack_cntr = 0;
for (auto& item : map_server) { for (auto& item : map_server) {
@ -2904,17 +2985,28 @@ void MsckfVio::pruneLastCamStateBuffer()
for (const auto& cam_state : state_server.cam_states) for (const auto& cam_state : state_server.cam_states)
involved_cam_state_ids.push_back(cam_state.first); involved_cam_state_ids.push_back(cam_state.first);
cout << "5: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
{ {
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows(); pstack_cntr += pH_xj.rows();
} }
} }
/*
cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
{
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
p2stack_cntr += p2H_xj.rows();
}
}
*/
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
@ -3003,6 +3095,7 @@ void MsckfVio::pruneCamStateBuffer() {
// Find the size of the Jacobian matrix. // Find the size of the Jacobian matrix.
int jacobian_row_size = 0; int jacobian_row_size = 0;
int pjacobian_row_size = 0; int pjacobian_row_size = 0;
int p2jacobian_row_size = 0;
int twojacobian_row_size = 0; int twojacobian_row_size = 0;
for (auto& item : map_server) { for (auto& item : map_server) {
@ -3047,9 +3140,10 @@ void MsckfVio::pruneCamStateBuffer() {
continue; continue;
} }
} }
twojacobian_row_size += 4*involved_cam_state_ids.size(); twojacobian_row_size += 4*involved_cam_state_ids.size();
pjacobian_row_size += 2*N*N*involved_cam_state_ids.size(); pjacobian_row_size += 2*N*N*involved_cam_state_ids.size();
p2jacobian_row_size += 2*3*3*involved_cam_state_ids.size();
jacobian_row_size += 4*involved_cam_state_ids.size() - 3; jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
} }
@ -3061,11 +3155,19 @@ void MsckfVio::pruneCamStateBuffer() {
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size()); MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size); VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0; int stack_cntr = 0;
MatrixXd pH_xj; MatrixXd pH_xj;
VectorXd pr_j; VectorXd pr_j;
MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size()); MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd pr = VectorXd::Zero(pjacobian_row_size); VectorXd pr = VectorXd::Zero(pjacobian_row_size);
int pstack_cntr = 0; int pstack_cntr = 0;
MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, 21+7*state_server.cam_states.size());
VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
MatrixXd p2H_xj;
VectorXd p2r_j;
int p2stack_cntr = 0;
MatrixXd twoH_xj; MatrixXd twoH_xj;
VectorXd twor_j; VectorXd twor_j;
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
@ -3084,14 +3186,28 @@ void MsckfVio::pruneCamStateBuffer() {
if (involved_cam_state_ids.size() == 0) continue; if (involved_cam_state_ids.size() == 0) continue;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) cout << "5: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
{ {
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) {
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows(); pstack_cntr += pH_xj.rows();
} }
} }
cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
{
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
p2stack_cntr += p2H_xj.rows();
}
}
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);