41 #ifndef INCLUDE_OKVIS_PUBLISHER_HPP_
42 #define INCLUDE_OKVIS_PUBLISHER_HPP_
47 #include <pcl/point_types.h>
49 #include <geometry_msgs/PoseStamped.h>
50 #include <sensor_msgs/PointCloud2.h>
51 #include <visualization_msgs/Marker.h>
52 #pragma GCC diagnostic push
53 #pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
54 #include <opencv2/core/core.hpp>
56 #include <tf/transform_broadcaster.h>
57 #include <pcl_ros/point_cloud.h>
58 #pragma GCC diagnostic pop
59 #include <nav_msgs/Odometry.h>
60 #include <nav_msgs/Path.h>
61 #include <image_transport/image_transport.h>
76 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
132 const Eigen::Vector3d& omega_S);
157 void setImages(
const std::vector<cv::Mat> & images);
192 const Eigen::Matrix<double, 9, 1> & speedAndBiases,
193 const Eigen::Matrix<double, 3, 1> & omega_S);
216 const Eigen::Matrix<double, 9, 1> & speedAndBiases,
217 const Eigen::Matrix<double, 3, 1> & omega_S);
231 const Eigen::Matrix<double, 9, 1> & speedAndBiases,
232 const Eigen::Matrix<double, 3, 1> & omega_S,
234 Eigen::aligned_allocator<okvis::kinematics::Transformation> > & extrinsics);
void setTime(const okvis::Time &t)
Set the time for the next message to be published.
Definition: Publisher.hpp:151
ros::NodeHandle * nh_
The node handle.
Definition: Publisher.hpp:268
std::vector< cv::Mat > images_
The images.
Definition: Publisher.hpp:291
ros::Time lastOdometryTime2_
Timestamp of the last published odometry message. (publishOdometry())
Definition: Publisher.hpp:298
bool writeLandmarksCsvDescription()
Write CSV header for landmarks file.
Definition: Publisher.cpp:133
void publishPose()
Publish the pose.
Definition: Publisher.cpp:462
okvis::MapPointVector pointsMatched2_
Matched points vector.
Definition: Publisher.hpp:287
ros::Time _t
Header timestamp.
Definition: Publisher.hpp:284
bool setCsvFile(std::fstream &csvFile)
Set an odometry output CSV file.
Definition: Publisher.cpp:146
void publishTransform()
Publish the T_WS transform.
Definition: Publisher.cpp:484
pcl::PointCloud< pcl::PointXYZRGB > pointsUnmatched_
Point cloud for unmatched points.
Definition: Publisher.hpp:289
bool writeCsvDescription()
Write CSV header.
Definition: Publisher.cpp:117
okvis::VioParameters parameters_
All the parameters including publishing options.
Definition: Publisher.hpp:301
ros::Publisher pubPointsMatched_
The publisher for matched points.
Definition: Publisher.hpp:270
nav_msgs::Path path_
The path message.
Definition: Publisher.hpp:292
uint32_t sec
Definition: Time.hpp:128
void csvSaveFullStateWithExtrinsicsAsCallback(const okvis::Time &t, const okvis::kinematics::Transformation &T_WS, const Eigen::Matrix< double, 9, 1 > &speedAndBiases, const Eigen::Matrix< double, 3, 1 > &omega_S, const std::vector< okvis::kinematics::Transformation, Eigen::aligned_allocator< okvis::kinematics::Transformation > > &extrinsics)
Set and write full state including camera extrinsics to file.
Definition: Publisher.cpp:543
~Publisher()
Definition: Publisher.cpp:62
Publisher()
Default constructor.
Definition: Publisher.cpp:56
void setImages(const std::vector< cv::Mat > &images)
Set the images to be published next.
Definition: Publisher.cpp:622
visualization_msgs::Marker meshMsg_
Mesh message.
Definition: Publisher.hpp:293
void csvSaveFullStateAsCallback(const okvis::Time &t, const okvis::kinematics::Transformation &T_WS, const Eigen::Matrix< double, 9, 1 > &speedAndBiases, const Eigen::Matrix< double, 3, 1 > &omega_S)
Set and write full state to CSV file.
Definition: Publisher.cpp:515
void publishFullStateAsCallback(const okvis::Time &t, const okvis::kinematics::Transformation &T_WS, const Eigen::Matrix< double, 9, 1 > &speedAndBiases, const Eigen::Matrix< double, 3, 1 > &omega_S)
Set and publish full state.
Definition: Publisher.cpp:501
Header file for the TimeBase, Time and WallTime class.
geometry_msgs::TransformStamped poseMsg_
Pose message.
Definition: Publisher.hpp:285
pcl::PointCloud< pcl::PointXYZRGB > pointsTransferred_
Point cloud for transferred/marginalised points.
Definition: Publisher.hpp:290
std::vector< MapPoint, Eigen::aligned_allocator< MapPoint > > MapPointVector
Definition: FrameTypedefs.hpp:172
void setPose(const okvis::kinematics::Transformation &T_WS)
Set the pose message that is published next.
Definition: Publisher.cpp:198
std::shared_ptr< std::fstream > csvLandmarksFile_
CSV file to save landmarks in.
Definition: Publisher.hpp:306
void publishStateAsCallback(const okvis::Time &t, const okvis::kinematics::Transformation &T_WS)
Set and publish pose.
Definition: Publisher.cpp:493
void csvSaveLandmarksAsCallback(const okvis::Time &t, const okvis::MapPointVector &actualLandmarks, const okvis::MapPointVector &transferredLandmarks)
Set and write landmarks to file.
Definition: Publisher.cpp:592
void setPath(const okvis::kinematics::Transformation &T_WS)
Add a pose to the path that is published next. The path contains a maximum of pathLength_ (change wit...
Definition: Publisher.cpp:633
ros::Publisher pubTransform_
The publisher for the transform.
Definition: Publisher.hpp:275
void publishImages()
Publish the last set images.
Definition: Publisher.cpp:673
void publishOdometry()
Publish the last set odometry.
Definition: Publisher.cpp:473
std::shared_ptr< std::fstream > csvFile_
CSV file to save state in.
Definition: Publisher.hpp:305
Struct to combine all parameters and settings.
Definition: Parameters.hpp:280
ros::Publisher pubPointsTransferred_
The publisher for transferred/marginalised points.
Definition: Publisher.hpp:272
void setOdometry(const okvis::kinematics::Transformation &T_WS, const okvis::SpeedAndBiases &speedAndBiases, const Eigen::Vector3d &omega_S)
Set the odometry message that is published next.
Definition: Publisher.cpp:274
Eigen::Matrix< double, 9, 1 > SpeedAndBiases
Definition: FrameTypedefs.hpp:233
void setNodeHandle(ros::NodeHandle &nh)
Set the node handle and advertise topics.
Definition: Publisher.cpp:90
nav_msgs::Odometry odometryMsg_
Odometry message.
Definition: Publisher.hpp:286
ros::Publisher pubPointsUnmatched_
The publisher for unmatched points.
Definition: Publisher.hpp:271
std::vector< image_transport::ImageTransport > imageTransportVector_
The image transporters.
Definition: Publisher.hpp:278
void publishLandmarksAsCallback(const okvis::Time &t, const okvis::MapPointVector &actualLandmarks, const okvis::MapPointVector &transferredLandmarks)
Set and publish landmarks.
Definition: Publisher.cpp:580
ros::Publisher pubMesh_
The publisher for a robot / camera mesh.
Definition: Publisher.hpp:276
bool setLandmarksCsvFile(std::fstream &csvFile)
Set a CVS file where the landmarks will be saved to.
Definition: Publisher.cpp:171
pcl::PointCloud< pcl::PointXYZRGB > pointsMatched_
Point cloud for matched points.
Definition: Publisher.hpp:288
ros::Publisher pubPath_
The publisher for the path.
Definition: Publisher.hpp:274
void setParameters(const okvis::VioParameters ¶meters)
Set the parameters.
Definition: Publisher.hpp:136
void publishPoints()
Publish the last set points.
Definition: Publisher.cpp:614
ros::Time lastTransfromTime_
Timestamp of the last published transform. (publishTransform())
Definition: Publisher.hpp:299
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
This file contains struct definitions that encapsulate parameters and settings.
This class handles the publishing to either ROS topics or files.
Definition: Publisher.hpp:74
uint32_t nsec
Definition: Time.hpp:128
uint32_t ctr2_
The counter for the amount of transferred points. Used for the seq parameter in the header...
Definition: Publisher.hpp:303
void setPoints(const okvis::MapPointVector &pointsMatched, const okvis::MapPointVector &pointsUnmatched, const okvis::MapPointVector &pointsTransferred)
Set the points that are published next.
Definition: Publisher.cpp:359
tf::TransformBroadcaster pubTf_
The transform broadcaster.
Definition: Publisher.hpp:269
ros::Time lastOdometryTime_
Timestamp of the last broadcasted transform. (publishPose())
Definition: Publisher.hpp:297
void publishPath()
Publish the last set path.
Definition: Publisher.cpp:702
std::vector< image_transport::Publisher > pubImagesVector_
The publisher for the images.
Definition: Publisher.hpp:277
This file contains useful typedefs and structs related to frames.
ros::Publisher pubObometry_
The publisher for the odometry.
Definition: Publisher.hpp:273