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; double delta = 0;
if(type == "Sobel_x") 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.; delta = ((double)xderImage.at<short>(pose.y, pose.x))/255.;
}
else if (type == "Sobel_y") 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.; delta = ((double)yderImage.at<short>(pose.y, pose.x))/255.;
}
return delta; return delta;
} }

4
launch/msckf_vio_tum.launch
View File

@ -21,10 +21,10 @@
<param name="PHOTOMETRIC" value="true"/> <param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="PrintImages" value="true"/> <param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="3"/> <param name="patch_size_n" value="5"/>
<!-- Calibration parameters --> <!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/> <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(); utils::getTransformEigen(nh, "T_imu_body").inverse();
// Maximum number of camera states to be stored // 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; //cam_state_size = max_cam_state_size;
ROS_INFO("==========================================="); ROS_INFO("===========================================");
ROS_INFO("fixed frame id: %s", fixed_frame_id.c_str()); 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); // 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); // 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, 0) = dx * cam0.intrinsics[0];
dI_dhj(0, 1) = dy;// /(pixelDistance.y); dI_dhj(0, 1) = dy * cam0.intrinsics[1];
gradientVector.x += dx;
gradientVector.y += dy;
residualVector.x += dx * r_photo(count);
residualVector.y += dy * r_photo(count);
res_sum += r_photo(count);
//dh / d{}^Cp_{ij} //dh / d{}^Cp_{ij}
dh_dCpij(0, 0) = 1 / p_c0(2); dh_dCpij(0, 0) = 1 / p_c0(2);
@ -1424,7 +1416,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
{ {
feature.MarkerGeneration(marker_pub, state_server.cam_states); feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); 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; return;
@ -1767,7 +1759,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
// Compute the error of the state. // Compute the error of the state.
VectorXd delta_x = K * r; VectorXd delta_x = K * r;
// Update the IMU state. // Update the IMU state.
const VectorXd& delta_x_imu = delta_x.head<21>(); 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) { bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return 1;
MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P1 = H * state_server.state_cov * H.transpose();
@ -2085,7 +2077,7 @@ void MsckfVio::pruneLastCamStateBuffer()
else else
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); 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; H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows(); stack_cntr += H_xj.rows();
@ -2232,7 +2224,7 @@ void MsckfVio::pruneCamStateBuffer() {
else else
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); 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; H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows(); stack_cntr += H_xj.rows();