1. Camera类1.1 camera.h#ifndef CAMERA_H //宏定义 #define CAMERA_H #include myslam/common_include.h namespace myslam //命名空间 { class Camera { public: typedef std::shared_ptrCamera Ptr; //智能指针 float fx_, fy_, cx_, cy_, depth_scale_; Camera(); Camera ( float fx, float fy, float cx, float cy, float depth_scale0 ) : fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale ) {} Vector3d world2camera( const Vector3d p_w, const SE3 T_c_w ); Vector3d camera2world( const Vector3d p_c, const SE3 T_c_w ); Vector2d camera2pixel( const Vector3d p_c ); Vector3d pixel2camera( const Vector2d p_p, double depth1 ); Vector3d pixel2world ( const Vector2d p_p, const SE3 T_c_w, double depth1 ); Vector2d world2pixel ( const Vector3d p_w, const SE3 T_c_w ); }; } #endif1.2 camera.cpp#include myslam/camera.h namespace myslam { Camera::Camera() {} } Vector3d Camera :: world2camera( const Vector3d p_w, const SE3 T_c_w ) { return T_c_w*p_w; } Vector3d Camera :: camera2world( const Vector3d p_c, const SE3 T_c_w ) { return T_c_w.inverse()*p_c; } Vector2d Camera :: camera2pixel( const Vector3d p_c ) { return Vector2d( fx_ * p_c ( 0,0 ) / p_c ( 2,0 ) cx_, fy_ * p_c ( 1,0 ) / p_c ( 2,0 ) cy_ ); } Vector3d Camera :: pixel2camera( const Vector2d p_p, double depth ) //const Vector2d 这里是引用 { return Vector3d( depth * ( p_p ( 0,0 ) - cx_ ) / fx_, depth * ( p_p ( 1,0 ) - cy_ ) / fy_, depth ); } Vector3d Camera :: pixel2world( const Vector2d p_p, const SE3 T_c_w, double depth ) { Vector3d p_c pixel2camera( p_p, depth ); return camera2world( p_c, T_c_w ); } Vector2d Camera :: world2pixel( const Vector3d p_w, const SE3 T_c_w ) { Vector3d p_c world2camera( p_w, T_c_w ); return camera2pixel( p_c ); }2. Frame类2.1 frame.h#indef FRAME_H #define FRAME_H #include myslam/common_include.h #include myslam/camera.h namespace myslam { class MapPoint; class Frame{ public: typedef std::shared_ptrFrame Ptr; unsigned long id_; // id double time_stamp_; SE3 T_c_w_; Camera::Ptr camera_; Mat color_, depth_; std::vectorcv::KeyPoint keypoints_; //特征点 std::vectorMapPoint* map_points_; cv::Mat descriptors_; //描述子 Frame(); //默认构造函数 Frame(long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth); //带参构造函数 ~Frame(); //析构函数。对象销毁时自动调用 static Frame::Ptr createFrame(); //静态函数属于类本身直接调用,创建一个新对象 double findDepth(const cv::KeyPoint kp); //计算深度 Vector3d getCamCenter() const; bool isInFrame(const Vector3d pt_world); } } #endif2.2 frame.cpp#include myslam/frame.h namespace myslam { Frame::Frame() : id_(-1), time_stamp_(-1), camera_(nullptr) {}//构造函数初始化列表 Frame::Frame(long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth):id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth) {} Frame::~Frame() {} Frame::Ptr Frame::createFrame() { static long factory_id 0; return make_sharedFrame(factory_id); } double findDepth(const cv::KeyPoint kp) { int x cvRound(kp.pt.x); int y cvRound(kp.pt.y); ushort d depth.ptrushort(y)[x]; if(d ! 0) return double(d)/camera_-depth_scale_; else return -1.0; } Vector3d Frame::getCamCenter() const { return T_c_w_.inverse().translation(); //Twc和平移量 } bool Frame::isInFrame(const Vector3d pt_world) { Vector3d p_cam camera_-world2camera(pt_world, T_c_w_); if(p_cam(2,0)0) return false; Vector2d pixel camera_-camera2pixel(p_cam); return pixel(0,0)0 pixel(1,0)0 pixel(0,0)color_.cols pixel(1,0)color_.rows; } }3.mappoint类3.1 mappoint.h#indef MAPPOINT_H #define MAPPOINT_H #include myslam/common_include.h namespace myslam{ class Frame; //向前声明 class MapPoint{ public: typedef std::shared_ptrMapPoint Ptr; unsigned long id_; Vector3d pos_; //世界坐标 Vector3d norm_; //观测方向法向量 cv::Mat descriptor_; int observed_times_; int correct_times_; //参与优化次数 MapPoint(); MapPoint(long id, Vector3d position, Vector3d norm, cv::Mat descriptor); static MapPoint::Ptr createMapPoint(); //创建一个新的MapPoint对象 }; } #endif3.2 mappoint.cpp#include myslam/mappoint.h #include myslam/common_include.h namespace myslam { MapPoint::MapPoint() : id_(-1), pos_(0,0,0), norm_(0,0,0), descriptor_(cv::Mat()) {} MapPoint::MapPoint(long id, Vector3d position, Vector3d norm, cv::Mat descriptor) : id_(id), pos_(position), norm_(norm), descriptor_(descriptor) {} MapPoint::Ptr MapPoint::createMapPoint() { static long factory_id 0; return make_sharedMapPoint(factory_id); } }4.map类管理所有路标点如果只使用智能指针或者指针可以只向前声明要用到完整的功能还是要include4.1 .h#indef #define MAP_H #include myslam/common_include.h #include myslam/frame.h #include myslam/mappoint.h namespace myslam{ class Map{ public: typedef std::shared_ptrMap Ptr; std::unordered_mapunsigned long, MapPoint::Ptr map_points_; //所有地图点 std::unordered_mapunsigned long, Frame::Ptr keyframes_; //所有关键帧 Map(){} void insertKeyFrame(Frame::Ptr frame); //插入关键帧 void insertMapPoint(MapPoint::Ptr map_point); //插入地图点 }; } #endif4.2.cpp#include myslam/map.h namespace myslam { void Map::insertKeyFrame(Frame::Ptr frame) { if(keyframes_.find(frame-id_) keyframes_.end()) //没有这个点就插入 { keyframes_.insert(std::make_pair(frame-id_, frame)); } } void Map::insertMapPoint(MapPoint::Ptr map_point) { if(map_points_.find(map_point-id_) map_points_.end()) { map_points_.insert(std::make_pair(map_point-id_, map_point)); //make_pair构建值对插入 } } }5.config类#ifndef CONFIG_H #define CONFIG_H #include myslam/common_include.h namespace myslam { class Config { private: static std::shared_ptrConfig config_; cv::FileStorage file_; //打开yaml文件进行读写相机参数 Config () {} //放在私有区禁止外部实例化保证只有一个config对象 public: ~Config(); static void setParameterFile( const std::string filename ); template typename T //模板函数 static T get( const std::string key ) { return T( Config::config_-file_[key] ); //转成T类型 } }; } #endif#include myslam/config.h namespace myslam { std::shared_ptrConfig Config::config_ nullptr; //类型变量名初始值 Config::~Config() { if (file_.isOpened()) file_.release(); } void Config::setParameterFile(const std::string filename) { if (config_ nullptr) config_ std::shared_ptrConfig(new Config()); config_-file_ cv::FileStorage(filename, cv::FileStorage::READ); //打开yaml文件进行读写相机参数 if (config_-file_.isOpened() false) //打开失败 { LOG(ERROR) parameter file filename does not exist.; config_ nullptr; } } }6.VO6.1两帧的视觉里程计#ifndef VISUALODOMETRY_H #define VISUALODOMETRY_H #include myslam/common_include.h #include myslam/map.h #include opencv/features2d/features2d.h namespace myslam{ class VisualOdometry { public: typedef shared_ptrVisualOdometryPtr; enum VOState{ INITIALIZING -1; OK0; LOST }; VOState state_; Map::Ptr map_; Frame::Ptr ref_; //reference frame Frame::Ptr curr_; //current frame cv::Ptrcv::ORB orb_; vectorcv::Point3f pts_3d_ref_; vectorcv::Keypoint keypoint_curr_; Mat descriptors_curr_; Mat descriptors_ref_; vectorcv::DMatch feature_matches; SE3 T_c_r_estimated_; //估计的当前位姿 int num_inliers_; int num_lost_; int num_of_features_; double scale_factors_; int level_pyramid; float match_ratio_; //匹配率 int max_num_lost_; int min_inliers_; double key_frame_min_rot; double key_frame_min_trans; //关键帧条件 public: VisualOdometry(); ~VisualOdometry(); bool addFrame(Frame::Ptr frame); protected: void extractKeyPoints(); void computeDescriptors(); void featureMatching(); void poseEstimationPnP(); void setRef3DPoints(); void addKeyFrame(); bool checkEstimatedPose(); bool checkKeyFrame(); }; } #endif#include opencv2/highgui/highgui.hpp #include opencv2/imgproc/imgproc.hpp #include opencv2/calib3d/calib3d.hpp //PnP求解相机几何计算 #include algorithm #include boost/timer.hpp #include myslam/config.h #include myslam/visual_odometry.h namespace myslam { VisualOdometry::VisualOdometry() : state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 ) { num_of_features_ Config::getint ( number_of_features ); scale_factor_ Config::getdouble ( scale_factor ); level_pyramid_ Config::getint ( level_pyramid ); match_ratio_ Config::getfloat ( match_ratio ); max_num_lost_ Config::getfloat ( max_num_lost ); min_inliers_ Config::getint ( min_inliers ); key_frame_min_rot Config::getdouble ( keyframe_rotation ); key_frame_min_trans Config::getdouble ( keyframe_translation ); orb_ cv::ORB::create ( num_of_features_, scale_factor_, level_pyramid_ ); } VisualOdometry::~VisualOdometry() { } bool VisualOdometry::addFrame ( Frame::Ptr frame ) { switch ( state_ ) { case INITIALIZING: { state_ OK; curr_ ref_ frame; map_-insertKeyFrame ( frame ); extractKeyPoints(); computeDescriptors(); setRef3DPoints(); break; } case OK: { curr_ frame; extractKeyPoints(); computeDescriptors(); featureMatching(); poseEstimationPnP(); if ( checkEstimatedPose() true ) { curr_-T_c_w_ T_c_r_estimated_ * ref_-T_c_w_; // T_c_w T_c_r*T_r_w ref_ curr_; setRef3DPoints(); num_lost_ 0; if ( checkKeyFrame() true ) // 检查关键帧 { addKeyFrame(); } } else { num_lost_; if ( num_lost_ max_num_lost_ ) { state_ LOST; } return false; } break; } case LOST: { coutvo has lost.endl; break; } } return true; } void VisualOdometry::extractKeyPoints() { orb_-detect ( curr_-color_, keypoints_curr_ ); //检测关键点 } void VisualOdometry::computeDescriptors() { orb_-compute ( curr_-color_, keypoints_curr_, descriptors_curr_ ); //为关键点生成描述子 } //特征点匹配 void VisualOdometry::featureMatching() { vectorcv::DMatch matches; cv::BFMatcher matcher ( cv::NORM_HAMMING ); //汉明法暴力匹配 matcher.match ( descriptors_ref_, descriptors_curr_, matches ); // 筛选最佳匹配 float min_dis std::min_element ( matches.begin(), matches.end(), [] ( const cv::DMatch m1, const cv::DMatch m2 ) { return m1.distance m2.distance; //匹配规则 } )-distance; feature_matches_.clear(); for ( cv::DMatch m : matches ) { if ( m.distance maxfloat ( min_dis*match_ratio_, 30.0 ) ) { feature_matches_.push_back(m); } } coutgood matches: feature_matches_.size()endl; } void VisualOdometry::setRef3DPoints() { pts_3d_ref_.clear(); //重置 descriptors_ref_ Mat(); for ( size_t i0; ikeypoints_curr_.size(); i ) { double d ref_-findDepth(keypoints_curr_[i]); if ( d 0) { Vector3d p_cam ref_-camera_-pixel2camera( Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), d ); pts_3d_ref_.push_back( cv::Point3f( p_cam(0,0), p_cam(1,0), p_cam(2,0) )); descriptors_ref_.push_back(descriptors_curr_.row(i)); //每一行是一个关键点 } } } void VisualOdometry::poseEstimationPnP() { vectorcv::Point3f pts3d; vectorcv::Point2f pts2d; for ( cv::DMatch m:feature_matches_ ) { pts3d.push_back( pts_3d_ref_[m.queryIdx] ); //参考帧特征点[x,y,z] pts2d.push_back( keypoints_curr_[m.trainIdx].pt ); //新图片像素位置[x,y] } Mat K ( cv::Mat_double(3,3) ref_-camera_-fx_, 0, ref_-camera_-cx_, 0, ref_-camera_-fy_, ref_-camera_-cy_, 0,0,1 //内参方阵 ); Mat rvec, tvec, inliers; cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );//rvec输出相机旋转tvec输出相机平移 num_inliers_ inliers.rows; coutpnp inliers: num_inliers_endl; T_c_r_estimated_ SE3( SO3(rvec.atdouble(0,0), rvec.atdouble(1,0), rvec.atdouble(2,0)), Vector3d( tvec.atdouble(0,0), tvec.atdouble(1,0), tvec.atdouble(2,0)) ); //opencv的结果转为李代数得到相机的相对位置变换 } //检查位姿是否够好 bool VisualOdometry::checkEstimatedPose() { if ( num_inliers_ min_inliers_ ) //可靠匹配点是否充足 { coutreject because inlier is too small: num_inliers_endl; return false; } Sophus::Vector6d d T_c_r_estimated_.log(); //对SE3取对数得到李代数 if ( d.norm() 5.0 ) //模长 { coutreject because motion is too large: d.norm()endl; return false; } return true; } bool VisualOdometry::checkKeyFrame() { Sophus::Vector6d d T_c_r_estimated_.log(); Vector3d trans d.head3(); //Eigen的成员函数 Vector3d rot d.tail3(); if ( rot.norm() key_frame_min_rot || trans.norm() key_frame_min_trans ) return true; return false; } void VisualOdometry::addKeyFrame() { coutadding a key-frameendl; map_-insertKeyFrame ( curr_ ); } }复习一下J的计算全部代码slamlearning/project at master · yezi-bot/slamlearning slam前端设计
1. Camera类1.1 camera.h#ifndef CAMERA_H //宏定义 #define CAMERA_H #include myslam/common_include.h namespace myslam //命名空间 { class Camera { public: typedef std::shared_ptrCamera Ptr; //智能指针 float fx_, fy_, cx_, cy_, depth_scale_; Camera(); Camera ( float fx, float fy, float cx, float cy, float depth_scale0 ) : fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale ) {} Vector3d world2camera( const Vector3d p_w, const SE3 T_c_w ); Vector3d camera2world( const Vector3d p_c, const SE3 T_c_w ); Vector2d camera2pixel( const Vector3d p_c ); Vector3d pixel2camera( const Vector2d p_p, double depth1 ); Vector3d pixel2world ( const Vector2d p_p, const SE3 T_c_w, double depth1 ); Vector2d world2pixel ( const Vector3d p_w, const SE3 T_c_w ); }; } #endif1.2 camera.cpp#include myslam/camera.h namespace myslam { Camera::Camera() {} } Vector3d Camera :: world2camera( const Vector3d p_w, const SE3 T_c_w ) { return T_c_w*p_w; } Vector3d Camera :: camera2world( const Vector3d p_c, const SE3 T_c_w ) { return T_c_w.inverse()*p_c; } Vector2d Camera :: camera2pixel( const Vector3d p_c ) { return Vector2d( fx_ * p_c ( 0,0 ) / p_c ( 2,0 ) cx_, fy_ * p_c ( 1,0 ) / p_c ( 2,0 ) cy_ ); } Vector3d Camera :: pixel2camera( const Vector2d p_p, double depth ) //const Vector2d 这里是引用 { return Vector3d( depth * ( p_p ( 0,0 ) - cx_ ) / fx_, depth * ( p_p ( 1,0 ) - cy_ ) / fy_, depth ); } Vector3d Camera :: pixel2world( const Vector2d p_p, const SE3 T_c_w, double depth ) { Vector3d p_c pixel2camera( p_p, depth ); return camera2world( p_c, T_c_w ); } Vector2d Camera :: world2pixel( const Vector3d p_w, const SE3 T_c_w ) { Vector3d p_c world2camera( p_w, T_c_w ); return camera2pixel( p_c ); }2. Frame类2.1 frame.h#indef FRAME_H #define FRAME_H #include myslam/common_include.h #include myslam/camera.h namespace myslam { class MapPoint; class Frame{ public: typedef std::shared_ptrFrame Ptr; unsigned long id_; // id double time_stamp_; SE3 T_c_w_; Camera::Ptr camera_; Mat color_, depth_; std::vectorcv::KeyPoint keypoints_; //特征点 std::vectorMapPoint* map_points_; cv::Mat descriptors_; //描述子 Frame(); //默认构造函数 Frame(long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth); //带参构造函数 ~Frame(); //析构函数。对象销毁时自动调用 static Frame::Ptr createFrame(); //静态函数属于类本身直接调用,创建一个新对象 double findDepth(const cv::KeyPoint kp); //计算深度 Vector3d getCamCenter() const; bool isInFrame(const Vector3d pt_world); } } #endif2.2 frame.cpp#include myslam/frame.h namespace myslam { Frame::Frame() : id_(-1), time_stamp_(-1), camera_(nullptr) {}//构造函数初始化列表 Frame::Frame(long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth):id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth) {} Frame::~Frame() {} Frame::Ptr Frame::createFrame() { static long factory_id 0; return make_sharedFrame(factory_id); } double findDepth(const cv::KeyPoint kp) { int x cvRound(kp.pt.x); int y cvRound(kp.pt.y); ushort d depth.ptrushort(y)[x]; if(d ! 0) return double(d)/camera_-depth_scale_; else return -1.0; } Vector3d Frame::getCamCenter() const { return T_c_w_.inverse().translation(); //Twc和平移量 } bool Frame::isInFrame(const Vector3d pt_world) { Vector3d p_cam camera_-world2camera(pt_world, T_c_w_); if(p_cam(2,0)0) return false; Vector2d pixel camera_-camera2pixel(p_cam); return pixel(0,0)0 pixel(1,0)0 pixel(0,0)color_.cols pixel(1,0)color_.rows; } }3.mappoint类3.1 mappoint.h#indef MAPPOINT_H #define MAPPOINT_H #include myslam/common_include.h namespace myslam{ class Frame; //向前声明 class MapPoint{ public: typedef std::shared_ptrMapPoint Ptr; unsigned long id_; Vector3d pos_; //世界坐标 Vector3d norm_; //观测方向法向量 cv::Mat descriptor_; int observed_times_; int correct_times_; //参与优化次数 MapPoint(); MapPoint(long id, Vector3d position, Vector3d norm, cv::Mat descriptor); static MapPoint::Ptr createMapPoint(); //创建一个新的MapPoint对象 }; } #endif3.2 mappoint.cpp#include myslam/mappoint.h #include myslam/common_include.h namespace myslam { MapPoint::MapPoint() : id_(-1), pos_(0,0,0), norm_(0,0,0), descriptor_(cv::Mat()) {} MapPoint::MapPoint(long id, Vector3d position, Vector3d norm, cv::Mat descriptor) : id_(id), pos_(position), norm_(norm), descriptor_(descriptor) {} MapPoint::Ptr MapPoint::createMapPoint() { static long factory_id 0; return make_sharedMapPoint(factory_id); } }4.map类管理所有路标点如果只使用智能指针或者指针可以只向前声明要用到完整的功能还是要include4.1 .h#indef #define MAP_H #include myslam/common_include.h #include myslam/frame.h #include myslam/mappoint.h namespace myslam{ class Map{ public: typedef std::shared_ptrMap Ptr; std::unordered_mapunsigned long, MapPoint::Ptr map_points_; //所有地图点 std::unordered_mapunsigned long, Frame::Ptr keyframes_; //所有关键帧 Map(){} void insertKeyFrame(Frame::Ptr frame); //插入关键帧 void insertMapPoint(MapPoint::Ptr map_point); //插入地图点 }; } #endif4.2.cpp#include myslam/map.h namespace myslam { void Map::insertKeyFrame(Frame::Ptr frame) { if(keyframes_.find(frame-id_) keyframes_.end()) //没有这个点就插入 { keyframes_.insert(std::make_pair(frame-id_, frame)); } } void Map::insertMapPoint(MapPoint::Ptr map_point) { if(map_points_.find(map_point-id_) map_points_.end()) { map_points_.insert(std::make_pair(map_point-id_, map_point)); //make_pair构建值对插入 } } }5.config类#ifndef CONFIG_H #define CONFIG_H #include myslam/common_include.h namespace myslam { class Config { private: static std::shared_ptrConfig config_; cv::FileStorage file_; //打开yaml文件进行读写相机参数 Config () {} //放在私有区禁止外部实例化保证只有一个config对象 public: ~Config(); static void setParameterFile( const std::string filename ); template typename T //模板函数 static T get( const std::string key ) { return T( Config::config_-file_[key] ); //转成T类型 } }; } #endif#include myslam/config.h namespace myslam { std::shared_ptrConfig Config::config_ nullptr; //类型变量名初始值 Config::~Config() { if (file_.isOpened()) file_.release(); } void Config::setParameterFile(const std::string filename) { if (config_ nullptr) config_ std::shared_ptrConfig(new Config()); config_-file_ cv::FileStorage(filename, cv::FileStorage::READ); //打开yaml文件进行读写相机参数 if (config_-file_.isOpened() false) //打开失败 { LOG(ERROR) parameter file filename does not exist.; config_ nullptr; } } }6.VO6.1两帧的视觉里程计#ifndef VISUALODOMETRY_H #define VISUALODOMETRY_H #include myslam/common_include.h #include myslam/map.h #include opencv/features2d/features2d.h namespace myslam{ class VisualOdometry { public: typedef shared_ptrVisualOdometryPtr; enum VOState{ INITIALIZING -1; OK0; LOST }; VOState state_; Map::Ptr map_; Frame::Ptr ref_; //reference frame Frame::Ptr curr_; //current frame cv::Ptrcv::ORB orb_; vectorcv::Point3f pts_3d_ref_; vectorcv::Keypoint keypoint_curr_; Mat descriptors_curr_; Mat descriptors_ref_; vectorcv::DMatch feature_matches; SE3 T_c_r_estimated_; //估计的当前位姿 int num_inliers_; int num_lost_; int num_of_features_; double scale_factors_; int level_pyramid; float match_ratio_; //匹配率 int max_num_lost_; int min_inliers_; double key_frame_min_rot; double key_frame_min_trans; //关键帧条件 public: VisualOdometry(); ~VisualOdometry(); bool addFrame(Frame::Ptr frame); protected: void extractKeyPoints(); void computeDescriptors(); void featureMatching(); void poseEstimationPnP(); void setRef3DPoints(); void addKeyFrame(); bool checkEstimatedPose(); bool checkKeyFrame(); }; } #endif#include opencv2/highgui/highgui.hpp #include opencv2/imgproc/imgproc.hpp #include opencv2/calib3d/calib3d.hpp //PnP求解相机几何计算 #include algorithm #include boost/timer.hpp #include myslam/config.h #include myslam/visual_odometry.h namespace myslam { VisualOdometry::VisualOdometry() : state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 ) { num_of_features_ Config::getint ( number_of_features ); scale_factor_ Config::getdouble ( scale_factor ); level_pyramid_ Config::getint ( level_pyramid ); match_ratio_ Config::getfloat ( match_ratio ); max_num_lost_ Config::getfloat ( max_num_lost ); min_inliers_ Config::getint ( min_inliers ); key_frame_min_rot Config::getdouble ( keyframe_rotation ); key_frame_min_trans Config::getdouble ( keyframe_translation ); orb_ cv::ORB::create ( num_of_features_, scale_factor_, level_pyramid_ ); } VisualOdometry::~VisualOdometry() { } bool VisualOdometry::addFrame ( Frame::Ptr frame ) { switch ( state_ ) { case INITIALIZING: { state_ OK; curr_ ref_ frame; map_-insertKeyFrame ( frame ); extractKeyPoints(); computeDescriptors(); setRef3DPoints(); break; } case OK: { curr_ frame; extractKeyPoints(); computeDescriptors(); featureMatching(); poseEstimationPnP(); if ( checkEstimatedPose() true ) { curr_-T_c_w_ T_c_r_estimated_ * ref_-T_c_w_; // T_c_w T_c_r*T_r_w ref_ curr_; setRef3DPoints(); num_lost_ 0; if ( checkKeyFrame() true ) // 检查关键帧 { addKeyFrame(); } } else { num_lost_; if ( num_lost_ max_num_lost_ ) { state_ LOST; } return false; } break; } case LOST: { coutvo has lost.endl; break; } } return true; } void VisualOdometry::extractKeyPoints() { orb_-detect ( curr_-color_, keypoints_curr_ ); //检测关键点 } void VisualOdometry::computeDescriptors() { orb_-compute ( curr_-color_, keypoints_curr_, descriptors_curr_ ); //为关键点生成描述子 } //特征点匹配 void VisualOdometry::featureMatching() { vectorcv::DMatch matches; cv::BFMatcher matcher ( cv::NORM_HAMMING ); //汉明法暴力匹配 matcher.match ( descriptors_ref_, descriptors_curr_, matches ); // 筛选最佳匹配 float min_dis std::min_element ( matches.begin(), matches.end(), [] ( const cv::DMatch m1, const cv::DMatch m2 ) { return m1.distance m2.distance; //匹配规则 } )-distance; feature_matches_.clear(); for ( cv::DMatch m : matches ) { if ( m.distance maxfloat ( min_dis*match_ratio_, 30.0 ) ) { feature_matches_.push_back(m); } } coutgood matches: feature_matches_.size()endl; } void VisualOdometry::setRef3DPoints() { pts_3d_ref_.clear(); //重置 descriptors_ref_ Mat(); for ( size_t i0; ikeypoints_curr_.size(); i ) { double d ref_-findDepth(keypoints_curr_[i]); if ( d 0) { Vector3d p_cam ref_-camera_-pixel2camera( Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), d ); pts_3d_ref_.push_back( cv::Point3f( p_cam(0,0), p_cam(1,0), p_cam(2,0) )); descriptors_ref_.push_back(descriptors_curr_.row(i)); //每一行是一个关键点 } } } void VisualOdometry::poseEstimationPnP() { vectorcv::Point3f pts3d; vectorcv::Point2f pts2d; for ( cv::DMatch m:feature_matches_ ) { pts3d.push_back( pts_3d_ref_[m.queryIdx] ); //参考帧特征点[x,y,z] pts2d.push_back( keypoints_curr_[m.trainIdx].pt ); //新图片像素位置[x,y] } Mat K ( cv::Mat_double(3,3) ref_-camera_-fx_, 0, ref_-camera_-cx_, 0, ref_-camera_-fy_, ref_-camera_-cy_, 0,0,1 //内参方阵 ); Mat rvec, tvec, inliers; cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );//rvec输出相机旋转tvec输出相机平移 num_inliers_ inliers.rows; coutpnp inliers: num_inliers_endl; T_c_r_estimated_ SE3( SO3(rvec.atdouble(0,0), rvec.atdouble(1,0), rvec.atdouble(2,0)), Vector3d( tvec.atdouble(0,0), tvec.atdouble(1,0), tvec.atdouble(2,0)) ); //opencv的结果转为李代数得到相机的相对位置变换 } //检查位姿是否够好 bool VisualOdometry::checkEstimatedPose() { if ( num_inliers_ min_inliers_ ) //可靠匹配点是否充足 { coutreject because inlier is too small: num_inliers_endl; return false; } Sophus::Vector6d d T_c_r_estimated_.log(); //对SE3取对数得到李代数 if ( d.norm() 5.0 ) //模长 { coutreject because motion is too large: d.norm()endl; return false; } return true; } bool VisualOdometry::checkKeyFrame() { Sophus::Vector6d d T_c_r_estimated_.log(); Vector3d trans d.head3(); //Eigen的成员函数 Vector3d rot d.tail3(); if ( rot.norm() key_frame_min_rot || trans.norm() key_frame_min_trans ) return true; return false; } void VisualOdometry::addKeyFrame() { coutadding a key-frameendl; map_-insertKeyFrame ( curr_ ); } }复习一下J的计算全部代码slamlearning/project at master · yezi-bot/slamlearning