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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/msckf_vio/feature.hpp
View File

@ -498,15 +498,9 @@ double Feature::cvKernel(
double delta = 0;
if(type == "Sobel_x")
{
std::cout << "image value x" << ((double)xderImage.at<short>(pose.y, pose.x))/255. << std::endl;
delta = ((double)xderImage.at<short>(pose.y, pose.x))/255.;
}
else if (type == "Sobel_y")
{
std::cout << "image value y" << ((double)yderImage.at<short>(pose.y, pose.x))/255. << std::endl;
delta = ((double)yderImage.at<short>(pose.y, pose.x))/255.;
}
return delta;
}

4
launch/msckf_vio_tum.launch
View File

@ -21,10 +21,10 @@
<param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs -->
<param name="PrintImages" 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)"/>

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();