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

optimize the visualizer

Browse Source
This commit is contained in:
gfuse
2021-04-05 18:41:37 +02:00
parent da17901faa
commit e9256c9903
5 changed files with 62 additions and 308 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo_ros/include/svo_ros/visualizer.h
-13
View File
@@ -48,19 +48,11 @@ class Visualizer
public: public:
ros::NodeHandle pnh_; ros::NodeHandle pnh_;
size_t trace_id_; size_t trace_id_;
size_t img_pub_level_;
size_t img_pub_nth_;
size_t dense_pub_nth_;
ros::Publisher pub_frames_; ros::Publisher pub_frames_;
ros::Publisher pub_points_; ros::Publisher pub_points_;
ros::Publisher pub_pose_; ros::Publisher pub_pose_;
ros::Publisher pub_info_; ros::Publisher pub_info_;
ros::Publisher pub_dense_;
image_transport::Publisher pub_images_;
tf::TransformBroadcaster br_; tf::TransformBroadcaster br_;
bool publish_world_in_cam_frame_;
bool publish_map_every_frame_;
ros::Duration publish_points_display_time_;
SE3 T_world_from_vision_; SE3 T_world_from_vision_;
Visualizer(); Visualizer();
@@ -78,13 +70,8 @@ public:
const set<FramePtr>& core_kfs, const set<FramePtr>& core_kfs,
const Map& map); const Map& map);
void publishMapRegion(set<FramePtr> frames);
void removeDeletedPts(const Map& map); void removeDeletedPts(const Map& map);
void displayKeyframeWithMps(const FramePtr& frame, int ts);
void exportToDense(const FramePtr& frame);
}; };
} // end namespace svo } // end namespace svo
svo_ros/launch/usb_cam.launch → svo_ros/launch/Px30.launch
+1 -1
View File
@@ -9,7 +9,7 @@
<rosparam file="$(find svo_ros)/param/camera_pinhole.yaml" /> <rosparam file="$(find svo_ros)/param/camera_pinhole.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate --> <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find svo_ros)/param/vo_fast.yaml" /> <rosparam file="$(find svo_ros)/param/px30.yaml" />
<!-- Initial camera orientation, make it point downwards --> <!-- Initial camera orientation, make it point downwards -->
<param name="init_rx" value="0.00" /> <param name="init_rx" value="0.00" />
svo_ros/param/px30.yaml
+8
View File
@@ -0,0 +1,8 @@
cam_model: ATAN
cam_width: 640
cam_height: 480
cam_fx: 0.463632
cam_fy: 0.823661
cam_cx: 0.497465
cam_cy: 0.498443
cam_d0: 1.01687
svo_ros/src/visualizer.cpp
+5 -193
View File
@@ -22,18 +22,9 @@
#include <svo/feature.h> #include <svo/feature.h>
#include <svo_msgs/Info.h> #include <svo_msgs/Info.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h> #include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/tf.h>
#include <iostream>
#include <fstream>
#include <ros/package.h>
#include <vikit/timer.h> #include <vikit/timer.h>
#include <vikit/output_helper.h> #include <vikit/output_helper.h>
#include <vikit/params_helper.h>
#include <deque> #include <deque>
#include <algorithm>
#include <svo_msgs/DenseInput.h>
namespace svo { namespace svo {
@@ -41,12 +32,6 @@ Visualizer::
Visualizer() : Visualizer() :
pnh_("~"), pnh_("~"),
trace_id_(0), trace_id_(0),
img_pub_level_(vk::getParam<int>("svo/publish_img_pyr_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)),
publish_world_in_cam_frame_(vk::getParam<bool>("svo/publish_world_in_cam_frame", true)),
publish_map_every_frame_(vk::getParam<bool>("svo/publish_map_every_frame", false)),
publish_points_display_time_(vk::getParam<double>("svo/publish_point_display_time", 0)),
T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()) T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
{ {
// Init ROS Marker Publishers // Init ROS Marker Publishers
@@ -54,11 +39,6 @@ Visualizer() :
pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000); pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10); pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
pub_info_ = pnh_.advertise<svo_msgs::Info>("info", 10); pub_info_ = pnh_.advertise<svo_msgs::Info>("info", 10);
pub_dense_ = pnh_.advertise<svo_msgs::DenseInput>("dense_input",10);
// create video publisher
image_transport::ImageTransport it(pnh_);
pub_images_ = it.advertise("image", 10);
} }
void Visualizer::publishMinimal( void Visualizer::publishMinimal(
@@ -90,97 +70,16 @@ void Visualizer::publishMinimal(
msg_info.num_matches = 0; msg_info.num_matches = 0;
pub_info_.publish(msg_info); pub_info_.publish(msg_info);
} }
if(frame == NULL)
{
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);
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();
it_ref != px_ref.end(); ++it_ref, ++it_cur)
cv::line(img_rgb,
cv::Point2f(it_cur->x/scale, it_cur->y/scale),
cv::Point2f(it_ref->x/scale, it_ref->y/scale), cv::Scalar(0,255,0), 2);
}
if(img_pub_level_ == 0)
{
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->type == Feature::EDGELET)
cv::line(img_rgb,
cv::Point2f((*it)->px[0]+3*(*it)->grad[1], (*it)->px[1]-3*(*it)->grad[0]),
cv::Point2f((*it)->px[0]-3*(*it)->grad[1], (*it)->px[1]+3*(*it)->grad[0]),
cv::Scalar(255,0,255), 2);
else//point size 5x5
cv::rectangle(img_rgb,
cv::Point2f((*it)->px[0]-2, (*it)->px[1]-2),
cv::Point2f((*it)->px[0]+2, (*it)->px[1]+2),
cv::Scalar(0,255,0), CV_FILLED);
}
}
else if(img_pub_level_ == 1){//point size 3x3
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
cv::rectangle(img_rgb,
cv::Point2f((*it)->px[0]/scale-1, (*it)->px[1]/scale-1),
cv::Point2f((*it)->px[0]/scale+1, (*it)->px[1]/scale+1),
cv::Scalar(0,255,0), CV_FILLED);
}else{ //point size 1x1
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it){
cv::Vec3b &p= img_rgb.at<cv::Vec3b>((*it)->px[1]/scale, (*it)->px[0]/scale);
p[0]=0;p[1]=255;p[2]=0;
}
}
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());
}
if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME) if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
{ {
Quaterniond q; Quaterniond q;
Vector3d p; Vector3d p;
Matrix<double,6,6> Cov; Matrix<double,6,6> Cov;
if(publish_world_in_cam_frame_) // publish cam in world frame (Estimated odometry in the worls frame)
{ SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
// publish world in cam frame q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
SE3 T_cam_from_world(frame->T_f_w_* T_world_from_vision_); p = T_world_from_cam.translation();
q = Quaterniond(T_cam_from_world.rotation_matrix()); Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
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); geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose->header = header_msg; msg_pose->header = header_msg;
msg_pose->pose.pose.position.x = p[0]; msg_pose->pose.pose.position.x = p[0];
@@ -210,28 +109,13 @@ void Visualizer::visualizeMarkers(
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0) if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
{ {
vk::output_helper::publishCameraMarker(
pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_),
1, 0.3, Vector3d(0.,0.,1.));
vk::output_helper::publishPointMarker( vk::output_helper::publishPointMarker(
pub_points_, T_world_from_vision_*frame->pos(), "trajectory", pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5)); ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
if(frame->isKeyframe() || publish_map_every_frame_)
publishMapRegion(core_kfs);
removeDeletedPts(map); removeDeletedPts(map);
} }
} }
void Visualizer::publishMapRegion(set<FramePtr> frames)
{
if(pub_points_.getNumSubscribers() > 0)
{
int ts = vk::Timer::getCurrentTime();
for(set<FramePtr>::iterator it=frames.begin(); it!=frames.end(); ++it)
displayKeyframeWithMps(*it, ts);
}
}
void Visualizer::removeDeletedPts(const Map& map) void Visualizer::removeDeletedPts(const Map& map)
{ {
if(pub_points_.getNumSubscribers() > 0) if(pub_points_.getNumSubscribers() > 0)
@@ -241,76 +125,4 @@ void Visualizer::removeDeletedPts(const Map& map)
} }
} }
void Visualizer::displayKeyframeWithMps(const FramePtr& frame, int ts)
{
// publish keyframe
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)
{
if((*it)->point == NULL)
continue;
if((*it)->point->last_published_ts_ == ts)
continue;
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),
publish_points_display_time_);
(*it)->point->last_published_ts_ = ts;
}
}
void Visualizer::exportToDense(const FramePtr& frame)
{
// publish air_ground_msgs
if(frame != NULL && dense_pub_nth_ > 0
&& trace_id_%dense_pub_nth_ == 0 && pub_dense_.getNumSubscribers() > 0)
{
svo_msgs::DenseInput msg;
msg.header.stamp = ros::Time(frame->timestamp_);
msg.header.frame_id = "/world";
msg.frame_id = frame->id_;
cv_bridge::CvImage img_msg;
img_msg.header.stamp = msg.header.stamp;
img_msg.header.frame_id = "camera";
img_msg.image = frame->img();
img_msg.encoding = sensor_msgs::image_encodings::MONO8;
msg.image = *img_msg.toImageMsg();
double min_z = std::numeric_limits<double>::max();
double max_z = std::numeric_limits<double>::min();
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point==NULL)
continue;
Vector3d pos = frame->T_f_w_*(*it)->point->pos_;
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;
// publish cam in world frame
SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
Quaterniond q(T_world_from_cam.rotation_matrix());
Vector3d p(T_world_from_cam.translation());
msg.pose.position.x = p[0];
msg.pose.position.y = p[1];
msg.pose.position.z = p[2];
msg.pose.orientation.w = q.w();
msg.pose.orientation.x = q.x();
msg.pose.orientation.y = q.y();
msg.pose.orientation.z = q.z();
pub_dense_.publish(msg);
}
}
} // end namespace svo } // end namespace svo
svo_ros/src/vo_node.cpp
+48 -101
View File
@@ -17,15 +17,12 @@
#include <string> #include <string>
#include <svo/frame_handler_mono.h> #include <svo/frame_handler_mono.h>
#include <svo/map.h> #include <svo/map.h>
#include <svo/config.h>
#include <svo_ros/visualizer.h> #include <svo_ros/visualizer.h>
#include <vikit/params_helper.h> #include <vikit/params_helper.h>
#include <sensor_msgs/Image.h> #include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h> #include <sensor_msgs/Imu.h>
#include <std_msgs/String.h> #include <std_msgs/String.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h> #include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h> #include <image_transport/image_transport.h>
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
@@ -34,124 +31,78 @@
#include <vikit/camera_loader.h> #include <vikit/camera_loader.h>
#include <vikit/user_input_thread.h> #include <vikit/user_input_thread.h>
namespace svo { namespace svo {
/// SVO Interface /// SVO Interface
class VoNode class VoNode
{ {
public: public:
svo::FrameHandlerMono* vo_; svo::FrameHandlerMono* vo_;
svo::Visualizer visualizer_; svo::Visualizer visualizer_;
bool publish_markers_; //!< publish only the minimal amount of info (choice for embedded devices) bool publish_markers_; //!< publish only the minimal amount of info (choice for embedded devices)
bool publish_dense_input_; vk::AbstractCamera* cam_;
boost::shared_ptr<vk::UserInputThread> user_input_thread_; FILE* logtime;
ros::Subscriber sub_remote_key_; bool quit_;
std::string remote_input_; VoNode();
vk::AbstractCamera* cam_; ~VoNode();
bool quit_; void imgCb(const sensor_msgs::ImageConstPtr& msg);
VoNode();
~VoNode();
void imgCb(const sensor_msgs::ImageConstPtr& msg);
void processUserActions();
void remoteKeyCb(const std_msgs::StringConstPtr& key_input);
}; };
VoNode::VoNode() : VoNode::VoNode() :
vo_(NULL), vo_(NULL),
publish_markers_(vk::getParam<bool>("svo/publish_markers", true)), publish_markers_(vk::getParam<bool>("svo/publish_markers", true)),
publish_dense_input_(vk::getParam<bool>("svo/publish_dense_input", false)), cam_(NULL),
remote_input_(""), quit_(false)
cam_(NULL),
quit_(false)
{ {
// Start user input thread in parallel thread that listens to console keys // Create Camera
if(vk::getParam<bool>("svo/accept_console_user_input", true)) if(!vk::camera_loader::loadFromRosNs("svo", cam_))
user_input_thread_ = boost::make_shared<vk::UserInputThread>(); throw std::runtime_error("Camera model not correctly specified.");
// Create Camera // Get initial position and orientation
if(!vk::camera_loader::loadFromRosNs("svo", cam_)) visualizer_.T_world_from_vision_ = Sophus::SE3(
throw std::runtime_error("Camera model not correctly specified."); vk::rpy2dcm(Vector3d(vk::getParam<double>("svo/init_rx", 0.0),
vk::getParam<double>("svo/init_ry", 0.0),
vk::getParam<double>("svo/init_rz", 0.0))),
Eigen::Vector3d(vk::getParam<double>("svo/init_tx", 0.0),
vk::getParam<double>("svo/init_ty", 0.0),
vk::getParam<double>("svo/init_tz", 0.0)));
// Get initial position and orientation // Init VO and start
visualizer_.T_world_from_vision_ = Sophus::SE3( vo_ = new svo::FrameHandlerMono(cam_);
vk::rpy2dcm(Vector3d(vk::getParam<double>("svo/init_rx", 0.0), vo_->start();
vk::getParam<double>("svo/init_ry", 0.0), logtime = fopen((std::string(getenv("HOME"))+"/Project/time.txt").c_str(), "w+");
vk::getParam<double>("svo/init_rz", 0.0))),
Eigen::Vector3d(vk::getParam<double>("svo/init_tx", 0.0),
vk::getParam<double>("svo/init_ty", 0.0),
vk::getParam<double>("svo/init_tz", 0.0)));
// Init VO and start
vo_ = new svo::FrameHandlerMono(cam_);
vo_->start();
} }
VoNode::~VoNode() VoNode::~VoNode()
{ {
delete vo_; fclose(logtime);
delete cam_; delete logtime;
if(user_input_thread_ != NULL) delete vo_;
user_input_thread_->stop(); delete cam_;
} }
void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
{ {
cv::Mat img; auto start=std::chrono::steady_clock::now();
try { try {
img = cv_bridge::toCvShare(msg, "mono8")->image; cv::Mat img;
} catch (cv_bridge::Exception& e) { img = cv_bridge::toCvShare(msg, "mono8")->image;
ROS_ERROR("cv_bridge exception: %s", e.what()); vo_->addImage(img, msg->header.stamp.toSec());
} visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
processUserActions();
vo_->addImage(img, msg->header.stamp.toSec());
visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED)
visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
if(publish_dense_input_) if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED)
visualizer_.exportToDense(vo_->lastFrame()); visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED) if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
usleep(100000); usleep(100000);
fprintf(logtime, "%s\n",std::to_string(std::chrono::duration<double>(std::chrono::steady_clock::now()-start).count()).c_str());
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
}
} }
void VoNode::processUserActions()
{
char input = remote_input_.c_str()[0];
remote_input_ = "";
if(user_input_thread_ != NULL)
{
char console_input = user_input_thread_->getInput();
if(console_input != 0)
input = console_input;
}
switch(input)
{
case 'q':
quit_ = true;
printf("SVO user input: QUIT\n");
break;
case 'r':
vo_->reset();
printf("SVO user input: RESET\n");
break;
case 's':
vo_->start();
printf("SVO user input: START\n");
break;
default: ;
}
}
void VoNode::remoteKeyCb(const std_msgs::StringConstPtr& key_input)
{
remote_input_ = key_input->data;
}
} // namespace svo } // namespace svo
int main(int argc, char **argv) int main(int argc, char **argv)
@@ -162,12 +113,8 @@ int main(int argc, char **argv)
svo::VoNode vo_node; svo::VoNode vo_node;
// subscribe to cam msgs // subscribe to cam msgs
std::string cam_topic(vk::getParam<std::string>("svo/cam_topic", "camera/image_raw"));
image_transport::ImageTransport it(nh); image_transport::ImageTransport it(nh);
image_transport::Subscriber it_sub = it.subscribe(cam_topic, 5, &svo::VoNode::imgCb, &vo_node); image_transport::Subscriber it_sub = it.subscribe(vk::getParam<std::string>("svo/cam_topic", "camera/image_raw"), 5, &svo::VoNode::imgCb, &vo_node);
// subscribe to remote input
vo_node.sub_remote_key_ = nh.subscribe("svo/remote_key", 5, &svo::VoNode::remoteKeyCb, &vo_node);
// start processing callbacks // start processing callbacks
while(ros::ok() && !vo_node.quit_) while(ros::ok() && !vo_node.quit_)