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

Edit visual

Browse Source
This commit is contained in:
gfuse
2021-06-07 09:48:47 +02:00
parent 27f92bc7e7
commit 8cce404073
5 changed files with 29 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/svo/include/gpu_svo/global.h
+2 -1
View File
@@ -65,7 +65,8 @@ namespace svo
void set(){
X_=T->translation().x();
Z_=T->translation().z();
tetha_=atan(T->matrix()(0,2)/T->matrix()(0,0));
tetha_=asin(2*(T->unit_quaternion().w()*T->unit_quaternion().y()-
T->unit_quaternion().z()*T->unit_quaternion().x()));
cos_theta_=cos(tetha_);
sin_theta_=sin(tetha_);
}
GPU_version/svo/src/imu_integration.cpp
+3 -9
View File
@@ -15,17 +15,11 @@
#include <vikit/params_helper.h>
#include <gpu_svo/config.h>
#include <gpu_svo/for_it.hpp>
#define debug 1
#define debug 0
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
valuesPtr=std::make_shared<gtsam::Values>();
// Example: pitch and roll of aircraft in an ENU Cartesian frame.
// If pitch and roll are zero for an aerospace frame,
// that means Z is pointing down, i.e., direction of Z = (0,0,-1)
//gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, svo::Config::Gravity(),1.0);
//parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(0.0,0.0,0.0)));
//parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
@@ -49,8 +43,8 @@ Imu_Integration::~Imu_Integration(){
bool Imu_Integration::update(double* imu){
if(syn)return false;
if(imu_factor_id<1)return false;
if(abs(imu[0])<1.0)imu[0]=pow(imu[0],3);
if(abs(imu[1])<1.0)imu[0]=pow(imu[1],3);
if(abs(imu[0])<1.0)imu[0]=pow(imu[0],3);//picked up from your code
if(abs(imu[1])<1.0)imu[0]=pow(imu[1],3);//picked up from your code
auto in=std::chrono::steady_clock::now();
if(std::chrono::duration<double>(in-t_1).count()>20.0){
t_1=in;
GPU_version/svo/src/initialization.cpp
+2 -1
View File
@@ -70,7 +70,8 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median;
SE3 tem=T_cur_from_ref_ * (*frame_ref_->T_f_w_.T);
frame_cur->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0)));
frame_cur->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),asin(2*(tem.unit_quaternion().w()*tem.unit_quaternion().y()-
tem.unit_quaternion().z()*tem.unit_quaternion().x())));
tem.translation() = -frame_cur->T_f_w_.T->rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
frame_cur->T_f_w_.X_ = tem.translation().x();
frame_cur->T_f_w_.Z_ = tem.translation().z();
GPU_version/svo/src/map.cpp
+2 -1
View File
@@ -179,7 +179,8 @@ void Map::transform(const Matrix3d& R, const Vector3d& t, const double& s)
Vector3d pos = s*R*(*it)->pos() + t;
Matrix3d rot = R*(*it)->T_f_w_.T->rotation_matrix().inverse();
SE3 inv=SE3(rot, pos).inverse();
(*it)->T_f_w_ = SE2_5(inv.translation().x(),inv.translation().z(),atan(inv.rotation_matrix()(0,2)/inv.rotation_matrix()(0,0)));
(*it)->T_f_w_ = SE2_5(inv.translation().x(),inv.translation().z(),asin(2*(inv.unit_quaternion().w()*inv.unit_quaternion().y()-
inv.unit_quaternion().z()*inv.unit_quaternion().x())));
for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)
{
if((*ftr)->point == NULL)