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

fixed visualization issue

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-12 16:36:36 +02:00
parent 876fa7617c
commit 5a80f97b68
6 changed files with 28 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,7 +9,7 @@ cam0:
0, 0, 0, 1.000000000000000] 0, 0, 0, 1.000000000000000]
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
distortion_model: radtan distortion_model: pre-radtan
intrinsics: [458.654, 457.296, 367.215, 248.375] intrinsics: [458.654, 457.296, 367.215, 248.375]
resolution: [752, 480] resolution: [752, 480]
timeshift_cam_imu: 0.0 timeshift_cam_imu: 0.0
@ -26,7 +26,7 @@ cam1:
0, 0, 0, 1.000000000000000] 0, 0, 0, 1.000000000000000]
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] distortion_coeffs: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05]
distortion_model: radtan distortion_model: pre-radtan
intrinsics: [457.587, 456.134, 379.999, 255.238] intrinsics: [457.587, 456.134, 379.999, 255.238]
resolution: [752, 480] resolution: [752, 480]
timeshift_cam_imu: 0.0 timeshift_cam_imu: 0.0

3
include/msckf_vio/feature.hpp
View File

@ -823,11 +823,10 @@ bool Feature::VisualizePatch(
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
cv::Point2f p_f(observations.find(cam_state_id)->second(0)*cam.intrinsics[0] + cam.intrinsics[2],observations.find(cam_state_id)->second(1)*cam.intrinsics[1] + cam.intrinsics[3]); cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
// move to real pixels // move to real pixels
p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
for(int i = 0; i<N; i++) for(int i = 0; i<N; i++)
{ {
for(int j = 0; j<N ; j++) for(int j = 0; j<N ; j++)

6
launch/image_processor_tinytum.launch
View File

@ -27,9 +27,9 @@
<param name="ransac_threshold" value="3"/> <param name="ransac_threshold" value="3"/>
<param name="stereo_threshold" value="5"/> <param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/imu02"/> <remap from="~imu" to="/imu0"/>
<remap from="~cam0_image" to="/cam0/image_raw2"/> <remap from="~cam0_image" to="/cam0/image_raw"/>
<remap from="~cam1_image" to="/cam1/image_raw2"/> <remap from="~cam1_image" to="/cam1/image_raw"/>
</node> </node>

4
launch/msckf_vio_euroc.launch
View File

@ -19,11 +19,11 @@
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two --> <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="FILTER" value="0"/> <param name="FILTER" value="1"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/> <param name="StreamPause" value="true"/>
<param name="PrintImages" value="false"/> <param name="PrintImages" value="true"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="3"/> <param name="patch_size_n" value="3"/>

12
src/image_processor.cpp
View File

@ -226,8 +226,16 @@ void ImageProcessor::stereoCallback(
ros::Time start_time = ros::Time::now(); ros::Time start_time = ros::Time::now();
image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); cv::Mat new_cam0;
cv::Mat new_cam1;
image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
new_cam0.copyTo(cam0_curr_img_ptr->image);
new_cam1.copyTo(cam1_curr_img_ptr->image);
//ROS_INFO("Publishing: %f", //ROS_INFO("Publishing: %f",
// (ros::Time::now()-start_time).toSec()); // (ros::Time::now()-start_time).toSec());

16
src/msckf_vio.cpp
View File

@ -544,13 +544,14 @@ void MsckfVio::manageMovingWindow(
cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8); sensor_msgs::image_encodings::MONO8);
image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); cv::Mat new_cam0;
image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); cv::Mat new_cam1;
image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
// save image information into moving window // save image information into moving window
cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone();
cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone();
cv::Mat xder; cv::Mat xder;
cv::Mat yder; cv::Mat yder;
@ -1704,7 +1705,8 @@ bool MsckfVio::PhotometricPatchPointJacobian(
dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0];
dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1];
//cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
cout << dI_dhj(1, 2) << ", " << dI_dhj(1, 3) << endl;
// add jacobian // add jacobian
@ -1782,6 +1784,8 @@ bool MsckfVio::PhotometricMeasurementJacobian(
if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
return false; return false;
cout << "-\n" << r_photo << "-" << endl;
//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;