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

added ground truth visualization

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-28 12:13:02 +02:00
parent 53e2a5d524
commit 7b7e966217
3 changed files with 33 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
src/msckf_vio.cpp
View File

@@ -261,6 +261,7 @@ bool MsckfVio::createRosIO() {
nh.setParam("/play_bag", true);
odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
truth_odom_pub = nh.advertise<nav_msgs::Odometry>("true_odom", 10);
feature_pub = nh.advertise<sensor_msgs::PointCloud2>(
"feature_point_cloud", 10);
marker_pub = nh.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 10);
@@ -351,11 +352,6 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){
void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){
static bool first_truth_odom_msg = true;
// errorstate
/*if(not ErrorState)
return;
*/
// If this is the first mocap odometry messsage, set
// the initial frame.
if (first_truth_odom_msg) {
@@ -1429,7 +1425,7 @@ void MsckfVio::twodotFeatureJacobian(
{
ofstream myfile;
myfile.open("/home/raphael/dev/octave/log2octave");
myfile.open("/home/raphael/dev/octave/two2octave");
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
<< "# name: Hx\n"
<< "# type: matrix\n"
@@ -1450,6 +1446,13 @@ void MsckfVio::twodotFeatureJacobian(
<< "# columns: " << 1 << "\n"
<< r_i << endl;
myfile << "# name: A\n"
<< "# type: matrix\n"
<< "# rows: " << A_null_space.rows() << "\n"
<< "# columns: " << 1 << "\n"
<< A_null_space << endl;
myfile.close();
std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true);
@@ -1633,7 +1636,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
if(PRINTIMAGES)
{
feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
//feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
}
@@ -1745,8 +1748,8 @@ void MsckfVio::PhotometricFeatureJacobian(
<< "# columns: " << 1 << "\n"
<< r_i << endl;
myfile << "# name: r\n"
<< "# type: A\n"
myfile << "# name: A\n"
<< "# type: matrix\n"
<< "# rows: " << A_null_space.rows() << "\n"
<< "# columns: " << A_null_space.cols() << "\n"
<< A_null_space << endl;
@@ -1920,8 +1923,8 @@ void MsckfVio::featureJacobian(
<< "# columns: " << 1 << "\n"
<< r_j << endl;
myfile << "# name: r\n"
<< "# type: A\n"
myfile << "# name: A\n"
<< "# type: matrix\n"
<< "# rows: " << A.rows() << "\n"
<< "# columns: " << A.cols() << "\n"
<< A << endl;
@@ -2915,6 +2918,23 @@ void MsckfVio::publish(const ros::Time& time) {
T_i_c_tf, time, child_frame_id, "camera"));
}
// Publish true odometry
// Ground truth odometry.
nav_msgs::Odometry truth_odom_msg;
truth_odom_msg.header.stamp = time;
truth_odom_msg.header.frame_id = fixed_frame_id;
truth_odom_msg.child_frame_id = child_frame_id+"_true";
Eigen::Isometry3d true_T_i_w = Eigen::Isometry3d::Identity();
true_T_i_w.linear() = quaternionToRotation(
true_state_server.imu_state.orientation).transpose();
true_T_i_w.translation() = true_state_server.imu_state.position;
tf::poseEigenToMsg(true_T_i_w, truth_odom_msg.pose.pose);
Eigen::Isometry3d true_T_b_w = IMUState::T_imu_body * true_T_i_w *
IMUState::T_imu_body.inverse();
tf::poseEigenToMsg(true_T_b_w, truth_odom_msg.pose.pose);
truth_odom_pub.publish(truth_odom_msg);
// Publish the odometry
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = time;