From 8cce4040736599e0d08f1021b053d732e68bbb85 Mon Sep 17 00:00:00 2001 From: gfuse Date: Mon, 7 Jun 2021 09:48:47 +0200 Subject: [PATCH] Edit visual --- GPU_version/svo/include/gpu_svo/global.h | 3 +- GPU_version/svo/src/imu_integration.cpp | 12 ++------ GPU_version/svo/src/initialization.cpp | 3 +- GPU_version/svo/src/map.cpp | 3 +- GPU_version/vio/src/visualizer.cpp | 35 ++++++++++++++---------- 5 files changed, 29 insertions(+), 27 deletions(-) diff --git a/GPU_version/svo/include/gpu_svo/global.h b/GPU_version/svo/include/gpu_svo/global.h index a307503..2cfd63b 100644 --- a/GPU_version/svo/include/gpu_svo/global.h +++ b/GPU_version/svo/include/gpu_svo/global.h @@ -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_); } diff --git a/GPU_version/svo/src/imu_integration.cpp b/GPU_version/svo/src/imu_integration.cpp index 20d06f0..f7d47a2 100644 --- a/GPU_version/svo/src/imu_integration.cpp +++ b/GPU_version/svo/src/imu_integration.cpp @@ -15,17 +15,11 @@ #include #include #include -#define debug 1 +#define debug 0 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ graphPtr=std::make_shared(); valuesPtr=std::make_shared(); - // 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(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z()))); parameterPtr = boost::shared_ptr(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(in-t_1).count()>20.0){ t_1=in; diff --git a/GPU_version/svo/src/initialization.cpp b/GPU_version/svo/src/initialization.cpp index 1681a9e..6d54a36 100644 --- a/GPU_version/svo/src/initialization.cpp +++ b/GPU_version/svo/src/initialization.cpp @@ -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(); diff --git a/GPU_version/svo/src/map.cpp b/GPU_version/svo/src/map.cpp index e46f9b0..6b27b1c 100644 --- a/GPU_version/svo/src/map.cpp +++ b/GPU_version/svo/src/map.cpp @@ -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) diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index 75047cc..b7f860f 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -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 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(