目录
1. 创建工作空间vins_ws，并在src目录下git源码并编译
3. (将相机与板载计算机固定在无人机上之后)抱起无人机在场地内缓慢走动，在vins的输出文件夹下打开extrinsic_parameter.txt
首先，安装相关环境依赖ceres solver、eigen以及ros-noetic版本(教程很多，此处省略)。
注：ubuntu 20.04会自带一个opencv 4.2版本。
一、下载编译Vins-fusion
1. 创建工作空间vins_ws，并在src目录下git源码并编译：
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ..
catkin_make
此时，编译会出现一系列错误：
make[2]: *** [VINS-Fusion/camera_models/CMakeFiles/Calibrations.dir/build.make:128：VINS-Fusion/camera_models/CMakeFiles/Calibrations.dir/src/camera_models/CostFunctionFactory.cc.o] 错误 1
make[1]: *** [CMakeFiles/Makefile2:843：VINS-Fusion/camera_models/CMakeFiles/Calibrations.dir/all] 错误 2
In file included from /usr/local/include/ceres/autodiff_cost_function.h:130,
from /usr/local/include/ceres/ceres.h:37,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.h:20,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.cpp:12:
/usr/local/include/ceres/internal/autodiff.h: In instantiation of ‘bool ceres::internal::AutoDifferentiate(const Functor&, const T* const*, int, T*, T**) [with int kNumResiduals = 6; ParameterDims = ceres::internal::ParameterDims<false, 4, 3, 4, 3>; Functor = RelativeRTError; T = double]’:
/usr/local/include/ceres/autodiff_cost_function.h:194:69: required from ‘bool ceres::AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns>::Evaluate(const double* const*, double*, double**) const [with CostFunctor = RelativeRTError; int kNumResiduals = 6; int ...Ns = {4, 3, 4, 3}]’
/usr/local/include/ceres/autodiff_cost_function.h:184:8: required from here
/usr/local/include/ceres/internal/autodiff.h:313:9: error: no type named ‘Parameters’ in ‘class ceres::internal::ParameterDims<false, 4, 3, 4, 3>’
313 | using Parameters = typename ParameterDims::Parameters;
| ^~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h:346:47: error: no type named ‘Parameters’ in ‘class ceres::internal::ParameterDims<false, 4, 3, 4, 3>’
346 | Make1stOrderPerturbations<Parameters>::Apply(parameters,
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~
347 | parameters_as_jets.data());
| ~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h:355:39: error: no type named ‘Parameters’ in ‘class ceres::internal::ParameterDims<false, 4, 3, 4, 3>’
355 | Take1stOrderParts<Parameters>::Apply(
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
356 | num_outputs, residuals_as_jets.data(), jacobians);
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h: In instantiation of ‘bool ceres::internal::AutoDifferentiate(const Functor&, const T* const*, int, T*, T**) [with int kNumResiduals = 3; ParameterDims = ceres::internal::ParameterDims<false, 3>; Functor = TError; T = double]’:
/usr/local/include/ceres/autodiff_cost_function.h:194:69: required from ‘bool ceres::AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns>::Evaluate(const double* const*, double*, double**) const [with CostFunctor = TError; int kNumResiduals = 3; int ...Ns = {3}]’
/usr/local/include/ceres/autodiff_cost_function.h:184:8: required from here
/usr/local/include/ceres/internal/autodiff.h:313:9: error: no type named ‘Parameters’ in ‘class ceres::internal::ParameterDims<false, 3>’
313 | using Parameters = typename ParameterDims::Parameters;
| ^~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h:346:47: error: no type named ‘Parameters’ in ‘class ceres::internal::ParameterDims<false, 3>’
346 | Make1stOrderPerturbations<Parameters>::Apply(parameters,
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~
347 | parameters_as_jets.data());
| ~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h:355:39: error: no type named ‘Parameters’ in ‘class ceres::internal::ParameterDims<false, 3>’
355 | Take1stOrderParts<Parameters>::Apply(
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
356 | num_outputs, residuals_as_jets.data(), jacobians);
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /usr/local/include/ceres/internal/autodiff.h:152,
from /usr/local/include/ceres/autodiff_cost_function.h:130,
from /usr/local/include/ceres/ceres.h:37,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.h:20,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.cpp:12:
/usr/local/include/ceres/internal/variadic_evaluate.h: In instantiation of ‘bool ceres::internal::VariadicEvaluateImpl(const Functor&, const T* const*, T*, const void*) [with ParameterDims = ceres::internal::ParameterDims<false, 4, 3, 4, 3>; Functor = RelativeRTError; T = double]’:
/usr/local/include/ceres/internal/variadic_evaluate.h:100:45: required from ‘bool ceres::internal::VariadicEvaluate(const Functor&, const T* const*, T*) [with ParameterDims = ceres::internal::ParameterDims<false, 4, 3, 4, 3>; Functor = RelativeRTError; T = double]’
/usr/local/include/ceres/autodiff_cost_function.h:191:55: required from ‘bool ceres::AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns>::Evaluate(const double* const*, double*, double**) const [with CostFunctor = RelativeRTError; int kNumResiduals = 6; int ...Ns = {4, 3, 4, 3}]’
/usr/local/include/ceres/autodiff_cost_function.h:184:8: required from here
/usr/local/include/ceres/internal/variadic_evaluate.h:74:52: error: ‘ParameterBlockIndices’ was not declared in this scope
74 | ParameterBlockIndices());
| ~~~~~~~~~~~~~~~~~~~~~^~
/usr/local/include/ceres/internal/variadic_evaluate.h: In instantiation of ‘bool ceres::internal::VariadicEvaluateImpl(const Functor&, const T* const*, T*, const void*) [with ParameterDims = ceres::internal::ParameterDims<false, 3>; Functor = TError; T = double]’:
/usr/local/include/ceres/internal/variadic_evaluate.h:100:45: required from ‘bool ceres::internal::VariadicEvaluate(const Functor&, const T* const*, T*) [with ParameterDims = ceres::internal::ParameterDims<false, 3>; Functor = TError; T = double]’
/usr/local/include/ceres/autodiff_cost_function.h:191:55: required from ‘bool ceres::AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns>::Evaluate(const double* const*, double*, double**) const [with CostFunctor = TError; int kNumResiduals = 3; int ...Ns = {3}]’
/usr/local/include/ceres/autodiff_cost_function.h:184:8: required from here
/usr/local/include/ceres/internal/variadic_evaluate.h:74:52: error: ‘ParameterBlockIndices’ was not declared in this scope
make[2]: *** [VINS-Fusion/global_fusion/CMakeFiles/global_fusion_node.dir/build.make:76：VINS-Fusion/global_fusion/CMakeFiles/global_fusion_node.dir/src/globalOpt.cpp.o] 错误 1
make[2]: *** 正在等待未完成的任务....
In file included from /usr/local/include/ceres/internal/parameter_dims.h:37,
from /usr/local/include/ceres/internal/autodiff.h:151,
from /usr/local/include/ceres/autodiff_cost_function.h:130,
from /usr/local/include/ceres/ceres.h:37,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.h:20,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOptNode.cpp:13:
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:64:21: error: ‘integer_sequence’ is not a member of ‘std’
64 | struct SumImpl<std::integer_sequence<T, N, Ns...>> {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:64:21: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:64:46: error: wrong number of template arguments (3, should be 1)
64 | struct SumImpl<std::integer_sequence<T, N, Ns...>> {
| ^~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:60:8: note: provided for ‘template<class Seq> struct ceres::internal::SumImpl’
60 | struct SumImpl;
| ^~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:64:49: error: expected unqualified-id before ‘>’ token
64 | struct SumImpl<std::integer_sequence<T, N, Ns...>> {
| ^~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:70:21: error: ‘integer_sequence’ is not a member of ‘std’
70 | struct SumImpl<std::integer_sequence<T, N1, N2, Ns...>> {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:70:21: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:70:51: error: wrong number of template arguments (4, should be 1)
70 | struct SumImpl<std::integer_sequence<T, N1, N2, Ns...>> {
| ^~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:60:8: note: provided for ‘template<class Seq> struct ceres::internal::SumImpl’
60 | struct SumImpl;
| ^~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:70:54: error: expected unqualified-id before ‘>’ token
70 | struct SumImpl<std::integer_sequence<T, N1, N2, Ns...>> {
| ^~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:77:21: error: ‘integer_sequence’ is not a member of ‘std’
77 | struct SumImpl<std::integer_sequence<T, N1, N2, N3, N4, Ns...>> {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:77:21: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:77:59: error: wrong number of template arguments (6, should be 1)
77 | struct SumImpl<std::integer_sequence<T, N1, N2, N3, N4, Ns...>> {
| ^~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:60:8: note: provided for ‘template<class Seq> struct ceres::internal::SumImpl’
60 | struct SumImpl;
| ^~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:77:62: error: expected unqualified-id before ‘>’ token
77 | struct SumImpl<std::integer_sequence<T, N1, N2, N3, N4, Ns...>> {
| ^~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:84:21: error: ‘integer_sequence’ is not a member of ‘std’
84 | struct SumImpl<std::integer_sequence<T, N>> {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:84:21: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:84:41: error: wrong number of template arguments (2, should be 1)
84 | struct SumImpl<std::integer_sequence<T, N>> {
| ^
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:60:8: note: provided for ‘template<class Seq> struct ceres::internal::SumImpl’
60 | struct SumImpl;
| ^~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:84:42: error: expected unqualified-id before ‘>’ token
84 | struct SumImpl<std::integer_sequence<T, N>> {
| ^~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:90:21: error: ‘integer_sequence’ is not a member of ‘std’
90 | struct SumImpl<std::integer_sequence<T>> {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:90:21: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:90:38: error: template argument 1 is invalid
90 | struct SumImpl<std::integer_sequence<T>> {
| ^
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:90:39: error: expected unqualified-id before ‘>’ token
90 | struct SumImpl<std::integer_sequence<T>> {
| ^~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:132:39: error: ‘integer_sequence’ is not a member of ‘std’
132 | struct ExclusiveScanImpl<T, Sum, std::integer_sequence<T, N, Ns...>,
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:132:39: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:132:67: error: template argument 3 is invalid
132 | struct ExclusiveScanImpl<T, Sum, std::integer_sequence<T, N, Ns...>,
| ^
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:132:67: error: type/value mismatch at argument 4 in template parameter list for ‘template<class T, T Sum, class SeqIn, class SeqOut> struct ceres::internal::ExclusiveScanImpl’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:132:67: note: expected a type, got ‘N’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:141:39: error: ‘integer_sequence’ is not a member of ‘std’
141 | struct ExclusiveScanImpl<T, Sum, std::integer_sequence<T>, SeqOut> {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:141:39: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:141:57: error: wrong number of template arguments (3, should be 4)
141 | struct ExclusiveScanImpl<T, Sum, std::integer_sequence<T>, SeqOut> {
| ^
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:129:8: note: provided for ‘template<class T, T Sum, class SeqIn, class SeqOut> struct ceres::internal::ExclusiveScanImpl’
129 | struct ExclusiveScanImpl;
| ^~~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:155:53: error: ‘integer_sequence’ is not a member of ‘std’
155 | typename ExclusiveScanImpl<T, T(0), Seq, std::integer_sequence<T>>::Type;
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:155:53: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:155:70: error: template argument 4 is invalid
155 | typename ExclusiveScanImpl<T, T(0), Seq, std::integer_sequence<T>>::Type;
| ^
/usr/local/include/ceres/internal/integer_sequence_algorithm.h:155:16: error: expected nested-name-specifier
155 | typename ExclusiveScanImpl<T, T(0), Seq, std::integer_sequence<T>>::Type;
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /usr/local/include/ceres/internal/autodiff.h:151,
from /usr/local/include/ceres/autodiff_cost_function.h:130,
from /usr/local/include/ceres/ceres.h:37,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.h:20,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOptNode.cpp:13:
/usr/local/include/ceres/internal/parameter_dims.h:44:55: error: ‘integer_sequence’ is not a member of ‘std’
44 | constexpr bool IsValidParameterDimensionSequence(std::integer_sequence<int>) {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/parameter_dims.h:44:72: error: expected primary-expression before ‘int’
44 | constexpr bool IsValidParameterDimensionSequence(std::integer_sequence<int>) {
| ^~~
/usr/local/include/ceres/internal/parameter_dims.h:50:10: error: ‘template<int N, int ...Ts> constexpr const bool ceres::internal::IsValidParameterDimensionSequence’ redeclared as different kind of entity
50 | std::integer_sequence<int, N, Ts...>) {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/parameter_dims.h:44:16: note: previous declaration ‘constexpr const bool ceres::internal::IsValidParameterDimensionSequence’
44 | constexpr bool IsValidParameterDimensionSequence(std::integer_sequence<int>) {
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/parameter_dims.h:50:10: error: ‘integer_sequence’ is not a member of ‘std’
50 | std::integer_sequence<int, N, Ts...>) {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/parameter_dims.h:50:27: error: expected primary-expression before ‘int’
50 | std::integer_sequence<int, N, Ts...>) {
| ^~~
/usr/local/include/ceres/internal/parameter_dims.h:50:40: error: expected ‘)’ before ‘>’ token
50 | std::integer_sequence<int, N, Ts...>) {
| ^
| )
/usr/local/include/ceres/internal/parameter_dims.h:49:49: note: to match this ‘(’
49 | constexpr bool IsValidParameterDimensionSequence(
| ^
/usr/local/include/ceres/internal/parameter_dims.h:69:27: error: ‘integer_sequence’ in namespace ‘std’ does not name a template type
69 | using Parameters = std::integer_sequence<int, Ns...>;
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/parameter_dims.h:74:41: error: there are no arguments to ‘Parameters’ that depend on a template parameter, so a declaration of ‘Parameters’ must be available [-fpermissive]
74 | IsValidParameterDimensionSequence(Parameters());
| ^~~~~~~~~~
/usr/local/include/ceres/internal/parameter_dims.h:74:41: note: (if you use ‘-fpermissive’, G++ will accept your code, but allowing the use of an undeclared name is deprecated)
/usr/local/include/ceres/internal/parameter_dims.h:85:16: error: ‘integer_sequence’ is not a member of ‘std’
85 | Sum<std::integer_sequence<int, Ns...>>::Value;
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/parameter_dims.h:85:16: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/parameter_dims.h:85:43: error: template argument 1 is invalid
85 | Sum<std::integer_sequence<int, Ns...>>::Value;
| ^~
/usr/local/include/ceres/internal/parameter_dims.h:101:20: error: ‘std::integer_sequence’ has not been declared
101 | T* ptr, std::integer_sequence<int, Indices...>) {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/parameter_dims.h:101:36: error: expected ‘,’ or ‘...’ before ‘<’ token
101 | T* ptr, std::integer_sequence<int, Indices...>) {
| ^
/usr/local/include/ceres/internal/parameter_dims.h: In static member function ‘static std::array<T*, ceres::internal::ParameterDims<IsDynamic, Ns>::kNumParameterBlocks> ceres::internal::ParameterDims<IsDynamic, Ns>::GetUnpackedParameters(T*)’:
/usr/local/include/ceres/internal/parameter_dims.h:94:35: error: ‘Parameters’ was not declared in this scope; did you mean ‘ParameterDims’?
94 | using Offsets = ExclusiveScan<Parameters>;
| ^~~~~~~~~~
| ParameterDims
/usr/local/include/ceres/internal/parameter_dims.h:94:45: error: template argument 1 is invalid
94 | using Offsets = ExclusiveScan<Parameters>;
| ^
/usr/local/include/ceres/internal/parameter_dims.h:95:39: error: there are no arguments to ‘Offsets’ that depend on a template parameter, so a declaration of ‘Offsets’ must be available [-fpermissive]
95 | return GetUnpackedParameters(ptr, Offsets());
| ^~~~~~~
In file included from /usr/local/include/ceres/internal/autodiff.h:152,
from /usr/local/include/ceres/autodiff_cost_function.h:130,
from /usr/local/include/ceres/ceres.h:37,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.h:20,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOptNode.cpp:13:
/usr/local/include/ceres/internal/variadic_evaluate.h: At global scope:
/usr/local/include/ceres/internal/variadic_evaluate.h:51:39: error: ‘std::integer_sequence’ has not been declared
51 | std::integer_sequence<int, Indices...>) {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/variadic_evaluate.h:51:55: error: expected ‘,’ or ‘...’ before ‘<’ token
51 | std::integer_sequence<int, Indices...>) {
| ^
/usr/local/include/ceres/internal/variadic_evaluate.h:62:39: error: ‘std::integer_sequence’ has not been declared
62 | std::integer_sequence<int>) {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/variadic_evaluate.h:62:55: error: expected ‘,’ or ‘...’ before ‘<’ token
62 | std::integer_sequence<int>) {
| ^
/usr/local/include/ceres/internal/variadic_evaluate.h: In function ‘bool ceres::internal::VariadicEvaluateImpl(const Functor&, const T* const*, T*, const void*)’:
/usr/local/include/ceres/internal/variadic_evaluate.h:71:12: error: ‘make_integer_sequence’ in namespace ‘std’ does not name a template type
71 | std::make_integer_sequence<int, ParameterDims::kNumParameterBlocks>;
| ^~~~~~~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/variadic_evaluate.h:74:31: error: there are no arguments to ‘ParameterBlockIndices’ that depend on a template parameter, so a declaration of ‘ParameterBlockIndices’ must be available [-fpermissive]
74 | ParameterBlockIndices());
| ^~~~~~~~~~~~~~~~~~~~~
In file included from /usr/local/include/ceres/autodiff_cost_function.h:130,
from /usr/local/include/ceres/ceres.h:37,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.h:20,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOptNode.cpp:13:
/usr/local/include/ceres/internal/autodiff.h: At global scope:
/usr/local/include/ceres/internal/autodiff.h:217:39: error: ‘integer_sequence’ is not a member of ‘std’
217 | struct Make1stOrderPerturbations<std::integer_sequence<int, N, Ns...>,
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h:217:39: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/autodiff.h:217:69: error: template argument 1 is invalid
217 | struct Make1stOrderPerturbations<std::integer_sequence<int, N, Ns...>,
| ^
/usr/local/include/ceres/internal/autodiff.h:232:39: error: ‘integer_sequence’ is not a member of ‘std’
232 | struct Make1stOrderPerturbations<std::integer_sequence<int>, ParameterIdx, Total> {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h:232:39: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/autodiff.h:232:59: error: template argument 1 is invalid
232 | struct Make1stOrderPerturbations<std::integer_sequence<int>, ParameterIdx, Total> {
| ^
/usr/local/include/ceres/internal/autodiff.h:280:31: error: ‘integer_sequence’ is not a member of ‘std’
280 | struct Take1stOrderParts<std::integer_sequence<int, N, Ns...>,
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h:280:31: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/autodiff.h:280:61: error: template argument 1 is invalid
280 | struct Take1stOrderParts<std::integer_sequence<int, N, Ns...>,
| ^
/usr/local/include/ceres/internal/autodiff.h:296:31: error: ‘integer_sequence’ is not a member of ‘std’
296 | struct Take1stOrderParts<std::integer_sequence<int>, ParameterIdx, Offset> {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/autodiff.h:296:31: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/autodiff.h:296:51: error: template argument 1 is invalid
296 | struct Take1stOrderParts<std::integer_sequence<int>, ParameterIdx, Offset> {
| ^
In file included from /usr/local/include/ceres/ceres.h:42,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.h:20,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOptNode.cpp:13:
/usr/local/include/ceres/cost_function_to_functor.h:161:29: error: ‘std::integer_sequence’ has not been declared
161 | std::integer_sequence<int, Indices...>) {
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/cost_function_to_functor.h:161:45: error: expected ‘,’ or ‘...’ before ‘<’ token
161 | std::integer_sequence<int, Indices...>) {
| ^
/usr/local/include/ceres/cost_function_to_functor.h: In member function ‘bool ceres::CostFunctionToFunctor<kNumResiduals, Ns>::operator()(const T*, Ts* ...) const’:
/usr/local/include/ceres/cost_function_to_functor.h:147:14: error: ‘make_integer_sequence’ in namespace ‘std’ does not name a template type
147 | std::make_integer_sequence<int,
| ^~~~~~~~~~~~~~~~~~~~~
/usr/local/include/ceres/cost_function_to_functor.h:150:41: error: there are no arguments to ‘Indices’ that depend on a template parameter, so a declaration of ‘Indices’ must be available [-fpermissive]
150 | GetParameterPointers<T>(params, Indices());
| ^~~~~~~
In file included from /usr/local/include/ceres/dynamic_numeric_diff_cost_function.h:44,
from /usr/local/include/ceres/ceres.h:48,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOpt.h:20,
from /home/fastpx4/dai_ws/src/VINS-Fusion/global_fusion/src/globalOptNode.cpp:13:
/usr/local/include/ceres/internal/numeric_diff.h: At global scope:
/usr/local/include/ceres/internal/numeric_diff.h:441:48: error: ‘integer_sequence’ is not a member of ‘std’
441 | std::integer_sequence<int, N, Ns...>,
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/numeric_diff.h:441:48: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/numeric_diff.h:441:78: error: template argument 2 is invalid
441 | std::integer_sequence<int, N, Ns...>,
| ^
/usr/local/include/ceres/internal/numeric_diff.h:485:63: error: ‘integer_sequence’ is not a member of ‘std’
485 | struct EvaluateJacobianForParameterBlocks<ParameterDims, std::integer_sequence<int>,
| ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/numeric_diff.h:485:63: error: ‘integer_sequence’ is not a member of ‘std’
/usr/local/include/ceres/internal/numeric_diff.h:485:83: error: template argument 2 is invalid
485 | struct EvaluateJacobianForParameterBlocks<ParameterDims, std::integer_sequence<int>,
| ^
make[2]: *** [VINS-Fusion/global_fusion/CMakeFiles/global_fusion_node.dir/build.make:63：VINS-Fusion/global_fusion/CMakeFiles/global_fusion_node.dir/src/globalOptNode.cpp.o] 错误 1
make[1]: *** [CMakeFiles/Makefile2:1167：VINS-Fusion/global_fusion/CMakeFiles/global_fusion_node.dir/all] 错误 2
make: *** [Makefile:141：all] 错误 2
Invoking "make -j8 -l8" failed
2. 修改Cmakelist (四个文件夹)
1）. 将c++11 修改为c++14
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++14")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC")
2）. 明确指定find_package中的OpenCV的版本：
#find_package(OpenCV REQUIRED)
find_package(OpenCV 4 REQUIRED)
3. 添加头文件
1). camera_models>include>camodocal>chessboard>Chessboard.h中添加
#include <opencv2/imgproc/types_c.h> #include <opencv2/calib3d/calib3d_c.h>2). 在以下几个文件中添加：
- camera_models/include/camodocal/calib/CameraCalibration.h
- loop_fusion/src/pose_graph.h
- vins_estimator/src/featureTracker/feature_tracker.h
- loop_fusion/src/ThirdParty/DVision/BRIEF.h
#include <opencv2/imgproc/types_c.h> #include <opencv2/imgproc/imgproc_c.h>
4. 仍然有一部分错误：
error: ‘CV_LOAD_IMAGE_GRAYSCALE’ was not declared in this scope
122 | imLeft = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE );
| ^~~~~~~~~~~~~~~~~~~~~~~
/home/fastpx4/dai_ws/src/VINS-Fusion/vins_estimator/src/KITTIOdomTest.cpp: In function ‘int main(int, char**)’:
/home/fastpx4/dai_ws/src/VINS-Fusion/vins_estimator/src/KITTIOdomTest.cpp:95:39: error: ‘CV_LOAD_IMAGE_GRAYSCALE’ was not declared in this scope
95 | imLeft = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE );
| 处理：将报错文件中的
CV_LOAD_IMAGE_GRAYSCALE修改为cv::IMREAD_GRAYSCALE
1). 修改报错文件：VINS-Fusion/vins_estimator/src/KITTIGPSTest.cpp
122 imLeft = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE );
123 imRight = cv::imread(rightImagePath, CV_LOAD_IMAGE_GRAYSCALE );
修改后：
imLeft = cv::imread(leftImagePath, cv::IMREAD_GRAYSCALE );
imRight = cv::imread(rightImagePath, cv::IMREAD_GRAYSCALE );2). 修改文件：VINS-Fusion/vins_estimator/src/KITTIOdomTest.cpp
imLeft = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE );
sensor_msgs::ImagePtr imLeftMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imLeft).toImageMsg();
imLeftMsg->header.stamp = ros::Time(imageTimeList[i]);
pubLeftImage.publish(imLeftMsg);
imRight = cv::imread(rightImagePath, CV_LOAD_IMAGE_GRAYSCALE );
sensor_msgs::ImagePtr imRightMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imRight).toImageMsg();
imRightMsg->header.stamp = ros::Time(imageTimeList[i]);
pubRightImage.publish(imRightMsg);
修改后：
imLeft = cv::imread(leftImagePath, cv::IMREAD_GRAYSCALE );
sensor_msgs::ImagePtr imLeftMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imLeft).toImageMsg();
imLeftMsg->header.stamp = ros::Time(imageTimeList[i]);
pubLeftImage.publish(imLeftMsg);
imRight = cv::imread(rightImagePath, cv::IMREAD_GRAYSCALE );
sensor_msgs::ImagePtr imRightMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imRight).toImageMsg();
imRightMsg->header.stamp = ros::Time(imageTimeList[i]);
pubRightImage.publish(imRightMsg);二、编译通过！测试运行：
source devel/setup.bash1. 运行数据集Euroc：
roslaunch vins vins_rviz.launch
rosrun vins vins_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosrun loop_fusion loop_fusion_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
cd ../data/
rosbag play MH_01_easy.bag运行成功
2. 运行OAK-D 双目相机：（教程网址）
1.4. ubuntu 18.04 OAK-D系列相机 运行VINS-Fusion 双目+IMU — DepthAI Docs 0.3.0.0 documentation
roslaunch depthai_examples stereo_inertial_node.launch enableRviz:=false depth_aligned:=false
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/dai_ws/src/VINS-Fusion/config/oak_d/config.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/dai_ws/src/VINS-Fusion/config/oak_d/config.yaml注意：直接使用官网配置文件可能导致无法运行
在官网教程中使用的是OAK-D-S2相机，我使用的是OAK-D-PRO相机，配置参数有一些区别，需要对配置文件进行针对性的修改(在left.yaml和right.yaml文件中修改相机畸变参数)，才可以正常运行VINS-Fusion程序。
1. 启动相机驱动，确认相机正常运行：
roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false*depth_aligned:=false 此参数控制是否输出深度映射图像，这里是不输出
2.查看话题输出
rostopic list/clicked_point /initialpose /joint_states /move_base_simple/goal /nodelet_manager/bond /rosout /rosout_agg /stereo_inertial_publisher/right/camera_info /stereo_inertial_publisher/right/image_rect /stereo_inertial_publisher/stereo/camera_info /stereo_inertial_publisher/stereo/depth /stereo_inertial_publisher/stereo/image /stereo_inertial_publisher/stereo/image/compressed /stereo_inertial_publisher/stereo/image/compressed/parameter_descriptions /stereo_inertial_publisher/stereo/image/compressed/parameter_updates /stereo_inertial_publisher/stereo/image/compressedDepth /stereo_inertial_publisher/stereo/image/compressedDepth/parameter_descriptions /stereo_inertial_publisher/stereo/image/compressedDepth/parameter_updates /stereo_inertial_publisher/stereo/image/theora /stereo_inertial_publisher/stereo/image/theora/parameter_descriptions /stereo_inertial_publisher/stereo/image/theora/parameter_updates /stereo_inertial_publisher/stereo/points /tf /tf_static3. 输出话题数据，并将参数写入left.yaml和right.yaml中：
rostopic echo /stereo_inertial_publisher/left/camera_info
header: seq: 7 stamp: secs: 1711676690 nsecs: 777232861 frame_id: "oak_left_camera_optical_frame" height: 720 width: 1280 distortion_model: "rational_polynomial" D: [-11.679576873779297, 105.025146484375, -0.0006934021948836744, 0.0018576113507151604, -51.34315490722656, -11.695950508117676, 104.31090545654297, -49.294837951660156] K: [798.2916259765625, 0.0, 651.572265625, 0.0, 798.2916259765625, 361.30743408203125, 0.0, 0.0, 1.0] R: [0.9997826814651489, 0.002714010654017329, -0.020668860524892807, -0.00276787206530571, 0.999992847442627, -0.00257775466889143, 0.020661717280745506, 0.002634403295814991, 0.9997830390930176] P: [799.2300415039062, 0.0, 654.2191772460938, 59.83273947490379, 0.0, 799.2300415039062, 359.2149658203125, 0.0, 0.0, 0.0, 1.0, 0.0] binning_x: 0 binning_y: 0 roi: x_offset: 0 y_offset: 0 height: 0 width: 0 do_rectify: False ---将K矩阵对应填入文件left.yaml中
%YAML:1.0 --- model_type: PINHOLE camera_name: camera image_width: 640 image_height: 400 distortion_parameters: k1: 0.011099142353676499 k2: -0.05769482092275897 p1: -0.0009757653113701839 p2: 0.0025548857914714745 projection_parameters: fx: 798.2916259765625 fy: 798.2916259765625 cx: 651.572265625 cy: 361.30743408203125rostopic echo /stereo_inertial_publisher/right/camera_info
header: seq: 291 stamp: secs: 1711676701 nsecs: 243829779 frame_id: "oak_right_camera_optical_frame" height: 720 width: 1280 distortion_model: "rational_polynomial" D: [-6.891867160797119, 81.7864990234375, -0.00016954877355601639, 0.0002883463748730719, -11.735593795776367, -6.942117691040039, 81.16423034667969, -9.775546073913574] K: [799.2300415039062, 0.0, 654.2191772460938, 0.0, 799.2300415039062, 359.2149658203125, 0.0, 0.0, 1.0] R: [0.999980092048645, -0.0040285540744662285, 0.004851967561990023, 0.004015895072370768, 0.9999884963035583, 0.002615976147353649, -0.0048624505288898945, -0.0025964390952140093, 0.9999848008155823] P: [799.2300415039062, 0.0, 654.2191772460938, 0.0, 0.0, 799.2300415039062, 359.2149658203125, 0.0, 0.0, 0.0, 1.0, 0.0] binning_x: 0 binning_y: 0 roi: x_offset: 0 y_offset: 0 height: 0 width: 0 do_rectify: False ---%YAML:1.0 --- model_type: PINHOLE camera_name: camera image_width: 640 image_height: 400 distortion_parameters: k1: 0.005536021298200547 k2: -0.048229113205249675 p1: -0.0002985290327403832 p2: 0.0037187807087799125 projection_parameters: fx: 799.2300415039062 fy: 799.2300415039062 cx: 654.2191772460938 cy: 359.2149658203125
修改完成后，重新启动相机驱动及VIns-Fusion程序，即可成功运行。
*补充：
在文件(depthai-ros/depthai_examples/launch/stereo_inertial_node.launch)中可以发现，参数depth_aligned会决定输出话题是左右目图像还是深度图映射。
<node if="$(eval arg('depth_aligned') == true)" pkg="nodelet" type="nodelet" name="depth_image_to_rgb_pointcloud" args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager"> <param name="queue_size" value="10"/> <remap from="rgb/camera_info" to="/stereo_inertial_publisher/color/camera_info"/> <remap from="rgb/image_rect_color" to="/stereo_inertial_publisher/color/image"/> <remap from="depth_registered/image_rect" to="/stereo_inertial_publisher/stereo/image"/> <remap from="depth_registered/points" to="/stereo_inertial_publisher/stereo/points"/> </node> <node if="$(eval arg('depth_aligned') == false)" pkg="nodelet" type="nodelet" name="depth_image_to_pointcloud" args="load depth_image_proc/point_cloud_xyz nodelet_manager"> <param name="queue_size" value="10"/> <remap from="camera_info" to="/stereo_inertial_publisher/stereo/camera_info"/> <remap from="image_rect" to="/stereo_inertial_publisher/stereo/image"/> <remap from="points" to="/stereo_inertial_publisher/stereo/points"/> </node>如果启动相机驱动时不关闭这个参数，话题输出会不同。
/clicked_point /initialpose /joint_states /move_base_simple/goal /nodelet_manager/bond /rosout /rosout_agg /stereo_inertial_publisher/color/camera_info /stereo_inertial_publisher/color/image /stereo_inertial_publisher/color/image/compressed /stereo_inertial_publisher/color/image/compressed/parameter_descriptions /stereo_inertial_publisher/color/image/compressed/parameter_updates /stereo_inertial_publisher/color/image/compressedDepth /stereo_inertial_publisher/color/image/compressedDepth/parameter_descriptions /stereo_inertial_publisher/color/image/compressedDepth/parameter_updates /stereo_inertial_publisher/color/image/theora /stereo_inertial_publisher/color/image/theora/parameter_descriptions /stereo_inertial_publisher/color/image/theora/parameter_updates /stereo_inertial_publisher/color/preview/camera_info /stereo_inertial_publisher/color/preview/image /stereo_inertial_publisher/color/preview/image/compressed /stereo_inertial_publisher/color/preview/image/compressed/parameter_descriptions /stereo_inertial_publisher/color/preview/image/compressed/parameter_updates /stereo_inertial_publisher/color/preview/image/compressedDepth /stereo_inertial_publisher/color/preview/image/compressedDepth/parameter_descriptions /stereo_inertial_publisher/color/preview/image/compressedDepth/parameter_updates /stereo_inertial_publisher/color/preview/image/theora /stereo_inertial_publisher/color/preview/image/theora/parameter_descriptions /stereo_inertial_publisher/color/preview/image/theora/parameter_updates /stereo_inertial_publisher/color/yolov4_Spatial_detections /stereo_inertial_publisher/imu /stereo_inertial_publisher/stereo/camera_info /stereo_inertial_publisher/stereo/depth /stereo_inertial_publisher/stereo/depth/compressed /stereo_inertial_publisher/stereo/depth/compressed/parameter_descriptions /stereo_inertial_publisher/stereo/depth/compressed/parameter_updates /stereo_inertial_publisher/stereo/depth/compressedDepth /stereo_inertial_publisher/stereo/depth/compressedDepth/parameter_descriptions /stereo_inertial_publisher/stereo/depth/compressedDepth/parameter_updates /stereo_inertial_publisher/stereo/depth/theora /stereo_inertial_publisher/stereo/depth/theora/parameter_descriptions /stereo_inertial_publisher/stereo/depth/theora/parameter_updates /stereo_inertial_publisher/stereo/image /stereo_inertial_publisher/stereo/image/compressed /stereo_inertial_publisher/stereo/image/compressed/parameter_descriptions /stereo_inertial_publisher/stereo/image/compressed/parameter_updates /stereo_inertial_publisher/stereo/image/compressedDepth /stereo_inertial_publisher/stereo/image/compressedDepth/parameter_descriptions /stereo_inertial_publisher/stereo/image/compressedDepth/parameter_updates /stereo_inertial_publisher/stereo/image/theora /stereo_inertial_publisher/stereo/image/theora/parameter_descriptions /stereo_inertial_publisher/stereo/image/theora/parameter_updates /stereo_inertial_publisher/stereo/points /tf /tf_static
3. 运行realsense_D435i 双目相机：
由于OAK-D相机暂时无法成功运行ego-planner程序，先找了一个原框架适配的realsense的相机。
1. 启动相机驱动
roslaunch realsense2_camera rs_d435_camera_with_model.launch2. 打开Rviz 或 rqt_image_view查看，确认有话题输出
/camera/depth/image_rect_raw(深度图)、
/camera/infra1/image_rect_raw(左目图像)、
/camera/infra2/image_rect_raw(右目图像)
3. 修改相机标定数据，如同上述步骤，此处省略。
三、VIns-Fusion配置参数矫正
参考浙江大学Fast-Lab教程
【完结】从0制作自主空中机器人 | 开源 | 浙江大学Fast-Lab_哔哩哔哩_bilibili
1）启动Vins程序，查看输出话题；
/clicked_point /feature_tracker/feature /globalEstimator/car_model /globalEstimator/global_path /initialpose /joint_states /loop_fusion/base_path /loop_fusion/camera_pose_visual /loop_fusion/margin_cloud_loop_rect /loop_fusion/pose_graph /move_base_simple/goal /nodelet_manager/bond /rosout /rosout_agg /stereo_inertial_publisher/imu /stereo_inertial_publisher/left/camera_info /stereo_inertial_publisher/left/image_rect /stereo_inertial_publisher/left/image_rect/compressed /stereo_inertial_publisher/left/image_rect/compressed/parameter_descriptions /stereo_inertial_publisher/left/image_rect/compressed/parameter_updates /stereo_inertial_publisher/left/image_rect/compressedDepth /stereo_inertial_publisher/left/image_rect/compressedDepth/parameter_descriptions /stereo_inertial_publisher/left/image_rect/compressedDepth/parameter_updates /stereo_inertial_publisher/left/image_rect/theora /stereo_inertial_publisher/left/image_rect/theora/parameter_descriptions /stereo_inertial_publisher/left/image_rect/theora/parameter_updates /stereo_inertial_publisher/right/camera_info /stereo_inertial_publisher/right/image_rect /stereo_inertial_publisher/right/image_rect/compressed /stereo_inertial_publisher/right/image_rect/compressed/parameter_descriptions /stereo_inertial_publisher/right/image_rect/compressed/parameter_updates /stereo_inertial_publisher/right/image_rect/compressedDepth /stereo_inertial_publisher/right/image_rect/compressedDepth/parameter_descriptions /stereo_inertial_publisher/right/image_rect/compressedDepth/parameter_updates /stereo_inertial_publisher/right/image_rect/theora /stereo_inertial_publisher/right/image_rect/theora/parameter_descriptions /stereo_inertial_publisher/right/image_rect/theora/parameter_updates /stereo_inertial_publisher/stereo/camera_info /stereo_inertial_publisher/stereo/depth /stereo_inertial_publisher/stereo/depth/compressed /stereo_inertial_publisher/stereo/depth/compressed/parameter_descriptions /stereo_inertial_publisher/stereo/depth/compressed/parameter_updates /stereo_inertial_publisher/stereo/depth/compressedDepth /stereo_inertial_publisher/stereo/depth/compressedDepth/parameter_descriptions /stereo_inertial_publisher/stereo/depth/compressedDepth/parameter_updates /stereo_inertial_publisher/stereo/depth/theora /stereo_inertial_publisher/stereo/depth/theora/parameter_descriptions /stereo_inertial_publisher/stereo/depth/theora/parameter_updates /stereo_inertial_publisher/stereo/image /stereo_inertial_publisher/stereo/image/compressed /stereo_inertial_publisher/stereo/image/compressed/parameter_descriptions /stereo_inertial_publisher/stereo/image/compressed/parameter_updates /stereo_inertial_publisher/stereo/image/compressedDepth /stereo_inertial_publisher/stereo/image/compressedDepth/parameter_descriptions /stereo_inertial_publisher/stereo/image/compressedDepth/parameter_updates /stereo_inertial_publisher/stereo/image/theora /stereo_inertial_publisher/stereo/image/theora/parameter_descriptions /stereo_inertial_publisher/stereo/image/theora/parameter_updates /stereo_inertial_publisher/stereo/points /tf /tf_static /vins_cam_switch /vins_fusion/camera_pose /vins_fusion/camera_pose_visual /vins_fusion/extrinsic /vins_fusion/image_track /vins_fusion/imu_propagate /vins_fusion/key_poses /vins_fusion/keyframe_point /vins_fusion/keyframe_pose /vins_fusion/margin_cloud /vins_fusion/odometry /vins_fusion/path /vins_fusion/point_cloud /vins_imu_switch /vins_restart2）终端输入，查看程序输出position的漂移情况
rostopic echo /vins_fusion/imu_propagateheader: seq: 30359 stamp: secs: 1711678556 nsecs: 905394554 frame_id: "world" child_frame_id: '' pose: pose: position: x: -0.0018558396542408368 y: 0.0006243309409628052 z: 0.002800527454180711 orientation: x: 0.8243210546283225 y: 0.009086388930822076 z: -0.5658114242209056 w: 0.01642161438402998 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] twist: twist: linear: x: 0.0003866194433702027 y: 0.0008024273412679251 z: 0.00022701032056310962 angular: x: 0.0 y: 0.0 z: 0.0 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ---3）(将相机与板载计算机固定在无人机上之后)抱起无人机在场地内缓慢走动，在vins的输出文件夹下打开extrinsic_parameter.txt
将文件内容替换在config.yaml文件中对应位置。
%YAML:1.0 #common parameters #support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; imu: 1 num_of_cam: 2 #imu_topic: "/mavros/imu/data_raw" imu_topic: "/stereo_inertial_publisher/imu" #/stereo_inertial_publisher/imu image0_topic: "/stereo_inertial_publisher/left/image_rect" image1_topic: "/stereo_inertial_publisher/right/image_rect" output_path: "/home/fastpx4/abc/vins_output" cam0_calib: "left.yaml" cam1_calib: "right.yaml" image_width: 640 #640 image_height: 400 #400 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ 1.1582253353211946e-02, 9.9992842406279825e-01, 2.9996930761942851e-03, -1.5936022004130995e-02, 9.9992731716619121e-01, -1.1572090774962351e-02, -3.3833563099195255e-03, -8.4883987362590493e-02, -3.3484014224460091e-03, 3.0386619399666269e-03, -9.9998977731851335e-01, -3.4809993558054787e-02, 0., 0., 0., 1. ] body_T_cam1: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ 1.1352436793192977e-02, 9.9993544249239663e-01, 4.8272807242477774e-04, -1.5103709396341888e-02, 9.9993074772788504e-01, -1.1350883268602896e-02, -3.1076031560835337e-03, -9.5661037692648648e-03, -3.1019231469685844e-03, 5.1797351081671458e-04, -9.9999505487588936e-01, -3.3604401456376665e-02, 0., 0., 0., 1. ] #Multiple thread support multiple_thread: 1 #feature traker paprameters max_cnt: 130 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 0 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 1 # publish tracking image as topic flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time keyframe_parallax: 10.0 # keyframe selection threshold (pixel) #imu parameters The more accurate parameters you provide, the better performance acc_n: 0.1 # accelerometer measurement noise standard deviation. gyr_n: 0.01 # gyroscope measurement noise standard deviation. acc_w: 0.001 # accelerometer bias random work noise standard deviation. gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. g_norm: 9.81007 # gravity magnitude #unsynchronization parameters estimate_td: 1 # online estimate time offset between camera and imu td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) #loop closure parameters load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' pose_graph_save_path: "/home/fastpx4/abc/vins_output" # save and load path save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0*重复上述过程，覆盖配置参数，直到话题中位置输出收敛到满意程度。
四、ego-planner导航
在浙大的Fast-Drone-250项目中，ego_planner相关的代码以及启动launch文件位置在src/planner/plan_manage。
1. 启动文件(single_run_in_exp.launch)
1）修改深度图像话题，对应上述D435相机驱动位置的话题
<arg name="depth_topic" value="/camera/depth/image_rect_raw"/>2）修改相机参数
<arg name="cx" value="319.7215270996094"/> <arg name="cy" value="239.18408203125"/> <arg name="fx" value="385.1890869140625"/> <arg name="fy" value="385.1890869140625"/>3）设置地图尺寸及vins里程计话题
<arg name="map_size_x" value="30"/> <arg name="map_size_y" value="30"/> <arg name="map_size_z" value="3.0"/> <arg name="odom_topic" value="/vins_fusion/imu_propagate"/>2. 参数配置文件(advanced_param_exp.xml)
根据自己的无人机设定配置参数
<param name="grid_map/resolution" value="0.2" /> <!-- 分辨率 --> <param name="grid_map/map_size_x" value="$(arg map_size_x_)" /> <param name="grid_map/map_size_y" value="$(arg map_size_y_)" /> <param name="grid_map/map_size_z" value="$(arg map_size_z_)" /> <param name="grid_map/local_update_range_x" value="5.5" /> <param name="grid_map/local_update_range_y" value="5.5" /> <param name="grid_map/local_update_range_z" value="4.5" /> <param name="grid_map/obstacles_inflation" value="0.4" /> <!-- 膨胀系数，最大以分辨率膨胀4倍--> <param name="grid_map/local_map_margin" value="10"/> <!-- 地面高度 --> <param name="grid_map/ground_height" value="-0.01"/>测试ego-planner建图效果
启动两个终端，分别输入：
roslaunch ego_planner single_run_in_exp.launchroslaunch ego_planner rviz.launch即可看到：(如果没有障碍物，检查深度图话题是否正常输出)
为方便启动，编写脚本，一次启动！
source devel/setup.bash roslaunch realsense2_camera rs_d435_camera_with_model.launch & sleep 8; roslaunch mavros px4.launch & sleep 5; roslaunch vins fast_drone_250.launch & sleep 3; roslaunch ego_planner single_run_in_exp.launch & sleep 2 ; roslaunch ego_planner rviz.launch ; wait;3. 自动起飞控制(src/realflight_modules/px4ctrl/launch/run_ctrl.launch)
在px4ctrl/config/ctrl_param_fpv.yaml文件下修改无人机重量、及自动起飞配置，悬停油门水平，其他参数可自行摸索，不需要重点关注。
######################################################################### # ⭐⭐⭐ Set parameters carefully for satisfied performance! ⭐⭐⭐ # ######################################################################### mass : 3.8 # kg gra : 9.81 pose_solver : 1 # 0:From ZhepeiWang (drag & less singular) 1:From ZhepeiWang, 2:From rotor-drag ctrl_freq_max : 100.0 use_bodyrate_ctrl: false max_manual_vel: 1.0 max_angle: 30 # Attitude angle limit in degree. A negative value means no limit. low_voltage: 13.2 # 4S battery rc_reverse: # *通道反向 roll: false pitch: false yaw: false throttle: false auto_takeoff_land: enable: true enable_auto_arm: true no_RC: false takeoff_height: 1.0 # m起飞高度 takeoff_land_speed: 0.3 # m/s速度 thrust_model: # The model that maps thrust signal u(0~1) to real thrust force F(Unit:N): F=K1*Voltage^K2*(K3*u^2+(1-K3)*u). print_value: true # display the value of “thr_scale_compensate” or “hover_percentage” during thrust model estimating. accurate_thrust_model: false # This can always enabled if don't require accurate control performance :-) # accurate thrust mapping parameters K1: 0.7583 # Needs precise calibration! K2: 1.6942 # Needs precise calibration! K3: 0.6786 # Needs precise calibration! K3 equals THR_MDL_FAC in https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html. # approximate thrust mapping parameters hover_percentage: 0.50 # 悬停油门水平 Thrust percentage in Stabilize/Arco mode # * gain: # Cascade PID controller. Recommend to read the code. Kp0: 1.5 Kp1: 1.5 Kp2: 1.5 Kv0: 1.5 Kv1: 1.5 Kv2: 1.5 # ↓↓↓ No use now -- Kvi0: 0.0 Kvi1: 0.0 Kvi2: 0.0 Kvd0: 0.0 Kvd1: 0.0 Kvd2: 0.0 # ↓↓↓ Only used in rate control mode. KAngR: 20.0 KAngP: 20.0 KAngY: 20.0 rotor_drag: x: 0.0 # The reduced acceleration on each axis caused by rotor drag. Unit:(m*s^-2)/(m*s^-1). y: 0.0 # Same as above z: 0.0 # Same as above k_thrust_horz: 0.0 # Set to 0 recommended... -- #超时阈值 msg_timeout: odom: 0.5 rc: 0.5 cmd: 0.5 imu: 0.5 bat: 0.54. 自主导航飞行(启动顺序)
1）启动双目相机
2）连接飞控
3）启动vins_fusion节点
4）启动ego-planner节点
5）启动ego可视化界面
source devel/setup.bash roslaunch realsense2_camera rs_d435_camera_with_model.launch & sleep 8; roslaunch mavros px4.launch & sleep 5; roslaunch vins fast_drone_250.launch & sleep 3; roslaunch ego_planner single_run_in_exp.launch & sleep 2 ; roslaunch ego_planner rviz.launch ; wait;6）在原地抱着飞机转几圈，确认vins里程计不飘
7）启动无人机控制
roslaunch px4ctrl run_ctrl.launch8）自动起飞(运行起飞脚本)
首先，确定打开飞机遥控器，进入板载模式，解锁。（可能不同的遥控器要求不同）
sh shfiles/takeoff.sh
起飞拒绝，可能原因，vins里程计漂移较大，遥控器模式不正确，具体可查看源码文件
(src/planner/plan_manage/src/ego_replan_fsm.cpp)
9）设定导航目标
选中rviz界面，按下G键，在地图中点击，设定导航目标。
