public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

track the trajectory with noise

Browse Source
This commit is contained in:
gfuse
2021-05-29 16:53:00 +02:00
parent cd6f0c5b97
commit faf00a6aad
20 changed files with 201 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/svo/include/gpu_svo/frame.h
+16 -5
View File
@@ -36,6 +36,7 @@ struct Feature;
typedef list<Feature*> Features;
typedef vector<cv::Mat> ImgPyr;
/// A frame saves the image, the associated features and the estimated pose.
class Frame : boost::noncopyable
{
@@ -46,7 +47,7 @@ public:
int id_; //!< Unique id of the frame.
double timestamp_; //!< Timestamp of when the image was recorded.
vk::AbstractCamera* cam_; //!< Camera model.
Sophus::SE3 T_f_w_; //!< Transform (f)rame from (w)orld.
SE2_5 T_f_w_; //!< Transform (f)rame from (w)orld.
Matrix<double, 6, 6> Cov_; //!< Covariance.
ImgPyr img_pyr_; //!< Image Pyramid.
Features fts_; //!< List of features in the image.
@@ -91,7 +92,11 @@ public:
inline bool isKeyframe() const { return is_keyframe_; }
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); }
inline Vector2d w2c(const Vector3d& xyz_w) const {
return cam_->world2cam( Vector3d(xyz_w.x()*T_f_w_.cos_theta_+xyz_w.z()*T_f_w_.sin_theta_+T_f_w_.X_,
xyz_w.y(),
-xyz_w.x()*T_f_w_.sin_theta_+xyz_w.z()*T_f_w_.cos_theta_+T_f_w_.Z_) );
}
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); }
@@ -102,16 +107,22 @@ public:
inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); }
/// Transforms point coordinates in world-frame (w) to camera-frams (f).
inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; }
inline Vector3d w2f(const Vector3d& xyz_w) const {
return Vector3d(xyz_w.x()*T_f_w_.cos_theta_+xyz_w.z()*T_f_w_.sin_theta_+T_f_w_.X_,
xyz_w.y(),
-xyz_w.x()*T_f_w_.sin_theta_+xyz_w.z()*T_f_w_.cos_theta_+T_f_w_.Z_);
}
/// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; }
inline Vector3d f2w(const Vector3d& f) const {
return T_f_w_.T->inverse() * f;
}
/// Projects Point from unit sphere (f) in camera pixels (c).
inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
/// Return the pose of the frame in the (w)orld coordinate frame.
inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
inline Vector3d pos() const { return T_f_w_.T->inverse().translation(); }
/// Frame jacobian for projection of 3D point in (f)rame coordinate to