ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

small fixes

This commit is contained in:
Vladyslav Usenko 2019-07-31 13:55:33 +02:00
parent 7694fc1825
commit aa0b7826fe
2 changed files with 29 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
include/basalt/io/dataset_io_rosbag.h
View File

@ -47,6 +47,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#undef private
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
@ -207,7 +208,8 @@ class RosbagIO : public DatasetIoInterface {
} else if (info->datatype == std::string("sensor_msgs/Imu")) {
imu_topic = info->topic;
} else if (info->datatype ==
std::string("geometry_msgs/TransformStamped")) {
std::string("geometry_msgs/TransformStamped") ||
info->datatype == std::string("geometry_msgs/PoseStamped")) {
mocap_topic = info->topic;
} else if (info->datatype == std::string("geometry_msgs/PointStamped")) {
point_topic = info->topic;
@ -288,6 +290,26 @@ class RosbagIO : public DatasetIoInterface {
if (mocap_topic == topic) {
geometry_msgs::TransformStampedConstPtr mocap_msg =
m.instantiate<geometry_msgs::TransformStamped>();
// Try different message type if instantiate did not work
if (!mocap_msg) {
geometry_msgs::PoseStampedConstPtr mocap_pose_msg =
m.instantiate<geometry_msgs::PoseStamped>();
geometry_msgs::TransformStampedPtr mocap_new_msg(
new geometry_msgs::TransformStamped);
mocap_new_msg->header = mocap_pose_msg->header;
mocap_new_msg->transform.rotation = mocap_pose_msg->pose.orientation;
mocap_new_msg->transform.translation.x =
mocap_pose_msg->pose.position.x;
mocap_new_msg->transform.translation.y =
mocap_pose_msg->pose.position.y;
mocap_new_msg->transform.translation.z =
mocap_pose_msg->pose.position.z;
mocap_msg = mocap_new_msg;
}
int64_t time = mocap_msg->header.stamp.toNSec();
mocap_msgs.push_back(mocap_msg);
@ -299,6 +321,7 @@ class RosbagIO : public DatasetIoInterface {
if (point_topic == topic) {
geometry_msgs::PointStampedConstPtr mocap_msg =
m.instantiate<geometry_msgs::PointStamped>();
int64_t time = mocap_msg->header.stamp.toNSec();
point_msgs.push_back(mocap_msg);

8
src/vio.cpp
View File

@ -568,9 +568,11 @@ void draw_scene() {
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor3ubv(cam_color);
Eigen::vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
vio_t_w_i.begin() + show_frame);
pangolin::glDrawLineStrip(sub_gt);
if (!vio_t_w_i.empty()) {
Eigen::vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
vio_t_w_i.begin() + show_frame);
pangolin::glDrawLineStrip(sub_gt);
}
glColor3ubv(gt_color);
if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i);