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
GPU_version/svo/include/gpu_svo/global.h
+21 -52
View File
@@ -27,7 +27,6 @@
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
//#include <vikit/performance_monitor.h>
#include <boost/shared_ptr.hpp>
#include<Eigen/StdVector>
#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
@@ -35,39 +34,13 @@
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
#endif
#include <ros/console.h>
#define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
#define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
#define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
#define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
#define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
#ifdef SVO_USE_ROS
#include <ros/console.h>
#define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
#define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
#define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
#define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
#define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
#else
#define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "<<x<<"\033[0;0m"<<std::endl;
#ifdef SVO_DEBUG_OUTPUT
#define SVO_DEBUG_STREAM(x) SVO_INFO_STREAM(x)
#else
#define SVO_DEBUG_STREAM(x)
#endif
#define SVO_WARN_STREAM(x) std::cerr<<"\033[0;33m[WARN] "<<x<<"\033[0;0m"<<std::endl;
#define SVO_ERROR_STREAM(x) std::cerr<<"\033[1;31m[ERROR] "<<x<<"\033[0;0m"<<std::endl;
#include <chrono> // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc.
#define SVO_WARN_STREAM_THROTTLE(rate, x) \
do { \
static double __log_stream_throttle__last_hit__ = 0.0; \
std::chrono::time_point<std::chrono::system_clock> __log_stream_throttle__now__ = \
std::chrono::system_clock::now(); \
if (__log_stream_throttle__last_hit__ + rate <= \
std::chrono::duration_cast<std::chrono::seconds>( \
__log_stream_throttle__now__.time_since_epoch()).count()) { \
__log_stream_throttle__last_hit__ = \
std::chrono::duration_cast<std::chrono::seconds>( \
__log_stream_throttle__now__.time_since_epoch()).count(); \
SVO_WARN_STREAM(x); \
} \
} while(0)
#endif
namespace svo
{
@@ -76,25 +49,21 @@ namespace svo
const double EPS = 0.0000000001;
const double PI = 3.14159265;
#ifdef SVO_TRACE
/*
extern vk::PerformanceMonitor* g_permon;
#define SVO_LOG(value) g_permon->log(std::string((#value)),(value))
#define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2)
#define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3)
#define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4)
#define SVO_START_TIMER(name) g_permon->startTimer((name))
#define SVO_STOP_TIMER(name) g_permon->stopTimer((name))
*/
#else
#define SVO_LOG(v)
#define SVO_LOG2(v1, v2)
#define SVO_LOG3(v1, v2, v3)
#define SVO_LOG4(v1, v2, v3, v4)
#define SVO_START_TIMER(name)
#define SVO_STOP_TIMER(name)
#endif
class SE2_5{
public:
double X_,Z_,tetha_,cos_theta_,sin_theta_;
SE3* T;
SE2_5(double x,double z,double tetha):X_(x),Z_(z),tetha_(tetha){
cos_theta_=cos(tetha_);
sin_theta_=sin(tetha_);
Eigen::Matrix<double, 3,3> R;
R<<cos_theta_,0.0,sin_theta_,
0.0,1.0,0.0,
-sin_theta_,0.0,cos_theta_;
T=new SE3(R,Vector3d(X_,0.0,Z_));
};
~SE2_5(){}
};
class Frame;
typedef boost::shared_ptr<Frame> FramePtr;
GPU_version/svo/include/gpu_svo/imu_integration.h
+1
View File
@@ -32,6 +32,7 @@ public:
bool init(boost::shared_ptr<svo::Frame>&);
Sophus::SE3 preintegrate_predict();
private:
FILE* logtime;
std::shared_ptr<gtsam::NavState> statePtr;
std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
std::shared_ptr<gtsam::Values> valuesPtr;
GPU_version/svo/include/gpu_svo/svoVisionFactor.h
+14 -23
View File
@@ -16,12 +16,11 @@
namespace svo {
class VisionFactor: public gtsam::NoiseModelFactor2<gtsam::Pose3,gtsam::Point3> {
class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
private:
// measurement information
gtsam::Point3 feature; ///<Unit-bearing vector of the feature.
int level; ///< Image pyramid level where feature was extracted.
gtsam::Pose3 pose; ///<Estimated pose in svo after GaussNewton optimization.
public:
/// shorthand for a smart pointer to a factor
@@ -29,36 +28,28 @@ namespace svo {
/**
* Constructor
* @param feature Unit-bearing vector of the feature.
* @param model is the standard deviation
* @param pose Camera pose.
* @param pointKey is the index of the landmark
* @param poseKey is the index of the camera pose
* @param landmark 3D position of the landmark
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param level Image pyramid level where feature was extracted.
*/
VisionFactor(const gtsam::Point3& feature,const int level,
VisionFactor(const gtsam::Pose3& pose,
const gtsam::SharedNoiseModel& H,
gtsam::Key poseKey,gtsam::Key landKey ) :
gtsam::NoiseModelFactor2<gtsam::Pose3,gtsam::Point3>(H, poseKey,landKey), feature(feature),level(level){}
gtsam::Key poseKey) :
gtsam::NoiseModelFactor1<gtsam::Pose3>(H, poseKey), pose(pose){}
virtual ~VisionFactor(){}
/// error function
/// @param p the Estimated pose in Pose3
/// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose3& p,const gtsam::Point3& l,
boost::optional<gtsam::Matrix&> H1 = boost::none,boost::optional<gtsam::Matrix&> H2 = boost::none) const {
gtsam::Vector evaluateError(const gtsam::Pose3& p
,boost::optional<gtsam::Matrix&> H1 = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
gtsam::Matrix26 J1;
gtsam::Matrix23 J2;
gtsam::Point3 point(p*l);
svo::Frame::jacobian_xyz2uv(point, J1);
svo::Frame::jacobian_l2uv(point, J2);
Vector2d e=(vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
if(H1)(*H1) = (gtsam::Matrix(2,6)<< J1*1.0 / (1<<level)).finished();
if(H2)(*H2) = (gtsam::Matrix(2,3)<< J2*1.0 / (1<<level)).finished();
return e;
Eigen::Matrix<double,3,3> E_r=0.5*(p.rotation().matrix().transpose()*pose.rotation().matrix()-pose.rotation().matrix().transpose()*p.rotation().matrix());
Vector3d e_t=p.translation()-pose.translation();
if(H1)(*H1) = (gtsam::Matrix(3,6)<< 1,0,0,0,0,0,
0,0,1,0,0,0,
0,0,0,0,1,0).finished();
return (gtsam::Matrix(3,1)<<e_t.x(),e_t.z(),-E_r(0,2)).finished();
}
// The second is a 'clone' function that allows the factor to be copied. Under most
// circumstances, the following code that employs the default copy constructor should