search

ethz-asl / ethzasl_ptam

Modified version of Parallel Tracking and Mapping (PTAM)
http://wiki.ros.org/ethzasl_ptam
235 stars 184 forks source link

Every second covariance estimate NaN in pose_world topic #68

Open nplayle opened 9 years ago

nplayle commented 9 years ago

Hi Everyone,

I think this is pretty repeatable for me, hopefully someone could verify by rostopic echo vslam/pose_world and letting me know.

Every second covariance estimate from PTAM has the first row and first column as NaN, shown below. 

header: 
  seq: 2787
  stamp: 
    secs: 1433362523
    nsecs: 34777761
  frame_id: camera
pose: 
  pose: 
    position: 
      x: 0.495793035514
      y: -0.0588677570468
      z: 1.23484701532
    orientation: 
      x: 0.999019662356
      y: -0.00353029160761
      z: -0.0421113464728
      w: 0.0131865752106
  covariance: [0.002535610311191974, 0.000776077416535294, 0.001676215459141258, 0.0007406145904691738, 0.0022517934742184425, 0.00030223399935076843, 0.0007760774165353798, 0.0029751898692453897, 0.0008529385689341823, 0.002500539301581462, 0.0004894859822209892, 0.0011451353427348134, 0.00167621545914125, 0.0008529385689342104, 0.0015569398653620006, 0.0006562760727783601, 0.0015886047437009886, 0.0004496559399827924, 0.0007406145904692383, 0.002500539301581463, 0.0006562760727783344, 0.002124665974364864, 0.0005203882798103098, 0.0008878096506912375, 0.002251793474218441, 0.0004894859822208856, 0.0015886047437009942, 0.0005203882798102397, 0.002029343476628026, 0.0003201370057766844, 0.00030223399935073894, 0.0011451353427348106, 0.00044965593998278505, 0.0008878096506912344, 0.000320137005776663, 0.0007455878788647277]

---
header: 
  seq: 2788
  stamp: 
    secs: 1433362523
    nsecs: 113442765
  frame_id: camera
pose: 
  pose: 
    position: 
      x: 0.495601469326
      y: -0.058929631568
      z: 1.2335834671
    orientation: 
      x: 0.999015186335
      y: -0.00358261204661
      z: -0.042197564747
      w: 0.013235856292
  covariance: [nan, nan, nan, nan, nan, nan, nan, 0.00269656567851687, 0.0006226468685732237, 0.002275876753019883, 0.00036926868095865255, 0.0010026406915631526, nan, 0.0006226468685731896, 0.0015745068839077819, 0.00041050278302538285, 0.001579970311495381, 0.000403054695642134, nan, 0.002275876753019883, 0.00041050278302541955, 0.001945971645771719, 0.00045527754669658923, 0.0007589469444526273, nan, 0.0003692686809587051, 0.00157997031149538, 0.0004552775466966209, 0.0019450416237665215, 0.00035606023608913257, nan, 0.001002640691563155, 0.0004030546956421426, 0.0007589469444526293, 0.00035606023608913625, 0.0007088289372466638]

---
header: 
  seq: 2789
  stamp: 
    secs: 1433362523
    nsecs: 195888082
  frame_id: camera
pose: 
  pose: 
    position: 
      x: 0.495521933644
      y: -0.0598647018254
      z: 1.23469567868
    orientation: 
      x: 0.999025689252
      y: -0.00372599436072
      z: -0.0420470865254
      w: 0.0128775655495
  covariance: [0.0028473619596112616, 0.0005871928024775257, 0.0020306494289498967, 0.000297367155552275, 0.0025638960812720967, 0.0005204963093914762, 0.0005871928024776803, 0.0030378194360064508, 0.0008727668383390994, 0.002552772346996778, 0.0006737986542753686, 0.0011988504875758846, 0.002030649428949906, 0.0008727668383390338, 0.001838414206051488, 0.0006401107349943718, 0.0019186125374644999, 0.0004885155301110796, 0.0002973671555524978, 0.0025527723469967786, 0.0006401107349944372, 0.0021698830488582517, 0.000435906762644174, 0.0009257237040134314, 0.0025638960812720997, 0.0006737986542752528, 0.0019186125374644942, 0.0004359067626440439, 0.0023364453082208056, 0.000478642940568805, 0.0005204963093914971, 0.0011988504875758816, 0.0004885155301110925, 0.0009257237040134285, 0.0004786429405688241, 0.0007943818882323011]

---
header: 
  seq: 2790
  stamp: 
    secs: 1433362523
    nsecs: 279099467
  frame_id: camera
pose: 
  pose: 
    position: 
      x: 0.495353299806
      y: -0.0597982902279
      z: 1.23455647308
    orientation: 
      x: 0.999028743501
      y: -0.00363626553808
      z: -0.0419718117598
      w: 0.0129117872402
  covariance: [nan, nan, nan, nan, nan, nan, nan, 0.0028445347453277116, 0.0005578313458114725, 0.002383241877591262, 0.0004661583692221177, 0.0011044707403657487, nan, 0.0005578313458115397, 0.0017257399669420926, 0.00028355035977630145, 0.0017907881969454838, 0.0004163420674271066, nan, 0.0023832418775912604, 0.0002835503597762068, 0.002024475770455104, 0.0005355571353195634, 0.0008341654551759523, nan, 0.0004661583692219933, 0.0017907881969454775, 0.0005355571353194868, 0.002234407425815829, 0.00037238306913927585, nan, 0.001104470740365753, 0.000416342067427093, 0.0008341654551759569, 0.0003723830691392486, 0.0007722252693918936]

Could someone please confirm?

nplayle commented 9 years ago

Hello,

I think I have solved this. It appears that TooN wasn't initialising the matrix to zeros when it was initialised and as a result some elements were occasionally interpreted as NaN for some reason.

Changing TooN::Matrix<6> CovRot; to TooN::Matrix<6> CovRot = Zeros(6,6); in System.cc appears to have solved the issue.

markusachtelik commented 9 years ago

great you spotted this! could you open a PR for this?