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/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;