より多くの注釈で視覚SLAM第九講プログラムを学ぶ[1].
11203 ワード
本文は多くのこのリンクの内容を参考にして、それからいくつかの新しい注釈を加えました.
目次
Cameraクラス
camera.h
camera.cpp
Frameクラス
frame.h
frame.cpp
MapPointクラス
mappoint.h
mappoint.cpp
Mapクラス
map.h
map.cpp
Configクラス
config.h
config.cpp
Cameraクラス
高博:Cameraクラスはカメラの内部パラメータと外部パラメータを格納し、カメラ座標系、画素座標系、世界座標系の間の座標変換を完了します.もちろん、世界座標系ではカメラの(変動した)外部パラメータが必要で、パラメータの形で入力します.
camera.h
camera.cpp
Frameクラス
高博:Frameクラスは基本データユニットなので、多くの場所で使われていますが、初期の設計段階では、後で追加する可能性がある内容はまだ分かりません.したがって、ここのFrameクラスは基本的なデータストレージとインタフェースのみを提供します.後で新しい内容があれば、引き続き追加します.
frame.h
frame.cpp
MapPointクラス
高博:MapPointは道路標示点を表します.その世界座標を推定し,現在のフレームから抽出された特徴点を地図中の道路標識点と一致させてカメラの動きを推定するので,対応する記述子を格納する必要がある.さらに,その良し悪しの程度を評価する指標として,点が観測された回数と一致した回数を記録する.
mappoint.h
mappoint.cpp
Mapクラス
高博:Mapクラスはすべての道路標識点を管理し、新しい道路標識の追加、悪い道路標識の削除などの仕事を担当しています.VOのマッチングプロセスはMapと付き合うだけでよい.もちろんMapにも多くの操作がありますが、現段階では主なデータ構造のみを定義します.
map.h
map.cpp
Configクラス
高博:Configクラスはパラメータファイルの読み取りを担当し、プログラムのどこでもいつでもパラメータの値を提供することができます.だから私たちはConfigを単品モード(Singleton)に書きました.グローバルオブジェクトは1つしかありません.パラメータファイルを設定すると、オブジェクトを作成してパラメータファイルを読み込み、パラメータ値に任意の場所でアクセスし、プログラムの終了時に自動的に破棄されます(~関数?)
config.h
config.cpp
目次
Cameraクラス
camera.h
camera.cpp
Frameクラス
frame.h
frame.cpp
MapPointクラス
mappoint.h
mappoint.cpp
Mapクラス
map.h
map.cpp
Configクラス
config.h
config.cpp
Cameraクラス
高博:Cameraクラスはカメラの内部パラメータと外部パラメータを格納し、カメラ座標系、画素座標系、世界座標系の間の座標変換を完了します.もちろん、世界座標系ではカメラの(変動した)外部パラメータが必要で、パラメータの形で入力します.
camera.h
#ifndef CAMERA_H
#define CAMERA_H
#include "myslam/common_include.h"
namespace myslam
{
// Pinhole RGBD camera model
class Camera
{
public:
// , :
//Camera::Ptr camera_=New(Camera);
typedef std::shared_ptr Ptr;
//
float fx_, fy_, cx_, cy_, depth_scale_; // Camera intrinsics
// ,depth_scale
Camera();
Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) :
fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale )
{}
// coordinate transform: world, camera, pixel
// , , , T_c_w,
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 depth=1 );
Vector3d pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth=1 );
Vector2d world2pixel ( const Vector3d& p_w, const SE3& T_c_w );
};
}
#endif // CAMERA_H
camera.cpp
#include "myslam/camera.h"
//5.1.2
// , opencv, cv
namespace myslam
{
Camera::Camera()
{
}
// to :w_to_c
Vector3d Camera::world2camera ( const Vector3d& p_w, const SE3& T_c_w )
{
return T_c_w*p_w;
}
// to :c_to_w
Vector3d Camera::camera2world ( const Vector3d& p_c, const SE3& T_c_w )
{
return T_c_w.inverse() *p_c;
}
// to : 5.5,p_c 3*1 , , p_c(n)
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 )
{
return Vector3d (
( p_p ( 0,0 )-cx_ ) *depth/fx_,
( p_p ( 1,0 )-cy_ ) *depth/fy_,
depth
);
}
//︿( ̄︶ ̄)︿
Vector2d Camera::world2pixel ( const Vector3d& p_w, const SE3& T_c_w )
{
return camera2pixel ( world2camera ( p_w, T_c_w ) );
}
//︿( ̄︶ ̄)︿
Vector3d Camera::pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth )
{
return camera2world ( pixel2camera ( p_p, depth ), T_c_w );
}
}
Frameクラス
高博:Frameクラスは基本データユニットなので、多くの場所で使われていますが、初期の設計段階では、後で追加する可能性がある内容はまだ分かりません.したがって、ここのFrameクラスは基本的なデータストレージとインタフェースのみを提供します.後で新しい内容があれば、引き続き追加します.
frame.h
#ifndef FRAME_H
#define FRAME_H
#include "myslam/common_include.h"
#include "myslam/camera.h"
namespace myslam
{
// forward declare
class MapPoint;
class Frame
{
public:
typedef std::shared_ptr Ptr;
unsigned long id_; // id of this frame
double time_stamp_; // when it is recorded
SE3 T_c_w_; // transform from world to camera
Camera::Ptr camera_; // Pinhole RGBD Camera model
Mat color_, depth_; // color and depth image
public: // data members
Frame();//
Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera=nullptr, Mat color=Mat(), Mat depth=Mat() );
//~ , , https://blog.csdn.net/qq_15267341/article/details/78585570
~Frame();
// factory function
static Frame::Ptr createFrame();
// find the depth in depth map//color
double findDepth( const cv::KeyPoint& kp );
// Get Camera Center
Vector3d getCamCenter() const;
// check if a point is in this frame
bool isInFrame( const Vector3d& pt_world );
};
}
#endif // FRAME_H
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()
{
}
// , factory_id++ Frame , , , , id_,
// ID 。
Frame::Ptr Frame::createFrame()
{
static long factory_id = 0;
return Frame::Ptr( new Frame(factory_id++) );
}
// cv::KeyPoint&, color detect keypoint , depth
double Frame::findDepth ( const cv::KeyPoint& kp )
{
int x = cvRound(kp.pt.x);
int y = cvRound(kp.pt.y);
// .ptr ,
ushort d = depth_.ptr(y)[x];
if ( d!=0 )
{
// return
return double(d)/camera_->depth_scale_;
}
// ( )
else
{
// check the nearby points
int dx[4] = {-1,0,1,0};
int dy[4] = {0,-1,0,1};
for ( int i=0; i<4; i++ )
{
d = depth_.ptr( y+dy[i] )[x+dx[i]];
if ( d!=0 )
{
return double(d)/camera_->depth_scale_;
}
}
}
// , -1.0,
return -1.0;
}
// T_c_w_.inverse() R^(-1)*(-t), (0,0,0) ,
// 1
3
4
// int main(int argc,char** argv)
// {
// Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );
// Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 3d, 4*4
// T.rotate ( rotation_vector ); // rotation_vector
// T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // (1,3,4)
// cout << "translation =
"<world2camera( pt_world, T_c_w_ );
// Z ,Z , 0 return false
if ( p_cam(2,0)<0 )
return false;
Vector2d pixel = camera_->world2pixel( pt_world, T_c_w_ );
//xy 0 color
return pixel(0,0)>0 && pixel(1,0)>0
&& pixel(0,0)
MapPointクラス
高博:MapPointは道路標示点を表します.その世界座標を推定し,現在のフレームから抽出された特徴点を地図中の道路標識点と一致させてカメラの動きを推定するので,対応する記述子を格納する必要がある.さらに,その良し悪しの程度を評価する指標として,点が観測された回数と一致した回数を記録する.
mappoint.h
#ifndef MAPPOINT_H
#define MAPPOINT_H
namespace myslam
{
class Frame;
class MapPoint
{
public:
typedef shared_ptr Ptr;
unsigned long id_; // ID
Vector3d pos_; // Position in world
Vector3d norm_; // Normal of viewing direction ?
Mat descriptor_; // Descriptor for matching
int observed_times_; // being observed by feature matching algo.
int correct_times_; // being an inliner in pose estimation
MapPoint();
MapPoint( long id, Vector3d position, Vector3d norm );
// factory function
static MapPoint::Ptr createMapPoint();
};
}
#endif // MAPPOINT_H
mappoint.cpp
#include "myslam/common_include.h"
#include "myslam/mappoint.h"
namespace myslam
{
MapPoint::MapPoint()
: id_(-1), pos_(Vector3d(0,0,0)), norm_(Vector3d(0,0,0)), observed_times_(0), correct_times_(0)
{
}
MapPoint::MapPoint ( long id, Vector3d position, Vector3d norm )
: id_(id), pos_(position), norm_(norm), observed_times_(0), correct_times_(0)
{
}
//
MapPoint::Ptr MapPoint::createMapPoint()
{
static long factory_id = 0;
return MapPoint::Ptr(
new MapPoint( factory_id++, Vector3d(0,0,0), Vector3d(0,0,0) )
);
}
Mapクラス
高博:Mapクラスはすべての道路標識点を管理し、新しい道路標識の追加、悪い道路標識の削除などの仕事を担当しています.VOのマッチングプロセスはMapと付き合うだけでよい.もちろんMapにも多くの操作がありますが、現段階では主なデータ構造のみを定義します.
map.h
#ifndef MAP_H
#define MAP_H
#include "myslam/common_include.h"
#include "myslam/frame.h"
#include "myslam/mappoint.h"
namespace myslam
{
class Map
{
public:
typedef shared_ptr
map.cpp
#include "myslam/map.h"
namespace myslam
{
void Map::insertKeyFrame ( Frame::Ptr frame )
{
cout<id_) == keyframes_.end() )
{
// , pair , pair id frame
keyframes_.insert( make_pair(frame->id_, frame) );
}
else
{
keyframes_[ frame->id_ ] = frame;
}
}
void Map::insertMapPoint ( MapPoint::Ptr map_point )
{
//
if ( map_points_.find(map_point->id_) == map_points_.end() )
{
map_points_.insert( make_pair(map_point->id_, map_point) );
}
else
{
map_points_[map_point->id_] = map_point;
}
}
Configクラス
高博:Configクラスはパラメータファイルの読み取りを担当し、プログラムのどこでもいつでもパラメータの値を提供することができます.だから私たちはConfigを単品モード(Singleton)に書きました.グローバルオブジェクトは1つしかありません.パラメータファイルを設定すると、オブジェクトを作成してパラメータファイルを読み込み、パラメータ値に任意の場所でアクセスし、プログラムの終了時に自動的に破棄されます(~関数?)
config.h
#ifndef CONFIG_H
#define CONFIG_H
#include "myslam/common_include.h"
namespace myslam
{
class Config
{
private:
static std::shared_ptr config_;
cv::FileStorage file_;
Config () {} // private constructor makes a singleton
public:
~Config(); // close the file when deconstructing
// set a new config file
static void setParameterFile( const std::string& filename );
// access the parameter values
template< typename T >
static T get( const std::string& key )
{
return T( Config::config_->file_[key] );
}
};
}
#endif // CONFIG_H
config.cpp
#include "myslam/config.h"
namespace myslam
{
void Config::setParameterFile( const std::string& filename )
{
if ( config_ == nullptr )
config_ = shared_ptr(new Config);// config
config_->file_ = cv::FileStorage( filename.c_str(), cv::FileStorage::READ );
if ( config_->file_.isOpened() == false ) //
{
std::cerr<file_.release();
return;
}
}
Config::~Config()
{
if ( file_.isOpened() )
file_.release();// file
}
//
shared_ptr Config::config_ = nullptr;
}