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)
GPU_version/vio/src/visualizer.cpp
+20 -15
View File
@@ -53,30 +53,35 @@ void Visualizer::publishMinimal(
header_msg.stamp = ros::Time(timestamp);
if(pub_pose_with_cov_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
{
//Quaterniond q;
Matrix<double,6,6> Cov;
// publish cam in world frame (Estimated odometry in the worls frame)
SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.T->inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
//q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
SE3 T_world_from_cam(frame->T_f_w_.T->inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
double yaw=asin(2*(frame->T_f_w_.T->unit_quaternion().w()*frame->T_f_w_.T->unit_quaternion().y()-
frame->T_f_w_.T->unit_quaternion().z()*frame->T_f_w_.T->unit_quaternion().x()));
Quaterniond q;
q = AngleAxisd(0.0, Vector3d::UnitX())
* AngleAxisd(0.0, Vector3d::UnitY())
* AngleAxisd(yaw-1.5708, Vector3d::UnitZ());
Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose_with_cov->header = header_msg;
msg_pose_with_cov->pose.pose.position.x = T_world_from_cam.translation().x();
msg_pose_with_cov->pose.pose.position.y = T_world_from_cam.translation().y();
msg_pose_with_cov->pose.pose.position.z = T_world_from_cam.translation().z();
msg_pose_with_cov->pose.pose.orientation.x = frame->T_f_w_.T->unit_quaternion().x()/frame->T_f_w_.T->unit_quaternion().norm();
msg_pose_with_cov->pose.pose.orientation.y = frame->T_f_w_.T->unit_quaternion().y()/frame->T_f_w_.T->unit_quaternion().norm();
msg_pose_with_cov->pose.pose.orientation.z = frame->T_f_w_.T->unit_quaternion().z()/frame->T_f_w_.T->unit_quaternion().norm();
msg_pose_with_cov->pose.pose.orientation.w = frame->T_f_w_.T->unit_quaternion().w()/frame->T_f_w_.T->unit_quaternion().norm();
msg_pose_with_cov->pose.pose.position.x = -1.0*T_world_from_cam.translation().x();
msg_pose_with_cov->pose.pose.position.y = -1.0*T_world_from_cam.translation().z();
msg_pose_with_cov->pose.pose.position.z = 0.0;
msg_pose_with_cov->pose.pose.orientation.x = q.x()/q.norm();
msg_pose_with_cov->pose.pose.orientation.y = q.y()/q.norm();
msg_pose_with_cov->pose.pose.orientation.z = q.z()/q.norm();
msg_pose_with_cov->pose.pose.orientation.w = q.w()/q.norm();
for(size_t i=0; i<36; ++i)
msg_pose_with_cov->pose.covariance[i] = Cov(i%6, i/6);
pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
vk::output_helper::publishPointMarker(
pub_points_, Eigen::Vector3d(-1.0*T_world_from_cam.translation().x(),-1.0*T_world_from_cam.translation().z(),0.0), "trajectory",
ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
}
}
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
vk::output_helper::publishPointMarker(
pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
}
}
void Visualizer::visualizeMarkers(