Public Member Functions | |
| bool | addFrame (const frame_common::CamParams &camera_parameters, const cv::Mat &left, const cv::Mat &right) |
| void | addProjections (fc::Frame &f0, fc::Frame &f1, const std::vector< cv::DMatch > &inliers, const Matrix< double, 3, 4 > &f2w, int ndi0, int ndi1) |
| void | configCb (vslam_system::StereoVslamNodeConfig &config, uint32_t level) |
| void | imageCb (const sensor_msgs::ImageConstPtr &l_image, const sensor_msgs::CameraInfoConstPtr &l_cam_info, const sensor_msgs::ImageConstPtr &r_image, const sensor_msgs::CameraInfoConstPtr &r_cam_info) |
| void | publishFrame () |
| void | publishLatestFrame () |
| int | publishNode (Eigen::Matrix< double, 4, 1 > trans, Eigen::Quaternion< double > fq, const frame_common::CamParams &cp, bool isFixed) |
| int | publishPoint (sba::Point pt) |
| void | publishProjection (int ci, int pi, Eigen::Vector3d &q, bool stereo) |
| VONode (const std::string &vocab_tree_file, const std::string &vocab_weights_file, const std::string &calonder_trees_file) | |
Private Types | |
| typedef dynamic_reconfigure::Server < vslam_system::StereoVslamNodeConfig > | ReconfigureServer |
Private Attributes | |
| image_geometry::StereoCameraModel | cam_model_ |
| cv::Ptr< cv::FeatureDetector > | detector_ |
| frame_common::FrameProc | frame_processor_ |
| std::vector < frame_common::Frame, Eigen::aligned_allocator < frame_common::Frame > > | frames_ |
| stereo image key frames in system | |
| bool | init |
| image_transport::ImageTransport | it_ |
| sensor_msgs::CvBridge | l_bridge_ |
| image_transport::SubscriberFilter | l_image_sub_ |
| message_filters::Subscriber < sensor_msgs::CameraInfo > | l_info_sub_ |
| ros::NodeHandle | nh_ |
| vector< sba::CameraNode > | node_msgs |
| int | numcameras |
| int | numframes |
| VO processor. | |
| int | numpoints |
| vector< sba::WorldPoint > | point_msgs |
| vector< sba::Projection > | proj_msgs |
| sensor_msgs::CvBridge | r_bridge_ |
| image_transport::SubscriberFilter | r_image_sub_ |
| message_filters::Subscriber < sensor_msgs::CameraInfo > | r_info_sub_ |
| ReconfigureServer | reconfigure_server_ |
| ros::Publisher | sba_frames_pub_ |
| message_filters::TimeSynchronizer < sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::CameraInfo > | sync_ |
| vslam::voSt | vo_ |
| cv::Mat | vo_display_ |
| image_transport::CameraPublisher | vo_tracks_pub_ |
Definition at line 30 of file vo_node.cpp.
typedef dynamic_reconfigure::Server<vslam_system::StereoVslamNodeConfig> VONode::ReconfigureServer [private] |
Definition at line 68 of file vo_node.cpp.
| VONode::VONode | ( | const std::string & | vocab_tree_file, |
| const std::string & | vocab_weights_file, | ||
| const std::string & | calonder_trees_file | ||
| ) | [inline] |
Definition at line 75 of file vo_node.cpp.
| bool VONode::addFrame | ( | const frame_common::CamParams & | camera_parameters, |
| const cv::Mat & | left, | ||
| const cv::Mat & | right | ||
| ) | [inline] |
Definition at line 158 of file vo_node.cpp.
| void VONode::addProjections | ( | fc::Frame & | f0, |
| fc::Frame & | f1, | ||
| const std::vector< cv::DMatch > & | inliers, | ||
| const Matrix< double, 3, 4 > & | f2w, | ||
| int | ndi0, | ||
| int | ndi1 | ||
| ) | [inline] |
Definition at line 317 of file vo_node.cpp.
| void VONode::configCb | ( | vslam_system::StereoVslamNodeConfig & | config, |
| uint32_t | level | ||
| ) | [inline] |
Definition at line 147 of file vo_node.cpp.
| void VONode::imageCb | ( | const sensor_msgs::ImageConstPtr & | l_image, |
| const sensor_msgs::CameraInfoConstPtr & | l_cam_info, | ||
| const sensor_msgs::ImageConstPtr & | r_image, | ||
| const sensor_msgs::CameraInfoConstPtr & | r_cam_info | ||
| ) | [inline] |
Definition at line 106 of file vo_node.cpp.
| void VONode::publishFrame | ( | ) | [inline] |
Definition at line 245 of file vo_node.cpp.
| void VONode::publishLatestFrame | ( | ) | [inline] |
TODO this also assume most recent VO operation was a keyframe should reconstruct inliers from most recent two frames
Also assumes that last node was published from this node as well
Definition at line 182 of file vo_node.cpp.
| int VONode::publishNode | ( | Eigen::Matrix< double, 4, 1 > | trans, |
| Eigen::Quaternion< double > | fq, | ||
| const frame_common::CamParams & | cp, | ||
| bool | isFixed | ||
| ) | [inline] |
Definition at line 255 of file vo_node.cpp.
| int VONode::publishPoint | ( | sba::Point | pt | ) | [inline] |
Definition at line 284 of file vo_node.cpp.
| void VONode::publishProjection | ( | int | ci, |
| int | pi, | ||
| Eigen::Vector3d & | q, | ||
| bool | stereo | ||
| ) | [inline] |
Definition at line 302 of file vo_node.cpp.
Definition at line 51 of file vo_node.cpp.
cv::Ptr<cv::FeatureDetector> VONode::detector_ [private] |
Definition at line 52 of file vo_node.cpp.
Definition at line 54 of file vo_node.cpp.
std::vector<frame_common::Frame, Eigen::aligned_allocator<frame_common::Frame> > VONode::frames_ [private] |
stereo image key frames in system
Definition at line 56 of file vo_node.cpp.
bool VONode::init [private] |
Definition at line 71 of file vo_node.cpp.
image_transport::ImageTransport VONode::it_ [private] |
Definition at line 34 of file vo_node.cpp.
sensor_msgs::CvBridge VONode::l_bridge_ [private] |
Definition at line 50 of file vo_node.cpp.
Definition at line 37 of file vo_node.cpp.
message_filters::Subscriber<sensor_msgs::CameraInfo> VONode::l_info_sub_ [private] |
Definition at line 38 of file vo_node.cpp.
ros::NodeHandle VONode::nh_ [private] |
Definition at line 33 of file vo_node.cpp.
vector<sba::CameraNode> VONode::node_msgs [private] |
Definition at line 65 of file vo_node.cpp.
int VONode::numcameras [private] |
Definition at line 61 of file vo_node.cpp.
int VONode::numframes [private] |
VO processor.
Definition at line 59 of file vo_node.cpp.
int VONode::numpoints [private] |
Definition at line 60 of file vo_node.cpp.
vector<sba::WorldPoint> VONode::point_msgs [private] |
Definition at line 64 of file vo_node.cpp.
vector<sba::Projection> VONode::proj_msgs [private] |
Definition at line 63 of file vo_node.cpp.
sensor_msgs::CvBridge VONode::r_bridge_ [private] |
Definition at line 50 of file vo_node.cpp.
Definition at line 37 of file vo_node.cpp.
message_filters::Subscriber<sensor_msgs::CameraInfo> VONode::r_info_sub_ [private] |
Definition at line 38 of file vo_node.cpp.
ReconfigureServer VONode::reconfigure_server_ [private] |
Definition at line 69 of file vo_node.cpp.
ros::Publisher VONode::sba_frames_pub_ [private] |
Definition at line 43 of file vo_node.cpp.
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::CameraInfo> VONode::sync_ [private] |
Definition at line 40 of file vo_node.cpp.
vslam::voSt VONode::vo_ [private] |
Definition at line 57 of file vo_node.cpp.
cv::Mat VONode::vo_display_ [private] |
Definition at line 47 of file vo_node.cpp.
Definition at line 46 of file vo_node.cpp.