diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h index 66d4595..bbde3ce 100644 --- a/svo/include/svo/frame_handler_base.h +++ b/svo/include/svo/frame_handler_base.h @@ -63,9 +63,6 @@ public: virtual ~FrameHandlerBase(); - /// Set initial pose of the camera. - void setInitialPose(const SE3& T_frame_from_world) { T_f_w_init_ = T_frame_from_world; } - /// Get the current map. const Map& map() const { return map_; } @@ -92,7 +89,6 @@ protected: bool set_reset_; //!< Flag that the user can set. Will reset the system before the next iteration. bool set_start_; //!< Flag the user can set to start the system when the next image is received. Map map_; //!< Map of keyframes created by the slam system. - SE3 T_f_w_init_; //!< Initial pose of the camera. vk::Timer timer_; //!< Stopwatch to measure time to process frame. vk::RingBuffer acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance. vk::RingBuffer acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance. diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp index 91bf5f0..f526cb3 100644 --- a/svo/src/frame_handler_base.cpp +++ b/svo/src/frame_handler_base.cpp @@ -120,14 +120,14 @@ int FrameHandlerBase::finishFrameProcessingCommon( num_obs_last_ = num_observations; SVO_STOP_TIMER("tot_time"); - #ifdef SVO_TRACE - g_permon->writeToFile(); - { - boost::unique_lock lock(map_.point_candidates_.mut_); - size_t n_candidates = map_.point_candidates_.candidates_.size(); - SVO_LOG(n_candidates); - } - #endif +#ifdef SVO_TRACE + g_permon->writeToFile(); + { + boost::unique_lock lock(map_.point_candidates_.mut_); + size_t n_candidates = map_.point_candidates_.candidates_.size(); + SVO_LOG(n_candidates); + } +#endif // reset if failure or requested if(dropout == RESULT_FAILURE || set_reset_) diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index d3ce965..e588ce9 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/svo/src/frame_handler_mono.cpp @@ -91,7 +91,7 @@ void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame) FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() { - new_frame_->T_f_w_ = T_f_w_init_; + new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero()); if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE) return RESULT_NO_KEYFRAME; new_frame_->setKeyframe(); diff --git a/svo_ros/include/svo_ros/visualizer.h b/svo_ros/include/svo_ros/visualizer.h index 1a7df1f..5cffa0e 100644 --- a/svo_ros/include/svo_ros/visualizer.h +++ b/svo_ros/include/svo_ros/visualizer.h @@ -59,6 +59,7 @@ public: ros::Publisher pub_dense_; image_transport::Publisher pub_images_; tf::TransformBroadcaster br_; + SE3 T_world_from_vision_; Visualizer(); ~Visualizer() {}; diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp index eda01c6..f952fa8 100644 --- a/svo_ros/src/visualizer.cpp +++ b/svo_ros/src/visualizer.cpp @@ -43,7 +43,8 @@ Visualizer() : trace_id_(0), img_pub_level_(vk::getParam("svo/image_publishing_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)) + dense_pub_nth_(vk::getParam("svo/publish_every_nth_dense_input", 1)), + T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()) { // Init ROS Marker Publishers pub_frames_ = pnh_.advertise("keyframes", 10); @@ -159,22 +160,27 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono pub_images_.publish(img_msg.toImageMsg()); } - // publish pose (world in camera frame) + // publish pose (camera in world frame) if(pub_pose_.getNumSubscribers() > 0 && frame != NULL) { + // 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 Cov_in_world = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj(); + + // publish message geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped); - Quaterniond q(frame->T_f_w_.rotation_matrix()); - Vector3d p(frame->T_f_w_.translation()); msg_pose->header = header_msg; - 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(); + 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(); for(size_t i=0; i<36; ++i) - msg_pose->pose.covariance[i] = frame->Cov_(i%6, i/6); + msg_pose->pose.covariance[i] = Cov_in_world(i%6, i/6); pub_pose_.publish(msg_pose); } } diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp index 9dc31dd..3dfdc92 100644 --- a/svo_ros/src/vo_node.cpp +++ b/svo_ros/src/vo_node.cpp @@ -74,18 +74,17 @@ VoNode() : if(!vk::camera_loader::loadFromRosNs("svo", cam_)) throw std::runtime_error("Camera model not correctly specified."); - // Init camera - vo_ = new svo::FrameHandlerMono(cam_); - - // Set initial position and orientation - Sophus::SE3 T_cam_from_world( + // 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))); - vo_->setInitialPose(T_cam_from_world); + + // Init VO and start + vo_ = new svo::FrameHandlerMono(cam_); vo_->start(); }