public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

can now choose in which coordinate frame to publish the pose. by default the pose of the world origin is published in the camera frame of reference

Browse Source
This commit is contained in:
Christian Forster
2014-04-29 13:51:07 +02:00
parent 335a25511f
commit 39bd1347f3
2 changed files with 109 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo_ros/include/svo_ros/visualizer.h
+18 -4
View File
@@ -42,8 +42,7 @@ class FrameHandlerMono;
typedef boost::shared_ptr<Frame> FramePtr;
/// This class bundles all functions to publish visualisation messages,
/// trace data and output debugging information.
/// This class bundles all functions to publish visualisation messages.
class Visualizer
{
public:
@@ -59,15 +58,30 @@ public:
ros::Publisher pub_dense_;
image_transport::Publisher pub_images_;
tf::TransformBroadcaster br_;
bool publish_world_in_cam_frame_;
SE3 T_world_from_vision_;
Visualizer();
~Visualizer() {};
void visualizeMarkers(const FramePtr& frame, const set<FramePtr>& core_kfs, const Map& map);
void publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono& slam, const double timestamp);
void publishMinimal(
const cv::Mat& img,
const FramePtr& frame,
const FrameHandlerMono& slam,
const double timestamp);
void visualizeMarkers(
const FramePtr& frame,
const set<FramePtr>& core_kfs,
const Map& map);
void publishMapRegion(set<FramePtr> frames);
void removeDeletedPts(const Map& map);
void displayKeyframeWithMps(const FramePtr& frame, int ts);
void exportToDense(const FramePtr& frame);
};
svo_ros/src/visualizer.cpp
+91 -66
View File
@@ -44,7 +44,8 @@ Visualizer() :
img_pub_level_(vk::getParam<int>("svo/image_publishing_level", 0)),
img_pub_nth_(vk::getParam<int>("svo/publish_every_nth_img", 1)),
dense_pub_nth_(vk::getParam<int>("svo/publish_every_nth_dense_input", 1)),
T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()),
publish_world_in_cam_frame_(vk::getParam<bool>("svo/publish_world_in_cam_frame", true))
{
// Init ROS Marker Publishers
pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
@@ -58,25 +59,11 @@ Visualizer() :
pub_images_ = it.advertise("image", 10);
}
void Visualizer::
visualizeMarkers(const FramePtr& frame, const set<FramePtr>& core_kfs, const Map& map)
{
if(frame != NULL)
{
// publish camera pose tf
vk::output_helper::publishTfTransform(frame->T_f_w_, ros::Time(frame->timestamp_), "cam_pos", "world", br_);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
{
vk::output_helper::publishHexacopterMarker(pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_), 1, 0, 0.3, Vector3d(0.,0.,1.));
vk::output_helper::publishPointMarker(pub_points_, frame->pos(), "trajectory", ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
publishMapRegion(core_kfs);
removeDeletedPts(map);
}
}
}
void Visualizer::
publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono& slam, const double timestamp)
void Visualizer::publishMinimal(
const cv::Mat& img,
const FramePtr& frame,
const FrameHandlerMono& slam,
const double timestamp)
{
++trace_id_;
std_msgs::Header header_msg;
@@ -84,7 +71,7 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
header_msg.seq = trace_id_;
header_msg.stamp = ros::Time(timestamp);
// publish svo info msgs
// publish info msg.
if(pub_info_.getNumSubscribers() > 0)
{
svo_msgs::Info msg_info;
@@ -102,17 +89,31 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
pub_info_.publish(msg_info);
}
// publish pyramid-image every nth-frame
if(frame != NULL && img_pub_nth_ > 0
&& trace_id_%img_pub_nth_ == 0 && pub_images_.getNumSubscribers() > 0)
if(frame == 0)
{
if(pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_PAUSED)
{
// Display image when slam is not running.
cv_bridge::CvImage img_msg;
img_msg.header.stamp = ros::Time::now();
img_msg.header.frame_id = "/image";
img_msg.image = img;
img_msg.encoding = sensor_msgs::image_encodings::MONO8;
pub_images_.publish(img_msg.toImageMsg());
}
return;
}
// Publish pyramid-image every nth-frame.
if(img_pub_nth_ > 0 && trace_id_%img_pub_nth_ == 0 && pub_images_.getNumSubscribers() > 0)
{
const int scale = (1<<img_pub_level_);
cv::Mat img_rgb(frame->img_pyr_[img_pub_level_].size(), CV_8UC3);
cv::cvtColor(frame->img_pyr_[img_pub_level_], img_rgb, CV_GRAY2RGB);
// during initialization, draw lines
if(slam.stage() == FrameHandlerBase::STAGE_SECOND_FRAME)
{
// During initialization, draw lines.
const vector<cv::Point2f>& px_ref(slam.initFeatureTrackRefPx());
const vector<cv::Point2f>& px_cur(slam.initFeatureTrackCurPx());
for(vector<cv::Point2f>::const_iterator it_ref=px_ref.begin(), it_cur=px_cur.begin();
@@ -122,7 +123,6 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
cv::Point2f(it_ref->x/scale, it_ref->y/scale), cv::Scalar(0,255,0), 2);
}
// draw features on frame
if(img_pub_level_ == 0)
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
cv::rectangle(img_rgb,
@@ -142,51 +142,74 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
cv::Scalar(0,255,0), CV_FILLED);
// publish image
cv_bridge::CvImage img_msg;
img_msg.header = header_msg;
img_msg.image = img_rgb;
img_msg.encoding = sensor_msgs::image_encodings::BGR8;
pub_images_.publish(img_msg.toImageMsg());
}
else if(frame == NULL && pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_PAUSED)
{
// display image when slam is not running
cv_bridge::CvImage img_msg;
img_msg.header.stamp = ros::Time::now();
img_msg.header.frame_id = "/image";
img_msg.image = img;
img_msg.encoding = sensor_msgs::image_encodings::MONO8;
pub_images_.publish(img_msg.toImageMsg());
}
// publish pose (camera in world frame)
if(pub_pose_.getNumSubscribers() > 0 && frame != NULL)
if(pub_pose_.getNumSubscribers() > 0)
{
// transform from vision-frame to world-frame
SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
Quaterniond q_world_from_cam(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
const Vector3d& p_cam_in_world(T_world_from_cam.translation());
Matrix<double,6,6> Cov_in_world = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
// publish message
Quaterniond q;
Vector3d p;
Matrix<double,6,6> Cov;
if(publish_world_in_cam_frame_)
{
// publish world in cam frame
SE3 T_cam_from_world(frame->T_f_w_* T_world_from_vision_);
q = Quaterniond(T_cam_from_world.rotation_matrix());
p = T_cam_from_world.translation();
Cov = frame->Cov_;
}
else
{
// publish cam in world frame
SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
p = T_world_from_cam.translation();
Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
}
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose->header = header_msg;
msg_pose->pose.pose.position.x = p_cam_in_world[0];
msg_pose->pose.pose.position.y = p_cam_in_world[1];
msg_pose->pose.pose.position.z = p_cam_in_world[2];
msg_pose->pose.pose.orientation.x = q_world_from_cam.x();
msg_pose->pose.pose.orientation.y = q_world_from_cam.y();
msg_pose->pose.pose.orientation.z = q_world_from_cam.z();
msg_pose->pose.pose.orientation.w = q_world_from_cam.w();
msg_pose->pose.pose.position.x = p[0];
msg_pose->pose.pose.position.y = p[1];
msg_pose->pose.pose.position.z = p[2];
msg_pose->pose.pose.orientation.x = q.x();
msg_pose->pose.pose.orientation.y = q.y();
msg_pose->pose.pose.orientation.z = q.z();
msg_pose->pose.pose.orientation.w = q.w();
for(size_t i=0; i<36; ++i)
msg_pose->pose.covariance[i] = Cov_in_world(i%6, i/6);
msg_pose->pose.covariance[i] = Cov(i%6, i/6);
pub_pose_.publish(msg_pose);
}
}
void Visualizer::
publishMapRegion(set<FramePtr> frames)
void Visualizer::visualizeMarkers(
const FramePtr& frame,
const set<FramePtr>& core_kfs,
const Map& map)
{
if(frame != NULL)
{
vk::output_helper::publishTfTransform(
frame->T_f_w_*T_world_from_vision_.inverse(),
ros::Time(frame->timestamp_), "cam_pos", "world", br_);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
{
vk::output_helper::publishHexacopterMarker(
pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_),
1, 0, 0.3, Vector3d(0.,0.,1.));
vk::output_helper::publishPointMarker(
pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
publishMapRegion(core_kfs);
removeDeletedPts(map);
}
}
}
void Visualizer::publishMapRegion(set<FramePtr> frames)
{
if(pub_points_.getNumSubscribers() > 0)
{
@@ -196,8 +219,7 @@ publishMapRegion(set<FramePtr> frames)
}
}
void Visualizer::
removeDeletedPts(const Map& map)
void Visualizer::removeDeletedPts(const Map& map)
{
if(pub_points_.getNumSubscribers() > 0)
{
@@ -206,11 +228,13 @@ removeDeletedPts(const Map& map)
}
}
void Visualizer::
displayKeyframeWithMps(const FramePtr& frame, int ts)
void Visualizer::displayKeyframeWithMps(const FramePtr& frame, int ts)
{
// publish keyframe
vk::output_helper::publishFrameMarker(pub_frames_, frame->T_f_w_.inverse().rotation_matrix(), frame->pos(), "kfs", ros::Time::now(), frame->id_*10, 0, 0.015);
SE3 T_world_cam(T_world_from_vision_*frame->T_f_w_.inverse());
vk::output_helper::publishFrameMarker(
pub_frames_, T_world_cam.rotation_matrix(),
T_world_cam.translation(), "kfs", ros::Time::now(), frame->id_*10, 0, 0.015);
// publish point cloud and links
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
@@ -221,13 +245,14 @@ displayKeyframeWithMps(const FramePtr& frame, int ts)
if((*it)->point->last_published_ts_ == ts)
continue;
vk::output_helper::publishPointMarker(pub_points_, (*it)->point->pos_, "pts", ros::Time::now(), (*it)->point->id_, 0, 0.005, Vector3d(1.0, 0., 1.0));
vk::output_helper::publishPointMarker(
pub_points_, T_world_from_vision_*(*it)->point->pos_, "pts",
ros::Time::now(), (*it)->point->id_, 0, 0.005, Vector3d(1.0, 0., 1.0));
(*it)->point->last_published_ts_ = ts;
}
}
void Visualizer::
exportToDense(const FramePtr& frame)
void Visualizer::exportToDense(const FramePtr& frame)
{
// publish air_ground_msgs
if(frame != NULL && dense_pub_nth_ > 0
@@ -252,8 +277,8 @@ exportToDense(const FramePtr& frame)
if((*it)->point==NULL)
continue;
Vector3d pos = frame->T_f_w_*(*it)->point->pos_;
min_z = min(pos[2], min_z);
max_z = max(pos[2], max_z);
min_z = fmin(pos[2], min_z);
max_z = fmax(pos[2], max_z);
}
msg.min_depth = (float) min_z;
msg.max_depth = (float) max_z;