00001 #include <vslam_system/vslam_mono.h> 00002 00003 using namespace sba; 00004 00005 namespace vslam 00006 { 00007 00008 VslamSystemMono::VslamSystemMono(const std::string& vocab_tree_file, const std::string& vocab_weights_file) 00009 : VslamSystem(vocab_tree_file, vocab_weights_file) 00010 { 00011 vo_.pose_estimator_ = boost::shared_ptr<pe::PoseEstimator>(new pe::PoseEstimator2d); 00012 } 00013 00014 bool VslamSystemMono::addFrame(const frame_common::CamParams& camera_parameters, 00015 const cv::Mat& image) 00016 { 00017 // Set up next frame and compute descriptors 00018 frame_common::Frame next_frame; 00019 next_frame.setCamParams(camera_parameters); // this sets the projection and reprojection matrices 00020 frame_processor_.setMonoFrame(next_frame, image); 00021 next_frame.frameId = sba_.nodes.size(); // index 00022 next_frame.img = cv::Mat(); // remove the images 00023 next_frame.imgRight = cv::Mat(); 00024 00025 // Add frame to visual odometer 00026 bool is_keyframe = vo_.addFrame(next_frame); 00027 00028 // grow full SBA 00029 if (is_keyframe) 00030 { 00031 frames_.push_back(next_frame); 00032 vo_.transferLatestFrame(frames_, sba_); 00033 } 00034 00035 if (frames_.size() > 1 && vo_.pose_estimator_->inliers.size() < 40) 00036 std::cout << std::endl << "******** Bad image match: " << std::endl << std::endl; 00037 00038 return is_keyframe; 00039 } 00040 00041 } // vslam