orb_slam_2_ros
orb_slam_2_ros copied to clipboard
TF as poseStamped is changing
hey guys, using orbslam2 monocular sometimes spits out different y, check these outputs they are both from the same simulation and initial.... well... everything: here y is negative:
---
header:
seq: 13388
stamp:
secs: 972
nsecs: 236000000
frame_id: "map"
pose:
position:
x: -6.61653301628
y: -0.481447576538
z: -0.111893122552
orientation:
x: -0.00248761751703
y: 0.000779075068147
z: 0.999625442736
w: -0.0272429630413
---
header:
seq: 13389
stamp:
secs: 972
nsecs: 300000000
frame_id: "map"
pose:
position:
x: -6.61575999469
y: -0.480828229211
z: -0.112066359357
orientation:
x: -0.00227222304475
y: 0.000758287425639
z: 0.99962147271
w: -0.0274075409438
---
header:
seq: 13390
stamp:
secs: 972
nsecs: 368000000
frame_id: "map"
pose:
position:
x: -6.61642443585
y: -0.481003090998
z: -0.112192068774
orientation:
x: -0.00235306388596
y: 0.000827180501587
z: 0.999622534554
w: -0.0273599575024
---
and here y is positive:
---
header:
seq: 1730
stamp:
secs: 334
nsecs: 956000000
frame_id: "map"
pose:
position:
x: -6.73923487699
y: 1.70155722934
z: -0.151763144649
orientation:
x: 0.00178283461538
y: 0.033923785254
z: 0.990028578768
w: 0.136709222501
---
header:
seq: 1731
stamp:
secs: 334
nsecs: 988000000
frame_id: "map"
pose:
position:
x: -6.73982137557
y: 1.70241165084
z: -0.150960128611
orientation:
x: 0.00158696851435
y: 0.0345710241416
z: 0.990011497272
w: 0.136673190822
---
header:
seq: 1732
stamp:
secs: 335
nsecs: 20000000
frame_id: "map"
pose:
position:
x: -6.73916918961
y: 1.70191212379
z: -0.151252794634
orientation:
x: 0.00183299876932
y: 0.034341660749
z: 0.990027035488
w: 0.136615370491
---
I thought that commenting out Lines:609&610 in Tracking.cc would solve this but i was wrong:
// Set Frame Poses
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
/*Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));*/
mCurrentFrame.SetPose(Tcw);
any kind of help would be appreciated