changed to simple irradiance calcualation with derivation of image per frame - not working
This commit is contained in:
parent
737c23f32a
commit
6bcc72f826
@ -184,6 +184,11 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
|
||||
const CameraCalibration& cam,
|
||||
Eigen::Vector3d& in_p) const;
|
||||
|
||||
double CompleteCvKernel(
|
||||
const cv::Point2f pose,
|
||||
const cv::Mat& frame,
|
||||
std::string type) const;
|
||||
|
||||
double cvKernel(
|
||||
const cv::Point2f pose,
|
||||
std::string type) const;
|
||||
@ -488,6 +493,45 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
|
||||
else return false;
|
||||
}
|
||||
|
||||
double Feature::CompleteCvKernel(
|
||||
const cv::Point2f pose,
|
||||
const cv::Mat& frame,
|
||||
std::string type) const
|
||||
{
|
||||
|
||||
double delta = 0;
|
||||
|
||||
cv::Mat xder;
|
||||
cv::Mat yder;
|
||||
cv::Mat deeper_frame;
|
||||
frame.convertTo(deeper_frame,CV_16S);
|
||||
//TODO remove this?
|
||||
|
||||
|
||||
cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
|
||||
cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
|
||||
|
||||
xder/=8.;
|
||||
yder/=8.;
|
||||
|
||||
/*
|
||||
cv::Mat norm_abs_xderImage;
|
||||
cv::Mat abs_xderImage2;
|
||||
cv::convertScaleAbs(xder, abs_xderImage2);
|
||||
|
||||
cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
|
||||
|
||||
cv::imshow("xder", norm_abs_xderImage);
|
||||
cvWaitKey(0);
|
||||
*/
|
||||
|
||||
if(type == "Sobel_x")
|
||||
delta = ((double)xder.at<short>(pose.y, pose.x))/255.;
|
||||
else if (type == "Sobel_y")
|
||||
delta = ((double)yder.at<short>(pose.y, pose.x))/255.;
|
||||
return delta;
|
||||
}
|
||||
|
||||
double Feature::cvKernel(
|
||||
const cv::Point2f pose,
|
||||
std::string type) const
|
||||
@ -823,6 +867,7 @@ bool Feature::VisualizePatch(
|
||||
// residual grid projection, positive - red, negative - blue colored
|
||||
namer.str(std::string());
|
||||
namer << "residual";
|
||||
std::cout << "-- photo_r -- \n" << photo_r << " -- " << std::endl;
|
||||
cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5),
|
||||
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
|
||||
|
||||
|
||||
@ -229,12 +229,14 @@ class MsckfVio {
|
||||
|
||||
bool PhotometricPatchPointJacobian(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
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, 6>& H_pAj,
|
||||
Eigen::Matrix<double, 1, 2>& dI_dhj);
|
||||
|
||||
bool PhotometricMeasurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
|
||||
@ -18,14 +18,14 @@
|
||||
output="screen">
|
||||
|
||||
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
|
||||
<param name="FILTER" value="0"/>
|
||||
<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="3"/>
|
||||
<param name="patch_size_n" value="5"/>
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
|
||||
@ -25,7 +25,7 @@
|
||||
<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)"/>
|
||||
|
||||
|
||||
@ -496,8 +496,8 @@ void MsckfVio::imageCallback(
|
||||
processing_end_time - processing_start_time;
|
||||
if (processing_time > 1.0/frame_rate) {
|
||||
++critical_time_cntr;
|
||||
ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
|
||||
processing_time, critical_time_cntr);
|
||||
//ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
|
||||
// processing_time, critical_time_cntr);
|
||||
//printf("IMU processing time: %f/%f\n",
|
||||
// imu_processing_time, imu_processing_time/processing_time);
|
||||
//printf("State augmentation time: %f/%f\n",
|
||||
@ -1418,8 +1418,7 @@ void MsckfVio::twodotMeasurementJacobian(
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
|
||||
feature.MarkerGeneration(marker_pub, state_server.cam_states);
|
||||
//feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
|
||||
feature.MarkerGeneration(marker_pub, state_server.cam_states);
|
||||
}
|
||||
|
||||
return;
|
||||
@ -1540,27 +1539,40 @@ bool MsckfVio::PhotometricPatchPointResidual(
|
||||
std::vector<double> estimate_photo_z;
|
||||
std::vector<double> photo_z;
|
||||
|
||||
// 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);
|
||||
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
|
||||
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));
|
||||
|
||||
|
||||
for(auto point : feature.anchorPatch_3d)
|
||||
{
|
||||
|
||||
|
||||
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, 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)
|
||||
return false;
|
||||
//add observation
|
||||
// add observation
|
||||
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
|
||||
|
||||
//calculate photom. residual
|
||||
r_photo(count) = photo_z[count] - estimate_photo_z[count];
|
||||
// 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];
|
||||
count++;
|
||||
}
|
||||
r = r_photo;
|
||||
@ -1570,12 +1582,14 @@ bool MsckfVio::PhotometricPatchPointResidual(
|
||||
// generates the jacobian of one patch point regarding rho, anchor and current frame
|
||||
bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
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, 6>& H_pAj,
|
||||
Matrix<double, 1, 2>& dI_dhj)
|
||||
{
|
||||
|
||||
const StateIDType anchor_state_id = feature.observations.begin()->first;
|
||||
@ -1592,7 +1606,7 @@ bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
|
||||
|
||||
// individual Jacobians
|
||||
Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
|
||||
/*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();
|
||||
@ -1606,13 +1620,17 @@ bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
double dx, dy;
|
||||
|
||||
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
|
||||
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, 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.cvKernel(p_in_anchor, "Sobel_x");
|
||||
dy = feature.cvKernel(p_in_anchor, "Sobel_y");
|
||||
dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x");
|
||||
dy = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_y");
|
||||
//cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl;
|
||||
|
||||
dI_dhj(0, 0) = dx * cam0.intrinsics[0];
|
||||
dI_dhj(0, 1) = dy * cam0.intrinsics[1];
|
||||
@ -1650,7 +1668,11 @@ bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
H_plj = dI_dhj * dh_dXplj; // 1 x 6
|
||||
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
|
||||
|
||||
return true;
|
||||
// check if point nullspaceable
|
||||
if (H_rhoj(0, 0) != 0)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
@ -1671,6 +1693,8 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
Eigen::Matrix<double, 1, 6> H_plj;
|
||||
Eigen::Matrix<double, 1, 6> H_pAj;
|
||||
|
||||
Eigen::MatrixXd dI_dh(N*N, 2);
|
||||
|
||||
// combined Jacobians
|
||||
Eigen::MatrixXd H_rho(N*N, 1);
|
||||
Eigen::MatrixXd H_pl(N*N, 6);
|
||||
@ -1683,26 +1707,33 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
// calculate jacobian for patch
|
||||
int count = 0;
|
||||
bool valid = false;
|
||||
Matrix<double, 1, 2> dI_dhj;// = Matrix<double, 1, 2>::Zero();
|
||||
for (auto point : feature.anchorPatch_3d)
|
||||
{
|
||||
// get jacobi of single point in patch
|
||||
PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj);
|
||||
|
||||
if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj))
|
||||
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;
|
||||
|
||||
dI_dh.block<1, 2>(count, 0) = dI_dhj;
|
||||
|
||||
count++;
|
||||
}
|
||||
|
||||
//Eigen::Matrix<double, 2, 1> h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo;
|
||||
//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, N*N+1);
|
||||
|
||||
MatrixXd H_yl = MatrixXd::Zero(N*N, 1);
|
||||
|
||||
ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
|
||||
|
||||
|
||||
// set to return values
|
||||
H_x = H_xl;
|
||||
H_y = H_yl;
|
||||
@ -1726,9 +1757,13 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
|
||||
}
|
||||
|
||||
return true;
|
||||
if(valid)
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
// uses the calcualted jacobians to construct the final Hx Hy jacobians used for nullspacing
|
||||
bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
|
||||
Eigen::MatrixXd& H_pl,
|
||||
Eigen::MatrixXd& H_pA,
|
||||
@ -1741,7 +1776,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, 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);
|
||||
@ -1749,21 +1784,20 @@ 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, 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);
|
||||
|
||||
// set irradiance error Block
|
||||
H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N);
|
||||
//H_yl.block(0, 0, N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N);
|
||||
|
||||
|
||||
/*for(int i = 0; i< N*N; i++)
|
||||
H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
|
||||
*/
|
||||
|
||||
H_yl.block(0, N*N, N*N, 1) = -H_rho;
|
||||
|
||||
H_yl = H_rho;
|
||||
}
|
||||
|
||||
|
||||
@ -1772,7 +1806,6 @@ bool MsckfVio::PhotometricFeatureJacobian(
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
MatrixXd& H_x, VectorXd& r)
|
||||
{
|
||||
|
||||
const auto& feature = map_server[feature_id];
|
||||
|
||||
// Check how many camera states in the provided camera
|
||||
@ -1792,7 +1825,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
|
||||
|
||||
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
|
||||
21+state_server.cam_states.size()*7);
|
||||
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1);
|
||||
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1
|
||||
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
|
||||
int stack_cntr = 0;
|
||||
|
||||
@ -1803,7 +1836,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
|
||||
Eigen::VectorXd r_l = VectorXd::Zero(N*N);
|
||||
|
||||
if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
|
||||
continue;
|
||||
continue;
|
||||
|
||||
auto cam_state_iter = state_server.cam_states.find(cam_id);
|
||||
int cam_state_cntr = std::distance(
|
||||
@ -1815,19 +1848,25 @@ bool MsckfVio::PhotometricFeatureJacobian(
|
||||
r_i.segment(stack_cntr, N*N) = r_l;
|
||||
stack_cntr += N*N;
|
||||
}
|
||||
if(stack_cntr < 2*N*N)
|
||||
return false;
|
||||
// if not enough to propper nullspace (in paper implementation)
|
||||
if(stack_cntr < N*N)
|
||||
return false;
|
||||
|
||||
// Project the residual and Jacobians onto the nullspace
|
||||
// of H_yj.
|
||||
|
||||
// get Nullspace
|
||||
|
||||
bool valid = false;
|
||||
for(int i = 0; i < H_yi.rows(); i++)
|
||||
if(H_yi(i,0) != 0)
|
||||
valid = true;
|
||||
|
||||
FullPivLU<MatrixXd> lu(H_yi.transpose());
|
||||
MatrixXd A_null_space = lu.kernel();
|
||||
|
||||
H_x = A_null_space.transpose() * H_xi;
|
||||
r = A_null_space.transpose() * r_i;
|
||||
|
||||
//cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
|
||||
|
||||
if(PRINTIMAGES)
|
||||
@ -1956,6 +1995,7 @@ void MsckfVio::measurementJacobian(
|
||||
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));
|
||||
|
||||
// cout << "h reg: \n" << r(0) << "\n" << r(1) << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
@ -2333,9 +2373,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
|
||||
SPQR<SparseMatrix<double> > spqr_helper;
|
||||
spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
|
||||
|
||||
cout << "comp" << endl;
|
||||
spqr_helper.compute(H_sparse);
|
||||
cout << "done" << endl;
|
||||
|
||||
MatrixXd H_temp;
|
||||
VectorXd r_temp;
|
||||
|
||||
@ -2364,6 +2403,9 @@ 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;
|
||||
|
||||
|
||||
if (FILTER != 1) return;
|
||||
|
||||
if(PRINTIMAGES)
|
||||
@ -2387,9 +2429,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
|
||||
|
||||
myfile.close();
|
||||
}
|
||||
|
||||
cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
|
||||
|
||||
const VectorXd& delta_x_imu = delta_x.head<21>();
|
||||
|
||||
if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
|
||||
@ -2445,7 +2484,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
|
||||
|
||||
|
||||
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
|
||||
|
||||
return true;
|
||||
MatrixXd P1 = H * state_server.state_cov * H.transpose();
|
||||
|
||||
MatrixXd P2 = Feature::observation_noise *
|
||||
@ -2453,8 +2492,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
|
||||
|
||||
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
|
||||
|
||||
//cout << "gate: " << dof << " " << gamma << " " <<
|
||||
//chi_squared_test_table[dof] << endl;
|
||||
cout << "gate: " << dof << " " << gamma << " " <<
|
||||
chi_squared_test_table[dof] << endl;
|
||||
|
||||
if (chi_squared_test_table[dof] == 0)
|
||||
return false;
|
||||
@ -2562,7 +2601,7 @@ void MsckfVio::removeLostFeatures() {
|
||||
MatrixXd twoH_xj;
|
||||
VectorXd twor_j;
|
||||
|
||||
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true)
|
||||
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
|
||||
{
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
@ -2747,7 +2786,7 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
|
||||
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
|
||||
{
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
|
||||
pstack_cntr += pH_xj.rows();
|
||||
@ -2769,6 +2808,7 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
|
||||
twostack_cntr += twoH_xj.rows();
|
||||
}
|
||||
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
feature.observations.erase(cam_id);
|
||||
}
|
||||
@ -2928,7 +2968,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
|
||||
{
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
|
||||
pstack_cntr += pH_xj.rows();
|
||||
@ -2937,7 +2977,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
|
||||
|
||||
|
||||
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;
|
||||
@ -2949,7 +2989,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
|
||||
twostack_cntr += twoH_xj.rows();
|
||||
}
|
||||
|
||||
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
feature.observations.erase(cam_id);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user