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

added minor visualization changes

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-02 17:02:44 +02:00
parent acbcf79417
commit ee40dff740
3 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/msckf_vio/feature.hpp
View File

@ -491,6 +491,12 @@ bool Feature::VisualizePatch(
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
// write feature position
std::stringstream pos_s;
pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
cv::putText(cam0.featureVisu, pos_s.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
// create line? // create line?
//save image //save image

2
launch/msckf_vio_tum.launch
View File

@ -19,7 +19,7 @@
<!-- Photometry Flag--> <!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/> <param name="PHOTOMETRIC" value="true"/>
<param name="PrintImages" value="false"/> <param name="PrintImages" value="true"/>
<param name="patch_size_n" value="7"/> <param name="patch_size_n" value="7"/>
<!-- Calibration parameters --> <!-- Calibration parameters -->

10
src/msckf_vio.cpp
View File

@ -1836,12 +1836,22 @@ void MsckfVio::publish(const ros::Time& time) {
Eigen::Vector3d body_velocity = Eigen::Vector3d body_velocity =
IMUState::T_imu_body.linear() * imu_state.velocity; IMUState::T_imu_body.linear() * imu_state.velocity;
// Generate camera frame form IMU Frame
Eigen::Isometry3d T_i_c = Eigen::Isometry3d::Identity();
T_i_c.linear() = imu_state.R_imu_cam0;
T_i_c.translation() = imu_state.t_cam0_imu;
// Publish tf // Publish tf
if (publish_tf) { if (publish_tf) {
tf::Transform T_b_w_tf; tf::Transform T_b_w_tf;
tf::transformEigenToTF(T_b_w, T_b_w_tf); tf::transformEigenToTF(T_b_w, T_b_w_tf);
tf_pub.sendTransform(tf::StampedTransform( tf_pub.sendTransform(tf::StampedTransform(
T_b_w_tf, time, fixed_frame_id, child_frame_id)); T_b_w_tf, time, fixed_frame_id, child_frame_id));
tf::Transform T_i_c_tf;
tf::transformEigenToTF(T_i_c, T_i_c_tf);
tf_pub.sendTransform(tf::StampedTransform(
T_i_c_tf, time, child_frame_id, "camera"));
} }
// Publish the odometry // Publish the odometry