From e9256c9903b304ee05c03df4f25b4259b98caeb5 Mon Sep 17 00:00:00 2001 From: gfuse Date: Mon, 5 Apr 2021 18:41:37 +0200 Subject: [PATCH] optimize the visualizer --- svo_ros/include/svo_ros/visualizer.h | 13 -- .../launch/{usb_cam.launch => Px30.launch} | 2 +- svo_ros/param/px30.yaml | 8 + svo_ros/src/visualizer.cpp | 198 +----------------- svo_ros/src/vo_node.cpp | 149 +++++-------- 5 files changed, 62 insertions(+), 308 deletions(-) rename svo_ros/launch/{usb_cam.launch => Px30.launch} (91%) create mode 100644 svo_ros/param/px30.yaml diff --git a/svo_ros/include/svo_ros/visualizer.h b/svo_ros/include/svo_ros/visualizer.h index dc8936d..4a09a8d 100644 --- a/svo_ros/include/svo_ros/visualizer.h +++ b/svo_ros/include/svo_ros/visualizer.h @@ -48,19 +48,11 @@ class Visualizer public: ros::NodeHandle pnh_; 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_points_; ros::Publisher pub_pose_; ros::Publisher pub_info_; - ros::Publisher pub_dense_; - image_transport::Publisher pub_images_; 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_; Visualizer(); @@ -78,13 +70,8 @@ public: const set& core_kfs, const Map& map); - void publishMapRegion(set frames); - void removeDeletedPts(const Map& map); - void displayKeyframeWithMps(const FramePtr& frame, int ts); - - void exportToDense(const FramePtr& frame); }; } // end namespace svo diff --git a/svo_ros/launch/usb_cam.launch b/svo_ros/launch/Px30.launch similarity index 91% rename from svo_ros/launch/usb_cam.launch rename to svo_ros/launch/Px30.launch index ad34684..53f9317 100644 --- a/svo_ros/launch/usb_cam.launch +++ b/svo_ros/launch/Px30.launch @@ -9,7 +9,7 @@ - + diff --git a/svo_ros/param/px30.yaml b/svo_ros/param/px30.yaml new file mode 100644 index 0000000..10fa932 --- /dev/null +++ b/svo_ros/param/px30.yaml @@ -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 \ No newline at end of file diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp index c4ea47f..3cda024 100644 --- a/svo_ros/src/visualizer.cpp +++ b/svo_ros/src/visualizer.cpp @@ -22,18 +22,9 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include -#include #include -#include -#include namespace svo { @@ -41,12 +32,6 @@ Visualizer:: Visualizer() : pnh_("~"), trace_id_(0), - img_pub_level_(vk::getParam("svo/publish_img_pyr_level", 0)), - img_pub_nth_(vk::getParam("svo/publish_every_nth_img", 1)), - dense_pub_nth_(vk::getParam("svo/publish_every_nth_dense_input", 1)), - publish_world_in_cam_frame_(vk::getParam("svo/publish_world_in_cam_frame", true)), - publish_map_every_frame_(vk::getParam("svo/publish_map_every_frame", false)), - publish_points_display_time_(vk::getParam("svo/publish_point_display_time", 0)), T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()) { // Init ROS Marker Publishers @@ -54,11 +39,6 @@ Visualizer() : pub_points_ = pnh_.advertise("points", 1000); pub_pose_ = pnh_.advertise("pose",10); pub_info_ = pnh_.advertise("info", 10); - pub_dense_ = pnh_.advertise("dense_input",10); - - // create video publisher - image_transport::ImageTransport it(pnh_); - pub_images_ = it.advertise("image", 10); } void Visualizer::publishMinimal( @@ -90,97 +70,16 @@ void Visualizer::publishMinimal( msg_info.num_matches = 0; 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_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& px_ref(slam.initFeatureTrackRefPx()); - const vector& px_cur(slam.initFeatureTrackCurPx()); - for(vector::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((*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) { Quaterniond q; Vector3d p; Matrix 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(); - } + // 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 + 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[0]; @@ -210,28 +109,13 @@ void Visualizer::visualizeMarkers( 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( pub_points_, T_world_from_vision_*frame->pos(), "trajectory", 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); } } -void Visualizer::publishMapRegion(set frames) -{ - if(pub_points_.getNumSubscribers() > 0) - { - int ts = vk::Timer::getCurrentTime(); - for(set::iterator it=frames.begin(); it!=frames.end(); ++it) - displayKeyframeWithMps(*it, ts); - } -} - void Visualizer::removeDeletedPts(const Map& map) { 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::max(); - double max_z = std::numeric_limits::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 diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp index 3417b4d..b7cdde4 100644 --- a/svo_ros/src/vo_node.cpp +++ b/svo_ros/src/vo_node.cpp @@ -17,15 +17,12 @@ #include #include #include -#include #include #include #include #include #include -#include #include -#include #include #include #include @@ -34,124 +31,78 @@ #include #include + namespace svo { /// SVO Interface class VoNode { public: - svo::FrameHandlerMono* vo_; - svo::Visualizer visualizer_; - bool publish_markers_; //!< publish only the minimal amount of info (choice for embedded devices) - bool publish_dense_input_; - boost::shared_ptr user_input_thread_; - ros::Subscriber sub_remote_key_; - std::string remote_input_; - vk::AbstractCamera* cam_; - bool quit_; - VoNode(); - ~VoNode(); - void imgCb(const sensor_msgs::ImageConstPtr& msg); - void processUserActions(); - void remoteKeyCb(const std_msgs::StringConstPtr& key_input); + svo::FrameHandlerMono* vo_; + svo::Visualizer visualizer_; + bool publish_markers_; //!< publish only the minimal amount of info (choice for embedded devices) + vk::AbstractCamera* cam_; + FILE* logtime; + bool quit_; + VoNode(); + ~VoNode(); + void imgCb(const sensor_msgs::ImageConstPtr& msg); }; VoNode::VoNode() : - vo_(NULL), - publish_markers_(vk::getParam("svo/publish_markers", true)), - publish_dense_input_(vk::getParam("svo/publish_dense_input", false)), - remote_input_(""), - cam_(NULL), - quit_(false) + vo_(NULL), + publish_markers_(vk::getParam("svo/publish_markers", true)), + cam_(NULL), + quit_(false) { - // Start user input thread in parallel thread that listens to console keys - if(vk::getParam("svo/accept_console_user_input", true)) - user_input_thread_ = boost::make_shared(); + // Create Camera + if(!vk::camera_loader::loadFromRosNs("svo", cam_)) + throw std::runtime_error("Camera model not correctly specified."); - // Create Camera - if(!vk::camera_loader::loadFromRosNs("svo", cam_)) - throw std::runtime_error("Camera model not correctly specified."); + // Get initial position and orientation + visualizer_.T_world_from_vision_ = Sophus::SE3( + vk::rpy2dcm(Vector3d(vk::getParam("svo/init_rx", 0.0), + vk::getParam("svo/init_ry", 0.0), + vk::getParam("svo/init_rz", 0.0))), + Eigen::Vector3d(vk::getParam("svo/init_tx", 0.0), + vk::getParam("svo/init_ty", 0.0), + vk::getParam("svo/init_tz", 0.0))); - // Get initial position and orientation - visualizer_.T_world_from_vision_ = Sophus::SE3( - vk::rpy2dcm(Vector3d(vk::getParam("svo/init_rx", 0.0), - vk::getParam("svo/init_ry", 0.0), - vk::getParam("svo/init_rz", 0.0))), - Eigen::Vector3d(vk::getParam("svo/init_tx", 0.0), - vk::getParam("svo/init_ty", 0.0), - vk::getParam("svo/init_tz", 0.0))); + // Init VO and start + vo_ = new svo::FrameHandlerMono(cam_); + vo_->start(); + logtime = fopen((std::string(getenv("HOME"))+"/Project/time.txt").c_str(), "w+"); - // Init VO and start - vo_ = new svo::FrameHandlerMono(cam_); - vo_->start(); } VoNode::~VoNode() { - delete vo_; - delete cam_; - if(user_input_thread_ != NULL) - user_input_thread_->stop(); + fclose(logtime); + delete logtime; + delete vo_; + delete cam_; } void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) { - cv::Mat img; - try { - img = cv_bridge::toCvShare(msg, "mono8")->image; - } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); - } - processUserActions(); - vo_->addImage(img, msg->header.stamp.toSec()); - visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec()); + auto start=std::chrono::steady_clock::now(); + try { + cv::Mat img; + img = cv_bridge::toCvShare(msg, "mono8")->image; + 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_) - visualizer_.exportToDense(vo_->lastFrame()); + if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED) + visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map()); - if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED) - usleep(100000); + if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED) + usleep(100000); + fprintf(logtime, "%s\n",std::to_string(std::chrono::duration(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 int main(int argc, char **argv) @@ -162,12 +113,8 @@ int main(int argc, char **argv) svo::VoNode vo_node; // subscribe to cam msgs - std::string cam_topic(vk::getParam("svo/cam_topic", "camera/image_raw")); image_transport::ImageTransport it(nh); - image_transport::Subscriber it_sub = it.subscribe(cam_topic, 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); + image_transport::Subscriber it_sub = it.subscribe(vk::getParam("svo/cam_topic", "camera/image_raw"), 5, &svo::VoNode::imgCb, &vo_node); // start processing callbacks while(ros::ok() && !vo_node.quit_)