pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

terminal commit

Browse Source
This commit is contained in:
admin1
2022-08-30 18:44:20 +03:00
parent 979c7a2250
commit 89f25ad747
8 changed files with 444036 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
global_fusion/src/globalOpt.cpp
View File

@@ -14,6 +14,8 @@
*******************************************************/
#include "globalOpt.h"
#include <utility>
#include "Factors.h"
GlobalOptimization::GlobalOptimization() {
@@ -43,6 +45,10 @@ void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quate
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
// std::cout << "inputOdom tCov.size() " << tCov.size() << std::endl;
// localTranslationCovariance[t] = tCov;
// localRotationCovariance[t] = rCov;
Eigen::Quaterniond globalQ;
globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
@@ -82,7 +88,7 @@ void GlobalOptimization::inputGPS(double t, double latitude, double longitude, d
newGPS = true;
}
void GlobalOptimization::optimize(){
[[noreturn]] void GlobalOptimization::optimize(){
while(true){
if (newGPS){
newGPS = false;
@@ -138,6 +144,26 @@ void GlobalOptimization::optimize(){
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);
// auto tCov = localTranslationCovariance[iterVIO->first];
// Eigen::VectorXd translationCovariance(tCov.size());
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// translationCovariance[r*3 + c] = tCov[r*3 + c];
// }
// }
// std::cout << "Eigen translation covariance norm is: " << translationCovariance.norm() << std::endl;
//
// auto rCov = localRotationCovariance[iterVIO->first];
// Eigen::VectorXd rotationCovariance(rCov.size());
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// rotationCovariance[r*3 + c] = rCov[r*3 + c];
// }
// }
// std::cout << "Eigen rotation covariance norm is: " << rotationCovariance.norm() << std::endl;
// std::cout << "Eigen rotation covariance first 3 elems: " << rotationCovariance[0] << " " <<
// rotationCovariance[1] << " " << rotationCovariance[2] << std::endl;
ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
iQj.w(), iQj.x(), iQj.y(), iQj.z(),
0.1, 0.01);

5
global_fusion/src/globalOpt.h
View File

@@ -48,11 +48,14 @@ private:
// this function is interesting. I suppose it transforms from the gps coordinate system to the
// local (vins) coordinate system. Which has the center at the start of the sequence by the way.
void GPS2XYZ(double latitude, double longitude, double altitude, double* xyz);
void optimize();
[[noreturn]] void optimize();
void updateGlobalPath();
// format t, tx, ty, tz, qw, qx, qy, qz
map<double, vector<double>> localPoseMap;
map<double, vector<double>> localTranslationCovariance;
map<double, vector<double>> localRotationCovariance;
map<double, vector<double>> globalPoseMap;
map<double, vector<double>> GPSPositionMap;
bool initGPS;

66
global_fusion/src/globalOptNode.cpp
View File

@@ -16,6 +16,7 @@
#include "ros/ros.h"
#include "globalOpt.h"
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Image.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
@@ -99,6 +100,39 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
vio_q.x() = pose_msg->pose.pose.orientation.x;
vio_q.y() = pose_msg->pose.pose.orientation.y;
vio_q.z() = pose_msg->pose.pose.orientation.z;
// auto cov = pose_msg->pose.covariance;
// auto rCovArr = pose_msg->twist.covariance;
// auto tCovArr = pose_msg->pose.covariance;
// convert boost::array to std::array
// std::vector<double> transCov(9);
// std::vector<double> rotCov(9);
// std::vector<double> tCov(tCovArr.size());
// Presuming that the first 3 block shows the translation cov, and the rest shows rotation cov
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// transCov[r*3 + c] = cov[r*6 + c];
// }
// }
//
// for (int r = 3; r < 6; r++){
// for (int c = 3; c < 6; c++){
// rotCov[(r-3)*3 + (c-3)] = cov[r*6 + c];
// }
// }
// std::cout << "The obtained below matrix from cov" << std::endl;
// // debugging. Output the below matrix
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// std::cout << rotCov[r*3 + c] << " ";
// }
// std::cout << std::endl;
// }
// ROS_INFO_STREAM("the filled vector size" << rCov.size() << " the filled vector first element: " << rCov[0]);
globalEstimator.inputOdom(t, vio_t, vio_q);
@@ -144,7 +178,6 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
nav_msgs::Odometry odometry;
odometry.header = pose_msg->header;
odometry.header.frame_id = "gps_global";
odometry.child_frame_id = "cam0_gps";
odometry.pose.pose.position.x = global_t.x();
odometry.pose.pose.position.y = global_t.y();
odometry.pose.pose.position.z = global_t.z();
@@ -156,18 +189,6 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
pub_global_path.publish(*global_path);
publish_car_model(t, global_t, global_q);
// Publish the tf so that Camera can bind to it.
tf::StampedTransform trans;
trans.frame_id_ = "gps_global";
trans.child_frame_id_ = "cam0_gps";
trans.stamp_ = pose_msg->header.stamp;
tf::Quaternion quat(global_q.x(), global_q.y(), global_q.z(), global_q.w());
trans.setRotation(quat);
tf::Vector3 orig(global_t.x(), global_t.y(), global_t.z());
trans.setOrigin(orig);
TfBr->sendTransform(trans);
// pub_imu_tf_global.publish(trans);
// write result to file
std::ofstream foutC("/home/admin1/workspace/catkin_ws_ov/src/open_vins/global_fusion/data/vio_global.csv", ios::app);
foutC.setf(ios::fixed, ios::floatfield);
@@ -184,6 +205,24 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
foutC.close();
}
void gps_img_callback(sensor_msgs::ImageConstPtr msg){
Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
globalEstimator.getGlobalOdom(global_t, global_q);
// Publish the tf so that Camera can bind to it.
tf::StampedTransform trans;
trans.frame_id_ = "gps_global";
trans.child_frame_id_ = "imu_gps";
trans.stamp_ = msg->header.stamp;
tf::Quaternion quat(global_q.x(), global_q.y(), global_q.z(), global_q.w());
trans.setRotation(quat);
tf::Vector3 orig(global_t.x(), global_t.y(), global_t.z());
trans.setOrigin(orig);
TfBr->sendTransform(trans);
// pub_imu_tf_global.publish(trans);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "globalEstimator");
@@ -194,6 +233,7 @@ int main(int argc, char **argv)
ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
ros::Subscriber sub_img = n.subscribe("/global_fusion_node/sensor_left", 100, gps_img_callback);
pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
// pub_imu_tf_global = n.advertise<tf::StampedTransform>("tf_imu_global", 100);