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-09-06 14:00:39 +02:00
parent ce0beff85a
commit 2ada3523cc
7 changed files with 168 additions and 28928 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+8 -172
View File
@@ -1,172 +1,8 @@
[11:52:21.714] init depth filter
[11:52:30.966] DepthFilter: RESET.
[11:52:32.781] add key frame
[11:52:32.781] update seed
[11:52:32.781] init seeds
[11:52:32.786] DepthFilter: Initialized 80 new seeds
[11:52:56.055] add new frame
[11:52:56.055] update seed
[11:52:56.055] If point is visible? 0.027952, -0.014905, 0.048499
[11:52:56.055] If point is visible? 0.027753, -0.015271, 0.048499
[11:52:56.055] If point is visible? 0.027771, -0.015148, 0.048527
[11:52:56.055] If point is visible? 0.029140, -0.017475, 0.046917
[11:52:56.055] If point is visible? 0.028009, -0.017347, 0.047646
[11:52:56.055] If point is visible? 0.028030, -0.017226, 0.047678
[11:52:56.055] If point is visible? 0.028174, -0.017227, 0.047593
[11:52:56.055] If point is visible? 0.020293, -0.020609, 0.050188
[11:52:56.055] If point is visible? 0.020449, -0.020617, 0.050122
[11:52:56.055] If point is visible? 0.020400, -0.020853, 0.050044
[11:52:56.055] If point is visible? 0.020556, -0.020861, 0.049977
[11:52:56.055] If point is visible? 0.028621, -0.023543, 0.044525
[11:52:56.055] If point is visible? 0.019731, -0.024487, 0.048648
[11:52:56.055] If point is visible? 0.019759, -0.024372, 0.048694
[11:52:56.055] If point is visible? 0.026732, -0.030262, 0.041544
[11:52:56.055] If point is visible? 0.036145, -0.030595, 0.033388
[11:52:56.055] If point is visible? 0.035791, -0.031574, 0.032855
[11:52:56.055] If point is visible? 0.013190, -0.038644, 0.041103
[11:52:56.055] If point is visible? 0.021000, -0.039503, 0.036818
[11:52:56.055] If point is visible? 0.014550, -0.038122, 0.041130
[11:52:56.055] If point is visible? 0.022922, -0.039614, 0.035533
[11:52:56.055] If point is visible? 0.020696, -0.040387, 0.036024
[11:52:56.055] If point is visible? 0.020838, -0.040389, 0.035940
[11:52:56.055] If point is visible? 0.020980, -0.040391, 0.035856
[11:52:56.055] If point is visible? 0.031891, -0.007310, 0.047806
[11:52:56.055] If point is visible? 0.029369, -0.010330, 0.048851
[11:52:56.055] If point is visible? 0.029702, -0.012574, 0.048119
[11:52:56.055] If point is visible? 0.028635, -0.011822, 0.048948
[11:52:56.055] If point is visible? 0.028479, -0.015279, 0.048074
[11:52:56.055] If point is visible? 0.002201, -0.015321, 0.055823
[11:52:56.055] If point is visible? 0.027844, -0.017467, 0.047700
[11:52:56.055] If point is visible? 0.028134, -0.017469, 0.047528
[11:52:56.055] If point is visible? 0.002279, -0.014845, 0.055948
[11:52:56.055] If point is visible? 0.020332, -0.016985, 0.051512
[11:52:56.055] If point is visible? 0.017772, -0.020466, 0.051193
[11:52:56.055] If point is visible? 0.019668, -0.020577, 0.050450
[11:52:56.055] If point is visible? 0.016714, -0.020876, 0.051383
[11:52:56.055] If point is visible? 0.017035, -0.020898, 0.051269
[11:52:56.055] If point is visible? 0.019716, -0.020339, 0.050527
[11:52:56.055] If point is visible? 0.015707, -0.023889, 0.050380
[11:52:56.055] If point is visible? 0.016028, -0.023911, 0.050268
[11:52:56.055] If point is visible? 0.028686, -0.022018, 0.045256
[11:52:56.055] If point is visible? 0.019021, -0.024801, 0.048772
[11:52:56.055] If point is visible? 0.019079, -0.024570, 0.048865
[11:52:56.055] If point is visible? -0.017505, -0.022466, 0.050464
[11:52:56.055] If point is visible? -0.017814, -0.022169, 0.050487
[11:52:56.055] If point is visible? 0.045583, -0.028017, 0.022278
[11:52:56.055] If point is visible? 0.026594, -0.029818, 0.041951
[11:52:56.055] If point is visible? 0.026450, -0.030262, 0.041723
[11:52:56.055] If point is visible? 0.012977, -0.039745, 0.040110
[11:52:56.055] If point is visible? 0.013285, -0.039767, 0.039987
[11:52:56.055] If point is visible? 0.013593, -0.039789, 0.039862
[11:52:56.055] If point is visible? 0.013690, -0.039594, 0.040022
[11:52:56.055] If point is visible? -0.011674, 0.010977, 0.055690
[11:52:56.055] If point is visible? -0.014744, 0.014737, 0.054077
[11:52:56.055] If point is visible? -0.014718, 0.014253, 0.054213
[11:52:56.055] If point is visible? 0.026064, -0.011029, 0.050542
[11:52:56.055] If point is visible? 0.026660, -0.011042, 0.050228
[11:52:56.055] If point is visible? 0.027841, -0.011064, 0.049579
[11:52:56.055] If point is visible? 0.026116, -0.010529, 0.050622
[11:52:56.055] If point is visible? 0.026712, -0.010543, 0.050308
[11:52:56.055] If point is visible? 0.027892, -0.010564, 0.049659
[11:52:56.055] If point is visible? 0.029056, -0.010578, 0.048985
[11:52:56.055] If point is visible? 0.027940, -0.010064, 0.049736
[11:52:56.055] If point is visible? 0.021839, -0.010380, 0.052637
[11:52:56.055] If point is visible? 0.029292, -0.013562, 0.048101
[11:52:56.055] If point is visible? 0.020533, -0.010818, 0.053073
[11:52:56.055] If point is visible? 0.027934, -0.015028, 0.048471
[11:52:56.055] If point is visible? 0.028584, -0.014543, 0.048240
[11:52:56.055] If point is visible? 0.017772, -0.020466, 0.051193
[11:52:56.055] If point is visible? 0.019040, -0.020542, 0.050704
[11:52:56.055] If point is visible? 0.027966, -0.018435, 0.047262
[11:52:56.055] If point is visible? 0.015873, -0.023194, 0.050651
[11:52:56.055] If point is visible? 0.016515, -0.023239, 0.050425
[11:52:56.055] If point is visible? 0.017043, -0.023745, 0.050012
[11:52:56.055] If point is visible? 0.018310, -0.023823, 0.049525
[11:52:56.055] If point is visible? 0.017674, -0.020939, 0.051035
[11:52:56.055] If point is visible? 0.033505, -0.031977, 0.034813
[11:52:56.055] If point is visible? 0.033664, -0.031544, 0.035054
[11:52:56.055] If point is visible? 0.023273, -0.037710, 0.037325
[11:52:56.560] add new frame
[11:52:56.560] update seed
[11:52:56.560] If point is visible? 0.029261, -0.014902, 0.047707
[11:52:56.560] If point is visible? 0.029062, -0.015267, 0.047713
[11:52:56.560] If point is visible? 0.029081, -0.015145, 0.047741
[11:52:56.560] If point is visible? 0.030404, -0.017472, 0.046092
[11:52:56.560] If point is visible? 0.029295, -0.017344, 0.046854
[11:52:56.560] If point is visible? 0.029316, -0.017222, 0.046885
[11:52:56.560] If point is visible? 0.029458, -0.017224, 0.046795
[11:52:56.560] If point is visible? 0.021653, -0.020605, 0.049613
[11:52:56.560] If point is visible? 0.021807, -0.020613, 0.049542
[11:52:56.560] If point is visible? 0.021756, -0.020850, 0.049465
[11:52:56.560] If point is visible? 0.021910, -0.020858, 0.049394
[11:52:56.560] If point is visible? 0.029817, -0.023540, 0.043716
[11:52:56.560] If point is visible? 0.021047, -0.024484, 0.048089
[11:52:56.560] If point is visible? 0.021077, -0.024368, 0.048134
[11:52:56.560] If point is visible? 0.027845, -0.030258, 0.040790
[11:52:56.560] If point is visible? 0.037024, -0.030592, 0.032372
[11:52:56.560] If point is visible? 0.036656, -0.031570, 0.031848
[11:52:56.560] If point is visible? 0.014296, -0.038640, 0.040731
[11:52:56.560] If point is visible? 0.021982, -0.039499, 0.036228
[11:52:56.560] If point is visible? 0.015657, -0.038119, 0.040720
[11:52:56.560] If point is visible? 0.023867, -0.039610, 0.034888
[11:52:56.560] If point is visible? 0.021656, -0.040384, 0.035442
[11:52:56.560] If point is visible? 0.021796, -0.040386, 0.035355
[11:52:56.560] If point is visible? 0.021935, -0.040387, 0.035266
[11:52:56.560] If point is visible? 0.033179, -0.007307, 0.046904
[11:52:56.560] If point is visible? 0.030688, -0.010327, 0.048019
[11:52:56.560] If point is visible? 0.030999, -0.012571, 0.047278
[11:52:56.560] If point is visible? 0.029956, -0.011818, 0.048137
[11:52:56.560] If point is visible? 0.029776, -0.015275, 0.047268
[11:52:56.560] If point is visible? 0.003728, -0.015317, 0.055755
[11:52:56.560] If point is visible? 0.029131, -0.017463, 0.046912
[11:52:56.560] If point is visible? 0.029415, -0.017466, 0.046732
[11:52:56.560] If point is visible? 0.003808, -0.014842, 0.055878
[11:52:56.560] If point is visible? 0.021729, -0.016982, 0.050934
[11:52:56.560] If point is visible? 0.019161, -0.020463, 0.050688
[11:52:56.560] If point is visible? 0.021036, -0.020573, 0.049891
[11:52:56.560] If point is visible? 0.018109, -0.020873, 0.050908
[11:52:56.560] If point is visible? 0.018427, -0.020894, 0.050785
[11:52:56.560] If point is visible? 0.021086, -0.020336, 0.049967
[11:52:56.560] If point is visible? 0.017074, -0.023885, 0.049933
[11:52:56.560] If point is visible? 0.017392, -0.023908, 0.049812
[11:52:56.560] If point is visible? 0.029903, -0.022014, 0.044446
[11:52:56.560] If point is visible? 0.020342, -0.024797, 0.048232
[11:52:56.560] If point is visible? 0.020403, -0.024567, 0.048324
[11:52:56.560] If point is visible? -0.016122, -0.022463, 0.050955
[11:52:56.560] If point is visible? -0.016430, -0.022165, 0.050987
[11:52:56.560] If point is visible? 0.046145, -0.028014, 0.020999
[11:52:56.560] If point is visible? 0.027718, -0.029815, 0.041200
[11:52:56.560] If point is visible? 0.027568, -0.030258, 0.040977
[11:52:56.560] If point is visible? 0.014055, -0.039741, 0.039745
[11:52:56.560] If point is visible? 0.014360, -0.039764, 0.039613
[11:52:56.560] If point is visible? 0.014664, -0.039785, 0.039479
[11:52:56.560] If point is visible? 0.014765, -0.039591, 0.039637
[11:52:56.560] If point is visible? -0.010146, 0.010981, 0.056014
[11:52:56.560] If point is visible? -0.013260, 0.014740, 0.054489
[11:52:56.560] If point is visible? -0.013230, 0.014257, 0.054624
[11:52:56.560] If point is visible? 0.027432, -0.011025, 0.049803
[11:52:56.560] If point is visible? 0.028019, -0.011039, 0.049472
[11:52:56.560] If point is visible? 0.029180, -0.011060, 0.048790
[11:52:56.560] If point is visible? 0.027486, -0.010526, 0.049882
[11:52:56.560] If point is visible? 0.028072, -0.010540, 0.049551
[11:52:56.560] If point is visible? 0.029234, -0.010561, 0.048869
[11:52:56.560] If point is visible? 0.030379, -0.010574, 0.048162
[11:52:56.560] If point is visible? 0.029285, -0.010061, 0.048944
[11:52:56.560] If point is visible? 0.023268, -0.010376, 0.052017
[11:52:56.560] If point is visible? 0.030590, -0.013559, 0.047272
[11:52:56.560] If point is visible? 0.021974, -0.010814, 0.052489
[11:52:56.560] If point is visible? 0.029242, -0.015024, 0.047680
[11:52:56.560] If point is visible? 0.029886, -0.014539, 0.047431
[11:52:56.560] If point is visible? 0.019161, -0.020463, 0.050688
[11:52:56.560] If point is visible? 0.020415, -0.020539, 0.050164
[11:52:56.560] If point is visible? 0.029240, -0.018432, 0.046471
[11:52:56.560] If point is visible? 0.017248, -0.023190, 0.050200
[11:52:56.560] If point is visible? 0.017883, -0.023235, 0.049956
[11:52:56.560] If point is visible? 0.018399, -0.023741, 0.049528
[11:52:56.560] If point is visible? 0.019652, -0.023820, 0.049005
[11:52:56.560] If point is visible? 0.019059, -0.020936, 0.050533
[11:52:56.560] If point is visible? 0.034426, -0.031974, 0.033870
[11:52:56.560] If point is visible? 0.034591, -0.031540, 0.034107
[11:52:56.560] If point is visible? 0.024269, -0.037707, 0.036670
[11:53:04.046] DepthFilter stop thread invoked.
[11:53:04.046] DepthFilter interrupt and join thread
[13:53:07.139] init depth filter
[13:55:51.997] DepthFilter: RESET.
[13:55:57.552] add key frame
[13:55:57.552] update seed
[13:55:57.552] init seeds
[13:55:57.555] DepthFilter: Initialized 83 new seeds
[13:56:18.563] DepthFilter stop thread invoked.
[13:56:18.563] DepthFilter interrupt and join thread
GPU_version/vio/frame_handler_log.txt
+123 -28728
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/frame.h
+2 -2
View File
@@ -153,11 +153,11 @@ namespace vio {
J(0,0) = -z_inv; // -1/z
J(0,1) = x*z_inv_2; // x/z^2
J(0,2) = -(1.0 + pow(x,2)/z_inv_2); // -(1.0 + x^2/z^2)
J(0,2) = -(1.0 + pow(x,2)*z_inv_2); // -(1.0 + x^2/z^2)
J(1,0) = 1e-19; // 0
J(1,1) = y*z_inv_2; // y/z^2
J(1,2) = -x*y/z_inv_2; // -x*y/z^2
J(1,2) = -x*y*z_inv_2; // -x*y/z^2
}
inline static void jacobian_l2uv(
const Matrix<double,3,1>& p,
GPU_version/vio/kernel/compute-residual.cl
+4 -4
View File
@@ -44,11 +44,11 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
void jacobian_xyz2uv(float3 xyz_in_f, float* J)
{
J[0] = -(1. / xyz_in_f.z); // -1/z
J[1] = (xyz_in_f.x) * ((1. / xyz_in_f.z) * (1. / xyz_in_f.z)); // x/z^2
J[2] = -(1.0 + pow((xyz_in_f.x),2) / ((1. / xyz_in_f.z) * (1. / xyz_in_f.z))); // -(1.0 + x^2/z^2)
J[1] = (xyz_in_f.x) / (xyz_in_f.z * xyz_in_f.z); // x/z^2
J[2] = -(1.0 + pow((xyz_in_f.x),2) / (xyz_in_f.z * xyz_in_f.z)); // -(1.0 + x^2/z^2)
J[3] = 1e-19; // 0
J[4] = (xyz_in_f.y) * ((1. / xyz_in_f.z) * (1. / xyz_in_f.z)); // y/z^2
J[5] = -(xyz_in_f.x) * (xyz_in_f.y) / ((1. / xyz_in_f.z) * (1. / xyz_in_f.z)); // -x*y/z^2
J[4] = (xyz_in_f.y) / (xyz_in_f.z * xyz_in_f.z); // y/z^2
J[5] = -(xyz_in_f.x) * (xyz_in_f.y) / (xyz_in_f.z * xyz_in_f.z); // -x*y/z^2
}
void compute_hessain(float3 j, __global float* H, float w)
GPU_version/vio/src/frame_handler_mono.cpp
+25 -16
View File
@@ -145,8 +145,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
new_frame_->Cov_ = init_f.first;
initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_);
#if VIO_DEBUG
fprintf(log_,"[%s] Init: distance between two frames:%f ,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm(),
fprintf(log_,"[%s] Init: distance between two frames:%f ,angle between two frames x:%f ,z=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
last_frame_->T_f_w_.se2().translation().x()-new_frame_->T_f_w_.se2().translation().x(),
last_frame_->T_f_w_.se2().translation().y()-new_frame_->T_f_w_.se2().translation().y(),
last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch());
#endif
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_);
@@ -169,7 +170,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
stage_ = STAGE_DEFAULT_FRAME;
klt_homography_init_->reset();
#if VIO_DEBUG
fprintf(log_,"[%s] Init: Selected first frame. \n",vio::time_in_HH_MM_SS_MMM().c_str());
fprintf(log_,"[%s] Init: Selected Second frame. \n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
return RESULT_IS_KEYFRAME;
}
@@ -187,25 +188,32 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second;
#if VIO_DEBUG
fprintf(log_,"[%s] Reprojection Map nPoint:%d ,nCell:%d ,nMatches:%d ,distance between two frames:%f ,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
fprintf(log_,"[%s] After Reprojection Map nPoint:%d ,nCell:%d ,nMatches:%d ,distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
n_point,
reprojector_.n_trials_,reprojector_.n_matches_,
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm(),
last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch());
last_frame_->T_f_w_.se2().translation().x()-new_frame_->T_f_w_.se2().translation().x(),
last_frame_->T_f_w_.se2().translation().y()-new_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif
if( reprojector_.n_trials_ < 10)return RESULT_FAILURE;
if( reprojector_.n_trials_ < 10){
auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
return RESULT_FAILURE;
}
size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
/* pose_optimizer::optimizeGaussNewton(
Config::poseOptimThresh(), 70, false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);*/
#if VIO_DEBUG
fprintf(log_,"[%s] After pose optimization, distance between two frames:%f ,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm(),
last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch());
fprintf(log_,"[%s] After pose optimization, distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
last_frame_->T_f_w_.se2().translation().x()-new_frame_->T_f_w_.se2().translation().x(),
last_frame_->T_f_w_.se2().translation().y()-new_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI_4){
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2){
auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
@@ -216,9 +224,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first;
#if VIO_DEBUG
fprintf(log_,"[%s] Update EKF and 3D points, distance between two frames:%f ,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm(),
last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch());
fprintf(log_,"[%s] Update EKF and 3D points, distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
last_frame_->T_f_w_.se2().translation().x()-new_frame_->T_f_w_.se2().translation().x(),
last_frame_->T_f_w_.se2().translation().y()-new_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
depth_filter_->addFrame(new_frame_);
GPU_version/vio/src/pose_optimizer.cpp
+2 -2
View File
@@ -89,11 +89,11 @@ void optimizeGaussNewton(
Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
Frame::jacobian_xyz2uv(xyz_f, J);
Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
#if VIO_DEBUG
/*#if VIO_DEBUG
fprintf(log,"[%s] xyz_f:%f, %f, %f Reprojection Error: %f, %f Feature: %f, %f, %f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z(),e.x(),e.y(),
(*it)->point->pos_.x(),(*it)->point->pos_.y(),(*it)->point->pos_.z());
#endif
#endif*/
double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
e *= sqrt_inv_cov;
if(iter == 0)
GPU_version/vio/src/visualizer.cpp
+4 -4
View File
@@ -52,14 +52,14 @@ void Visualizer::publishMinimal(
{
// publish cam in world frame (Estimated odometry in the worls frame)
auto odom=ukf.get_location();
//Rviz frame z up, x forward, y left -> right hands (yaw counts from x)
//Rviz frame z up, x right, y forward -> right hands (yaw counts from y)
//Camera frame z front, x right, y down -> right hands (pitch counts from z)
//IMU frame y front, x right, z up -> left hands (theta counts from y)
Quaterniond q(AngleAxisd(-odom.second.pitch(), Vector3d::UnitZ()));
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 = odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.y = -odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.z = 0.0;
msg_pose_with_cov->pose.pose.orientation.x = q.x();
msg_pose_with_cov->pose.pose.orientation.y = q.y();
@@ -74,7 +74,7 @@ void Visualizer::publishMinimal(
pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
publishPointMarker(
pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),-odom.second.se2().translation()(0),0.0), "trajectory",
pub_points_, Eigen::Vector3d(odom.second.se2().translation()(0),odom.second.se2().translation()(1),0.0), "trajectory",
ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
}
}