solve bandle adjustment sync issue
This commit is contained in:
gfuse
@@ -1,60 +1,272 @@
|
|||||||
[16:55:41.271] New key frame
|
[14:17:35.009] New key frame
|
||||||
[16:55:41.271] BA loop run
|
[14:17:35.009] BA loop run
|
||||||
[16:55:41.272] init error: 575.642695
|
[14:17:35.010] init error: 148.036207
|
||||||
[16:55:41.273] end error: 0.013166
|
[14:17:35.011] end error: 0.026000
|
||||||
[16:55:41.405] New key frame
|
[14:17:35.957] New key frame
|
||||||
[16:55:41.405] BA loop run
|
[14:17:35.957] BA loop run
|
||||||
[16:55:41.405] init error: 189.835833
|
[14:17:35.958] init error: 38918.365174
|
||||||
[16:55:41.407] end error: 0.468588
|
[14:17:35.958] end error: 0.013444
|
||||||
[16:55:41.507] New key frame
|
[14:17:36.925] New key frame
|
||||||
[16:55:41.507] BA loop run
|
[14:17:36.925] BA loop run
|
||||||
[16:55:41.508] init error: 911.856919
|
[14:17:36.926] init error: 212.526317
|
||||||
[16:55:41.510] end error: 1.789062
|
[14:17:36.927] end error: 0.000262
|
||||||
[16:55:41.611] New key frame
|
[14:17:37.760] New key frame
|
||||||
[16:55:41.611] BA loop run
|
[14:17:37.760] BA loop run
|
||||||
[16:55:41.612] init error: 18717.033656
|
[14:17:37.760] init error: 138.384450
|
||||||
[16:55:41.613] end error: 4.936020
|
[14:17:37.761] end error: 0.000041
|
||||||
[16:55:44.998] New key frame
|
[14:17:37.847] New key frame
|
||||||
[16:55:44.998] BA loop run
|
[14:17:37.847] BA loop run
|
||||||
[16:55:44.998] init error: 39.791183
|
[14:17:37.847] init error: 89.222508
|
||||||
[16:55:45.001] end error: 10.492954
|
[14:17:37.848] end error: 0.000252
|
||||||
[16:55:45.116] New key frame
|
[14:17:38.645] New key frame
|
||||||
[16:55:45.116] BA loop run
|
[14:17:38.645] BA loop run
|
||||||
[16:55:45.117] init error: 786.965335
|
[14:17:38.645] init error: 10.820945
|
||||||
[16:55:45.122] end error: 1.421966
|
[14:17:38.646] end error: 0.000143
|
||||||
[16:55:45.268] New key frame
|
[14:17:39.342] New key frame
|
||||||
[16:55:45.268] BA loop run
|
[14:17:39.342] BA loop run
|
||||||
[16:55:45.270] init error: 1782.595577
|
[14:17:39.343] init error: 8.056582
|
||||||
[16:55:45.277] end error: 2.222177
|
[14:17:39.343] end error: 0.000025
|
||||||
[16:55:45.468] New key frame
|
[14:17:39.768] New key frame
|
||||||
[16:55:45.469] BA loop run
|
[14:17:39.769] BA loop run
|
||||||
[16:55:45.473] init error: 3538.063991
|
[14:17:39.769] init error: 17.415217
|
||||||
[16:55:45.484] end error: 15.716388
|
[14:17:39.770] end error: 0.000767
|
||||||
[16:55:45.749] New key frame
|
[14:17:40.130] New key frame
|
||||||
[16:55:45.749] BA loop run
|
[14:17:40.130] BA loop run
|
||||||
[16:55:45.753] init error: 16354.561639
|
[14:17:40.130] init error: 32.854374
|
||||||
[16:55:45.761] end error: 8.216372
|
[14:17:40.131] end error: 0.000032
|
||||||
[16:55:46.186] New key frame
|
[14:17:40.925] New key frame
|
||||||
[16:55:46.186] BA loop run
|
[14:17:40.925] BA loop run
|
||||||
[16:55:46.206] init error: 414249.527414
|
[14:17:40.925] init error: 78.734056
|
||||||
[16:55:46.289] end error: 138.234439
|
[14:17:40.926] end error: 0.000000
|
||||||
[16:55:48.072] New key frame
|
[14:17:41.579] New key frame
|
||||||
[16:55:48.072] BA loop run
|
[14:17:41.579] BA loop run
|
||||||
[16:55:48.108] init error: 627388.776280
|
[14:17:41.579] init error: 60.909114
|
||||||
[16:55:48.264] end error: 1448.316701
|
[14:17:41.580] end error: 0.000000
|
||||||
[16:55:48.945] New key frame
|
[14:17:41.938] New key frame
|
||||||
[16:55:48.945] BA loop run
|
[14:17:41.938] BA loop run
|
||||||
[16:55:49.017] init error: 146955.415274
|
[14:17:41.939] init error: 46.031892
|
||||||
[16:55:49.918] end error: 941.453528
|
[14:17:41.940] end error: 0.000042
|
||||||
[16:56:07.244] New key frame
|
[14:17:42.150] New key frame
|
||||||
[16:56:07.244] BA loop run
|
[14:17:42.150] BA loop run
|
||||||
[16:56:07.330] init error: 125101697.443424
|
[14:17:42.150] init error: 1076.813205
|
||||||
[16:56:08.112] end error: 7023.248058
|
[14:17:42.152] end error: 0.000812
|
||||||
[16:56:29.389] New key frame
|
[14:17:42.345] New key frame
|
||||||
[16:56:29.389] BA loop run
|
[14:17:42.345] BA loop run
|
||||||
[16:56:29.529] init error: 258236615.290955
|
[14:17:42.346] init error: 754.379730
|
||||||
[16:56:31.054] end error: 24797.607025
|
[14:17:42.347] end error: 0.081436
|
||||||
[16:56:52.584] New key frame
|
[14:17:42.756] New key frame
|
||||||
[16:56:52.584] BA loop run
|
[14:17:42.756] BA loop run
|
||||||
[16:56:52.866] init error: 8329221.907695
|
[14:17:42.757] init error: 2677.765414
|
||||||
[16:57:03.183] end error: 3862.867416
|
[14:17:42.895] end error: 0.001519
|
||||||
|
[14:17:43.028] New key frame
|
||||||
|
[14:17:43.028] BA loop run
|
||||||
|
[14:17:43.029] init error: 146.947081
|
||||||
|
[14:17:43.145] end error: 0.002729
|
||||||
|
[14:17:43.437] New key frame
|
||||||
|
[14:17:43.437] BA loop run
|
||||||
|
[14:17:43.438] init error: 13982.056448
|
||||||
|
[14:17:43.553] end error: 0.004119
|
||||||
|
[14:17:43.771] New key frame
|
||||||
|
[14:17:43.771] BA loop run
|
||||||
|
[14:17:43.772] init error: 76.715863
|
||||||
|
[14:17:43.773] end error: 0.007292
|
||||||
|
[14:17:43.858] New key frame
|
||||||
|
[14:17:43.858] BA loop run
|
||||||
|
[14:17:43.858] init error: 215.511731
|
||||||
|
[14:17:43.860] end error: 0.000221
|
||||||
|
[14:17:43.938] New key frame
|
||||||
|
[14:17:43.938] BA loop run
|
||||||
|
[14:17:43.939] init error: 81.875323
|
||||||
|
[14:17:43.940] end error: 0.000025
|
||||||
|
[14:17:44.118] New key frame
|
||||||
|
[14:17:44.118] BA loop run
|
||||||
|
[14:17:44.118] init error: 2422.342876
|
||||||
|
[14:17:44.119] end error: 0.000097
|
||||||
|
[14:17:44.221] New key frame
|
||||||
|
[14:17:44.221] BA loop run
|
||||||
|
[14:17:44.221] init error: 207.581916
|
||||||
|
[14:17:44.223] end error: 0.000000
|
||||||
|
[14:17:44.328] New key frame
|
||||||
|
[14:17:44.328] BA loop run
|
||||||
|
[14:17:44.328] init error: 162.566432
|
||||||
|
[14:17:44.329] end error: 0.001409
|
||||||
|
[14:17:44.422] New key frame
|
||||||
|
[14:17:44.422] BA loop run
|
||||||
|
[14:17:44.423] init error: 14435.424627
|
||||||
|
[14:17:44.425] end error: 7.540367
|
||||||
|
[14:17:44.519] New key frame
|
||||||
|
[14:17:44.519] BA loop run
|
||||||
|
[14:17:44.520] init error: 133.788853
|
||||||
|
[14:17:44.524] end error: 1.809725
|
||||||
|
[14:17:44.638] New key frame
|
||||||
|
[14:17:44.638] BA loop run
|
||||||
|
[14:17:44.639] init error: 1324.756419
|
||||||
|
[14:17:44.641] end error: 0.000203
|
||||||
|
[14:17:44.720] New key frame
|
||||||
|
[14:17:44.720] BA loop run
|
||||||
|
[14:17:44.721] init error: 566.029171
|
||||||
|
[14:17:44.723] end error: 0.000364
|
||||||
|
[14:17:44.827] New key frame
|
||||||
|
[14:17:44.827] BA loop run
|
||||||
|
[14:17:44.828] init error: 13851.507144
|
||||||
|
[14:17:44.829] end error: 0.000267
|
||||||
|
[14:17:44.934] New key frame
|
||||||
|
[14:17:44.934] BA loop run
|
||||||
|
[14:17:44.935] init error: 187.782764
|
||||||
|
[14:17:44.937] end error: 0.000267
|
||||||
|
[14:17:45.035] New key frame
|
||||||
|
[14:17:45.035] BA loop run
|
||||||
|
[14:17:45.035] init error: 839.892009
|
||||||
|
[14:17:45.037] end error: 0.000267
|
||||||
|
[14:17:45.135] New key frame
|
||||||
|
[14:17:45.135] BA loop run
|
||||||
|
[14:17:45.136] init error: 82.259093
|
||||||
|
[14:17:45.138] end error: 0.004693
|
||||||
|
[14:17:45.222] New key frame
|
||||||
|
[14:17:45.222] BA loop run
|
||||||
|
[14:17:45.223] init error: 1073.580439
|
||||||
|
[14:17:45.224] end error: 0.001541
|
||||||
|
[14:17:45.328] New key frame
|
||||||
|
[14:17:45.328] BA loop run
|
||||||
|
[14:17:45.329] init error: 893.975632
|
||||||
|
[14:17:45.331] end error: 0.001541
|
||||||
|
[14:17:45.435] New key frame
|
||||||
|
[14:17:45.435] BA loop run
|
||||||
|
[14:17:45.436] init error: 162.307891
|
||||||
|
[14:17:45.439] end error: 0.001541
|
||||||
|
[14:17:45.537] New key frame
|
||||||
|
[14:17:45.538] BA loop run
|
||||||
|
[14:17:45.539] init error: 494869.499643
|
||||||
|
[14:17:45.541] end error: 0.502589
|
||||||
|
[14:17:45.636] New key frame
|
||||||
|
[14:17:45.636] BA loop run
|
||||||
|
[14:17:45.637] init error: 713.370566
|
||||||
|
[14:17:45.638] end error: 0.058017
|
||||||
|
[14:17:45.744] New key frame
|
||||||
|
[14:17:45.744] BA loop run
|
||||||
|
[14:17:45.745] init error: 749.167754
|
||||||
|
[14:17:45.749] end error: 0.779043
|
||||||
|
[14:17:45.846] New key frame
|
||||||
|
[14:17:45.846] BA loop run
|
||||||
|
[14:17:45.847] init error: 12412.756356
|
||||||
|
[14:17:45.850] end error: 1.941401
|
||||||
|
[14:17:45.938] New key frame
|
||||||
|
[14:17:45.938] BA loop run
|
||||||
|
[14:17:45.939] init error: 1009.645679
|
||||||
|
[14:17:45.943] end error: 1.298569
|
||||||
|
[14:17:46.130] New key frame
|
||||||
|
[14:17:46.130] BA loop run
|
||||||
|
[14:17:46.131] init error: 1805.845656
|
||||||
|
[14:17:46.136] end error: 52.687598
|
||||||
|
[14:17:46.225] New key frame
|
||||||
|
[14:17:46.225] BA loop run
|
||||||
|
[14:17:46.227] init error: 1384.023739
|
||||||
|
[14:17:46.231] end error: 3.293355
|
||||||
|
[14:17:46.332] New key frame
|
||||||
|
[14:17:46.332] BA loop run
|
||||||
|
[14:17:46.334] init error: 1916.816311
|
||||||
|
[14:17:46.340] end error: 15.678229
|
||||||
|
[14:17:46.422] New key frame
|
||||||
|
[14:17:46.422] BA loop run
|
||||||
|
[14:17:46.424] init error: 1364.949035
|
||||||
|
[14:17:46.430] end error: 11.994543
|
||||||
|
[14:17:46.542] New key frame
|
||||||
|
[14:17:46.542] BA loop run
|
||||||
|
[14:17:46.544] init error: 1803.705818
|
||||||
|
[14:17:46.549] end error: 13.248052
|
||||||
|
[14:17:46.735] New key frame
|
||||||
|
[14:17:46.735] BA loop run
|
||||||
|
[14:17:46.737] init error: 359.627244
|
||||||
|
[14:17:46.744] end error: 15.917097
|
||||||
|
[14:17:46.848] New key frame
|
||||||
|
[14:17:46.848] BA loop run
|
||||||
|
[14:17:46.850] init error: 132.767637
|
||||||
|
[14:17:46.859] end error: 7.496239
|
||||||
|
[14:17:46.942] New key frame
|
||||||
|
[14:17:46.942] BA loop run
|
||||||
|
[14:17:46.945] init error: 273.140713
|
||||||
|
[14:17:46.955] end error: 9.084497
|
||||||
|
[14:17:47.035] New key frame
|
||||||
|
[14:17:47.035] BA loop run
|
||||||
|
[14:17:47.038] init error: 44858.514613
|
||||||
|
[14:17:47.092] end error: 23.858194
|
||||||
|
[14:17:47.261] New key frame
|
||||||
|
[14:17:47.261] BA loop run
|
||||||
|
[14:17:47.265] init error: 19240.174352
|
||||||
|
[14:17:47.425] end error: 33.269102
|
||||||
|
[14:17:47.443] New key frame
|
||||||
|
[14:17:47.443] BA loop run
|
||||||
|
[14:17:47.451] init error: 9490.855291
|
||||||
|
[14:17:47.791] end error: 60.008907
|
||||||
|
[14:17:47.877] New key frame
|
||||||
|
[14:17:47.877] BA loop run
|
||||||
|
[14:17:47.885] init error: 250641.349869
|
||||||
|
[14:17:48.203] end error: 95.233779
|
||||||
|
[14:17:49.274] New key frame
|
||||||
|
[14:17:49.274] BA loop run
|
||||||
|
[14:17:49.278] init error: 9949.519439
|
||||||
|
[14:17:49.559] end error: 50.564651
|
||||||
|
[14:17:50.848] New key frame
|
||||||
|
[14:17:50.848] BA loop run
|
||||||
|
[14:17:50.852] init error: 5178.825342
|
||||||
|
[14:17:50.979] end error: 22.268369
|
||||||
|
[14:17:51.763] New key frame
|
||||||
|
[14:17:51.764] BA loop run
|
||||||
|
[14:17:51.767] init error: 543.547794
|
||||||
|
[14:17:52.061] end error: 17.817167
|
||||||
|
[14:17:52.065] New key frame
|
||||||
|
[14:17:52.066] BA loop run
|
||||||
|
[14:17:52.077] init error: 1339.153605
|
||||||
|
[14:17:52.261] end error: 20.246683
|
||||||
|
[14:17:56.550] New key frame
|
||||||
|
[14:17:56.550] BA loop run
|
||||||
|
[14:17:56.554] init error: 8303.291564
|
||||||
|
[14:17:56.629] end error: 26.594103
|
||||||
|
[14:17:57.059] New key frame
|
||||||
|
[14:17:57.059] BA loop run
|
||||||
|
[14:17:57.063] init error: 91935.690165
|
||||||
|
[14:17:57.271] end error: 38.526509
|
||||||
|
[14:17:57.852] New key frame
|
||||||
|
[14:17:57.852] BA loop run
|
||||||
|
[14:17:57.858] init error: 232131.159180
|
||||||
|
[14:17:57.996] end error: 61.418385
|
||||||
|
[14:17:58.081] New key frame
|
||||||
|
[14:17:58.081] BA loop run
|
||||||
|
[14:17:58.089] init error: 9070.775705
|
||||||
|
[14:17:58.447] end error: 34.291902
|
||||||
|
[14:17:58.494] New key frame
|
||||||
|
[14:17:58.494] BA loop run
|
||||||
|
[14:17:58.500] init error: 68366.090407
|
||||||
|
[14:17:58.991] end error: 113.614379
|
||||||
|
[14:17:59.249] New key frame
|
||||||
|
[14:17:59.249] BA loop run
|
||||||
|
[14:17:59.253] init error: 1635180.603207
|
||||||
|
[14:17:59.360] end error: 198.804844
|
||||||
|
[14:18:00.085] New key frame
|
||||||
|
[14:18:00.085] BA loop run
|
||||||
|
[14:18:00.090] init error: 131349.804393
|
||||||
|
[14:18:00.273] end error: 1609.872889
|
||||||
|
[14:18:00.682] New key frame
|
||||||
|
[14:18:00.682] BA loop run
|
||||||
|
[14:18:00.687] init error: 12215847.631416
|
||||||
|
[14:18:00.909] end error: 523.178192
|
||||||
|
[14:18:01.335] New key frame
|
||||||
|
[14:18:01.335] BA loop run
|
||||||
|
[14:18:01.341] init error: 1425.392849
|
||||||
|
[14:18:01.786] end error: 104.404989
|
||||||
|
[14:18:01.795] New key frame
|
||||||
|
[14:18:01.795] BA loop run
|
||||||
|
[14:18:01.803] init error: 65647.241336
|
||||||
|
[14:18:02.125] end error: 85.889376
|
||||||
|
[14:18:02.141] New key frame
|
||||||
|
[14:18:02.141] BA loop run
|
||||||
|
[14:18:02.149] init error: 556607.881609
|
||||||
|
[14:18:02.371] end error: 312.798019
|
||||||
|
[14:18:02.682] New key frame
|
||||||
|
[14:18:02.682] BA loop run
|
||||||
|
[14:18:02.688] init error: 1285.331214
|
||||||
|
[14:18:02.907] end error: 54.310029
|
||||||
|
[14:18:02.923] New key frame
|
||||||
|
[14:18:02.923] BA loop run
|
||||||
|
[14:18:02.932] init error: 63079.696761
|
||||||
|
[14:18:03.756] end error: 27.679050
|
||||||
|
|||||||
+1172
-627
File diff suppressed because it is too large
Load Diff
@@ -66,6 +66,7 @@ public:
|
|||||||
vio::time_in_HH_MM_SS_MMM().c_str());
|
vio::time_in_HH_MM_SS_MMM().c_str());
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
boost::mutex ba_mux_;
|
||||||
protected:
|
protected:
|
||||||
bool new_keyframe_=false;
|
bool new_keyframe_=false;
|
||||||
boost::condition_variable cond_;
|
boost::condition_variable cond_;
|
||||||
|
|||||||
@@ -111,6 +111,17 @@ protected:
|
|||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
void debug(FramePtr ref,FramePtr cur){
|
||||||
|
cv::Mat reference,current;
|
||||||
|
cv::cvtColor(ref->img(),reference,cv::COLOR_GRAY2RGB);
|
||||||
|
cv::cvtColor(cur->img(),current,cv::COLOR_GRAY2RGB);
|
||||||
|
for(auto&& f:ref->fts_)cv::circle(reference, cv::Point(f->px.x(),f->px.y() ), 2, cv::Scalar(0, 255,0),2);
|
||||||
|
for(auto&& f:cur->fts_)cv::circle(current, cv::Point(f->px.x(),f->px.y() ), 2, cv::Scalar(0, 255,0),2);
|
||||||
|
cv::imshow("Reference",reference);
|
||||||
|
cv::imshow("Current",current);
|
||||||
|
cv::waitKey();
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
/// Detect Fast corners in the image.
|
/// Detect Fast corners in the image.
|
||||||
|
|||||||
@@ -57,13 +57,15 @@ public:
|
|||||||
void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const;
|
void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const;
|
||||||
|
|
||||||
/// Return the keyframe which is furthest apart from pos.
|
/// Return the keyframe which is furthest apart from pos.
|
||||||
FramePtr getFurthestKeyframe(const Vector2d& pos) const;
|
FramePtr getFurthestKeyframe(const Vector2d& pos);
|
||||||
|
|
||||||
/// Empty trash bin of deleted keyframes and map points. We don't delete the
|
/// Empty trash bin of deleted keyframes and map points. We don't delete the
|
||||||
/// points immediately to ensure proper cleanup and to provide the visualizer
|
/// points immediately to ensure proper cleanup and to provide the visualizer
|
||||||
/// a list of objects which must be removed.
|
/// a list of objects which must be removed.
|
||||||
void emptyTrash();
|
void emptyTrash();
|
||||||
|
|
||||||
|
bool checkKeyFrames();
|
||||||
|
|
||||||
|
|
||||||
/// Return the number of keyframes in the map
|
/// Return the number of keyframes in the map
|
||||||
inline size_t size() const { return keyframes_.size(); }
|
inline size_t size() const { return keyframes_.size(); }
|
||||||
|
|||||||
@@ -23,7 +23,7 @@ public:
|
|||||||
virtual ~Base(){
|
virtual ~Base(){
|
||||||
};
|
};
|
||||||
void predict(double dx,double dy,double dpitch,double time){
|
void predict(double dx,double dy,double dpitch,double time){
|
||||||
double dt=0.1;
|
double dt=0.2;
|
||||||
/*if(t_cmd==0.0){
|
/*if(t_cmd==0.0){
|
||||||
t_cmd=time;
|
t_cmd=time;
|
||||||
return;
|
return;
|
||||||
@@ -46,7 +46,7 @@ public:
|
|||||||
cov_h_=G*cov_*G.transpose()+R;
|
cov_h_=G*cov_*G.transpose()+R;
|
||||||
}
|
}
|
||||||
void correct(double ddx, double ddy, double dpitch, double time){
|
void correct(double ddx, double ddy, double dpitch, double time){
|
||||||
double dt=0.005;
|
double dt=0.01;
|
||||||
/*if(t_imu==0.0){
|
/*if(t_imu==0.0){
|
||||||
t_imu=time;
|
t_imu=time;
|
||||||
return;
|
return;
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
<launch>
|
<launch>
|
||||||
|
|
||||||
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
|
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
|
||||||
|
|
||||||
<!-- Camera topic to subscribe to -->
|
<!-- Camera topic to subscribe to -->
|
||||||
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
|
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
|
||||||
|
|||||||
@@ -111,7 +111,7 @@ void FastDetector::detect(
|
|||||||
ROS_ERROR("GPU Driver crash try again!");
|
ROS_ERROR("GPU Driver crash try again!");
|
||||||
assert(0);
|
assert(0);
|
||||||
}
|
}
|
||||||
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4);
|
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
|
||||||
cv::Mat descriptor;
|
cv::Mat descriptor;
|
||||||
extractor->compute(frame->img(), keypoints, descriptor);
|
extractor->compute(frame->img(), keypoints, descriptor);
|
||||||
for(auto&& p:_for(keypoints))fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64)));
|
for(auto&& p:_for(keypoints))fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64)));
|
||||||
|
|||||||
@@ -205,6 +205,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
|
|||||||
new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(),
|
new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(),
|
||||||
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
|
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
|
||||||
#endif
|
#endif
|
||||||
|
boost::unique_lock< boost::mutex > lock(ba_glob_->ba_mux_);
|
||||||
size_t sfba_n_edges_final=0;
|
size_t sfba_n_edges_final=0;
|
||||||
double sfba_thresh, sfba_error_init, sfba_error_final;
|
double sfba_thresh, sfba_error_init, sfba_error_final;
|
||||||
pose_optimizer::optimizeGaussNewton(
|
pose_optimizer::optimizeGaussNewton(
|
||||||
@@ -256,6 +257,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
|
|||||||
}
|
}
|
||||||
// add keyframe to map
|
// add keyframe to map
|
||||||
map_.addKeyframe(new_frame_);
|
map_.addKeyframe(new_frame_);
|
||||||
|
map_.checkKeyFrames();
|
||||||
ba_glob_->new_key_frame();
|
ba_glob_->new_key_frame();
|
||||||
return RESULT_IS_KEYFRAME;
|
return RESULT_IS_KEYFRAME;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -96,6 +96,7 @@ namespace vio {
|
|||||||
// init g2o
|
// init g2o
|
||||||
list<EdgeContainerSE3> edges;
|
list<EdgeContainerSE3> edges;
|
||||||
list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
|
list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
|
||||||
|
ba_mux_.lock();
|
||||||
// Go through all Keyframes
|
// Go through all Keyframes
|
||||||
size_t v_id = 0;
|
size_t v_id = 0;
|
||||||
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
|
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
|
||||||
@@ -107,10 +108,9 @@ namespace vio {
|
|||||||
optimizer.addVertex(v_kf);
|
optimizer.addVertex(v_kf);
|
||||||
for(auto&& it_ftr:(*it_kf)->fts_)
|
for(auto&& it_ftr:(*it_kf)->fts_)
|
||||||
{
|
{
|
||||||
|
if(it_ftr->point==NULL)continue;
|
||||||
// for each keyframe add edges to all observed mapoints
|
// for each keyframe add edges to all observed mapoints
|
||||||
std::shared_ptr<Point> mp = it_ftr->point;
|
std::shared_ptr<Point> mp = it_ftr->point;
|
||||||
if(mp == NULL)
|
|
||||||
continue;
|
|
||||||
g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
|
g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
|
||||||
if(v_mp == NULL)
|
if(v_mp == NULL)
|
||||||
{
|
{
|
||||||
@@ -139,10 +139,7 @@ namespace vio {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Optimization
|
// Optimization
|
||||||
|
|
||||||
point_mut_.lock();
|
|
||||||
optimizer.initializeOptimization();
|
optimizer.initializeOptimization();
|
||||||
optimizer.computeActiveErrors();
|
optimizer.computeActiveErrors();
|
||||||
#if VIO_DEBUG
|
#if VIO_DEBUG
|
||||||
@@ -158,21 +155,20 @@ namespace vio {
|
|||||||
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
|
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
|
||||||
it_kf != map_.keyframes_.end();++it_kf)
|
it_kf != map_.keyframes_.end();++it_kf)
|
||||||
{
|
{
|
||||||
(*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(),
|
(*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation(),
|
||||||
(*it_kf)->v_kf_->estimate().translation());
|
(*it_kf)->v_kf_->estimate().translation()));
|
||||||
(*it_kf)->v_kf_ = NULL;
|
(*it_kf)->v_kf_ = NULL;
|
||||||
for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
|
for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
|
||||||
{
|
{
|
||||||
std::shared_ptr<Point> mp = (*it_ftr)->point;
|
if((*it_ftr)->point == NULL)
|
||||||
if(mp == NULL)
|
|
||||||
continue;
|
continue;
|
||||||
|
std::shared_ptr<Point> mp = (*it_ftr)->point;
|
||||||
if(mp->v_pt_ == NULL)
|
if(mp->v_pt_ == NULL)
|
||||||
continue; // mp was updated before
|
continue; // mp was updated before
|
||||||
mp->pos_ = mp->v_pt_->estimate();
|
mp->pos_ = mp->v_pt_->estimate();
|
||||||
mp->v_pt_ = NULL;
|
mp->v_pt_ = NULL;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
point_mut_.unlock();
|
|
||||||
|
|
||||||
// Remove Measurements with too large reprojection error
|
// Remove Measurements with too large reprojection error
|
||||||
for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
|
for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
|
||||||
@@ -186,6 +182,7 @@ namespace vio {
|
|||||||
map_.removePtFrameRef(it->frame, it->feature);
|
map_.removePtFrameRef(it->frame, it->feature);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
ba_mux_.unlock();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -111,6 +111,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
|
|||||||
frame_ref_->addFeature(f.item);
|
frame_ref_->addFeature(f.item);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
//debug(frame_ref_,frame_cur);
|
||||||
#if VIO_DEBUG
|
#if VIO_DEBUG
|
||||||
fprintf(log_,"[%s] Init finished: Homography RANSAC (inlier) is:%d ,While %d inliers minimum required. px average disparity is:%f ,While minimum is: %f KLT tracked: %d\n",
|
fprintf(log_,"[%s] Init finished: Homography RANSAC (inlier) is:%d ,While %d inliers minimum required. px average disparity is:%f ,While minimum is: %f KLT tracked: %d\n",
|
||||||
vio::time_in_HH_MM_SS_MMM().c_str(),
|
vio::time_in_HH_MM_SS_MMM().c_str(),
|
||||||
|
|||||||
@@ -39,6 +39,7 @@ void Map::reset()
|
|||||||
|
|
||||||
bool Map::safeDeleteFrame(FramePtr frame)
|
bool Map::safeDeleteFrame(FramePtr frame)
|
||||||
{
|
{
|
||||||
|
//boost::unique_lock<boost::mutex> lock(point_mut_);
|
||||||
bool found = false;
|
bool found = false;
|
||||||
size_t position;
|
size_t position;
|
||||||
for(auto&& keyframe:_for(keyframes_)){
|
for(auto&& keyframe:_for(keyframes_)){
|
||||||
@@ -62,7 +63,6 @@ bool Map::safeDeleteFrame(FramePtr frame)
|
|||||||
|
|
||||||
void Map::removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr)
|
void Map::removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr)
|
||||||
{
|
{
|
||||||
boost::unique_lock<boost::mutex> lock(point_mut_);
|
|
||||||
if(ftr->point == NULL)
|
if(ftr->point == NULL)
|
||||||
return; // mappoint may have been deleted in a previous ref. removal
|
return; // mappoint may have been deleted in a previous ref. removal
|
||||||
if(ftr->point->obs_.size() <= 2)
|
if(ftr->point->obs_.size() <= 2)
|
||||||
@@ -77,6 +77,7 @@ void Map::removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr)
|
|||||||
|
|
||||||
void Map::safeDeletePoint(std::shared_ptr<Point> pt)
|
void Map::safeDeletePoint(std::shared_ptr<Point> pt)
|
||||||
{
|
{
|
||||||
|
boost::unique_lock<boost::mutex> lock(point_mut_);
|
||||||
// Delete references to mappoints in all keyframes
|
// Delete references to mappoints in all keyframes
|
||||||
if(pt->obs_.size()>1){
|
if(pt->obs_.size()>1){
|
||||||
for(auto&& ftr:pt->obs_){
|
for(auto&& ftr:pt->obs_){
|
||||||
@@ -126,17 +127,21 @@ void Map::getCloseKeyframes(
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const
|
FramePtr Map::getFurthestKeyframe(const Vector2d& pos)
|
||||||
{
|
{
|
||||||
FramePtr furthest_kf;
|
FramePtr furthest_kf;
|
||||||
double maxdist = 0.0;
|
double maxdist = 0.0;
|
||||||
for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
|
for(auto it=keyframes_.begin(); it!=keyframes_.end(); )
|
||||||
{
|
{
|
||||||
|
if((*it)->T_f_w_.empty()){
|
||||||
|
it=keyframes_.erase(it);
|
||||||
|
}
|
||||||
double dist = ((*it)->pos()-pos).norm();
|
double dist = ((*it)->pos()-pos).norm();
|
||||||
if(dist > maxdist) {
|
if(dist > maxdist) {
|
||||||
maxdist = dist;
|
maxdist = dist;
|
||||||
furthest_kf = *it;
|
furthest_kf = *it;
|
||||||
}
|
}
|
||||||
|
++it;
|
||||||
}
|
}
|
||||||
return furthest_kf;
|
return furthest_kf;
|
||||||
}
|
}
|
||||||
@@ -148,6 +153,17 @@ void Map::emptyTrash()
|
|||||||
for(auto&& t:trash_points_)t.reset();
|
for(auto&& t:trash_points_)t.reset();
|
||||||
trash_points_.clear();
|
trash_points_.clear();
|
||||||
}
|
}
|
||||||
|
bool Map::checkKeyFrames() {
|
||||||
|
for(auto it=keyframes_.begin();it!=keyframes_.end();){
|
||||||
|
if((*it)->T_f_w_.empty()){
|
||||||
|
for(auto&& fts:(*it)->fts_)removePtFrameRef(*it, fts);
|
||||||
|
it=keyframes_.erase(it);
|
||||||
|
}
|
||||||
|
++it;
|
||||||
|
}
|
||||||
|
if(keyframes_.size()>0)return true;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
namespace map_debug {
|
namespace map_debug {
|
||||||
|
|||||||
@@ -59,7 +59,6 @@ Point::~Point()
|
|||||||
|
|
||||||
void Point::addFrameRef(std::shared_ptr<Feature> ftr)
|
void Point::addFrameRef(std::shared_ptr<Feature> ftr)
|
||||||
{
|
{
|
||||||
boost::unique_lock<boost::mutex> lock(point_mut_);
|
|
||||||
obs_.push_front(ftr);
|
obs_.push_front(ftr);
|
||||||
++n_obs_;
|
++n_obs_;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -56,10 +56,10 @@ void optimizeGaussNewton(
|
|||||||
- vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
|
- vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
|
||||||
e *= 1.0 / (1<<(*it)->level);
|
e *= 1.0 / (1<<(*it)->level);
|
||||||
//std::cerr<<"pose optimizer - error : "<<e.norm()<<std::endl;
|
//std::cerr<<"pose optimizer - error : "<<e.norm()<<std::endl;
|
||||||
if(e.norm() > 0.01){
|
/* if(e.norm() > 0.01){
|
||||||
it = frame->fts_.erase(it);
|
it = frame->fts_.erase(it);
|
||||||
continue;
|
continue;
|
||||||
}
|
}*/
|
||||||
chi2_vec_init.push_back(e.norm()); // just for debug
|
chi2_vec_init.push_back(e.norm()); // just for debug
|
||||||
errors.push_back(e.norm());
|
errors.push_back(e.norm());
|
||||||
it++;
|
it++;
|
||||||
@@ -122,7 +122,7 @@ void optimizeGaussNewton(
|
|||||||
if(vk::norm_max(dT) <= EPS)
|
if(vk::norm_max(dT) <= EPS)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
point_mut_.lock();
|
||||||
size_t n_deleted_refs = 0;
|
size_t n_deleted_refs = 0;
|
||||||
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
|
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
|
||||||
{
|
{
|
||||||
@@ -142,6 +142,7 @@ void optimizeGaussNewton(
|
|||||||
else
|
else
|
||||||
it++;
|
it++;
|
||||||
}
|
}
|
||||||
|
point_mut_.unlock();
|
||||||
num_obs -= n_deleted_refs;
|
num_obs -= n_deleted_refs;
|
||||||
#if VIO_DEBUG
|
#if VIO_DEBUG
|
||||||
error_init=0.0;
|
error_init=0.0;
|
||||||
|
|||||||
@@ -125,19 +125,23 @@ namespace vio {
|
|||||||
else{
|
else{
|
||||||
matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskdown));
|
matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskdown));
|
||||||
}
|
}
|
||||||
|
/*cv::Mat imgMatch,key_point_ref,key_point_cur;
|
||||||
|
cv::drawMatches( it_frame.item.first->img_pyr_[0], keypoints_kfs, frame->img_pyr_[0], keypoints_cur,matches, imgMatch);
|
||||||
|
cv::imshow("img",imgMatch);
|
||||||
|
cv::waitKey();*/
|
||||||
for(auto&& match:matches.back()){
|
for(auto&& match:matches.back()){
|
||||||
it_cur=keypoints.begin();
|
it_cur=keypoints.begin();
|
||||||
std::advance(it_cur,match.trainIdx);
|
std::advance(it_cur,match.trainIdx);
|
||||||
if(!(*it_cur))continue;
|
if(!(*it_cur))continue;
|
||||||
if ((*it_ref)->point == NULL){
|
|
||||||
const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) *
|
const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) *
|
||||||
grid_.grid_n_cols
|
grid_.grid_n_cols
|
||||||
+ static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
|
+ static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
|
||||||
if(k>grid_.cells.size()-1)continue;
|
if(k>grid_.cells.size()-1)continue;
|
||||||
assert(grid_.cells.at(k) != nullptr);
|
assert(grid_.cells.at(k) != nullptr);
|
||||||
|
if(grid_.cells.at(k)->size()> Config::gridSize()-1)continue;
|
||||||
Vector2d px((int) (*it_cur)->px.x(),
|
Vector2d px((int) (*it_cur)->px.x(),
|
||||||
(int) (*it_cur)->px.y());
|
(int) (*it_cur)->px.y());
|
||||||
|
if ((*it_ref)->point == NULL){
|
||||||
SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3();
|
SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3();
|
||||||
// pose with respect to reference frame
|
// pose with respect to reference frame
|
||||||
Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
|
Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
|
||||||
@@ -155,13 +159,6 @@ namespace vio {
|
|||||||
overlap_kfs.back().second++;
|
overlap_kfs.back().second++;
|
||||||
}else{
|
}else{
|
||||||
(*it_ref)->point->last_frame_overlap_id_ = frame->id_;
|
(*it_ref)->point->last_frame_overlap_id_ = frame->id_;
|
||||||
const int k = static_cast<int>((*it_cur)->px.y()/ grid_.cell_size) *
|
|
||||||
grid_.grid_n_cols
|
|
||||||
+ static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
|
|
||||||
if(k>grid_.cells.size()-1)continue;
|
|
||||||
assert(grid_.cells.at(k) != nullptr);
|
|
||||||
Vector2d px((int) (*it_cur)->px.x(),
|
|
||||||
(int) (*it_cur)->px.y());
|
|
||||||
frame->addFeature(std::make_shared<Feature>(frame,
|
frame->addFeature(std::make_shared<Feature>(frame,
|
||||||
(*it_ref)->point,
|
(*it_ref)->point,
|
||||||
px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
|
px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
|
||||||
|
|||||||
@@ -116,13 +116,7 @@ VioNode::~VioNode()
|
|||||||
|
|
||||||
void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
|
void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
|
||||||
{
|
{
|
||||||
/* ++cam_syn_;
|
|
||||||
if(vo_->ukfPtr_.cam_syn_[cam_syn_ % 3]){
|
|
||||||
vo_->ukfPtr_.cam_syn_[cam_syn_ % 3]=false;
|
|
||||||
}else{
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if(cam_syn_>50)cam_syn_=0;*/
|
|
||||||
if(!start_)return;
|
if(!start_)return;
|
||||||
try {
|
try {
|
||||||
cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
|
cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
|
||||||
@@ -150,9 +144,12 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
|
|||||||
void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
|
void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
|
||||||
if(!start_)return;
|
if(!start_)return;
|
||||||
double imu_in[3];
|
double imu_in[3];
|
||||||
imu_in[0] = imu->linear_acceleration.x;
|
imu_in[0] = 0.2*imu_[0]+0.8*imu->linear_acceleration.x;
|
||||||
imu_in[1] = imu->linear_acceleration.y;
|
imu_in[1] = 0.2*imu_[1]+0.8*imu->linear_acceleration.y;
|
||||||
imu_in[2] = imu->angular_velocity.z;
|
imu_in[2] = 0.2*imu_[2]+0.8*imu->angular_velocity.z;
|
||||||
|
if(fabs(imu_in[2])>0.4)return;
|
||||||
|
if(fabs(imu_in[1])>1.0)return;
|
||||||
|
if(fabs(imu_in[0])>1.0)return;
|
||||||
memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
|
memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
|
||||||
vo_->UpdateIMU(imu_in,imu->header.stamp);
|
vo_->UpdateIMU(imu_in,imu->header.stamp);
|
||||||
imu_time_=imu->header.stamp;
|
imu_time_=imu->header.stamp;
|
||||||
|
|||||||
Reference in New Issue
Block a user