全流程：ubuntu20.04运行Vins-fusion+ego_planner
求上进的小怪兽 2024-07-24 14:37:01 阅读 72
目录
一、下载编译Vins-fusion
1. 创建工作空间vins_ws，并在src目录下git源码并编译
2. 修改Cmakelist (四个文件夹)
3. 添加头文件
4. 仍然有一部分错误：
二、编译通过！测试运行：
1. 运行数据集Euroc
2. 运行OAK-D 双目相机
*补充：
三、VIns-Fusion配置参数矫正
1. 启动Vins程序，查看输出话题
2. 终端输入，查看程序输出position的漂移情况
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
此时，编译会出现一系列错误：
<code>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.bash
1. 运行数据集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
https://docs.oakchina.cn/en/latest/pages/HighLevel/ros/ros_VINS_Fusion.html按照官网教程安装相机驱动，可以使用系统自带的opencv4.2版本进行编译。
<code>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_static
3. 输出话题数据，并将参数写入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.30743408203125
rostopic 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会决定输出话题是左右目图像还是深度图映射。
<code> <node if="$(eval arg('depth_aligned') == true)" pkg="nodelet" type="nodelet" name="depth_image_to_rgb_pointcloud"code>
args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager">code>
<param name="queue_size" value="10"/>code>
<remap from="rgb/camera_info" to="/stereo_inertial_publisher/color/camera_info"/>code>
<remap from="rgb/image_rect_color" to="/stereo_inertial_publisher/color/image"/>code>
<remap from="depth_registered/image_rect" to="/stereo_inertial_publisher/stereo/image"/> code>
<remap from="depth_registered/points" to="/stereo_inertial_publisher/stereo/points"/>code>
</node>
<node if="$(eval arg('depth_aligned') == false)" pkg="nodelet" type="nodelet" name="depth_image_to_pointcloud"code>
args="load depth_image_proc/point_cloud_xyz nodelet_manager">code>
<param name="queue_size" value="10"/>code>
<remap from="camera_info" to="/stereo_inertial_publisher/stereo/camera_info"/>code>
<remap from="image_rect" to="/stereo_inertial_publisher/stereo/image"/> code>
<remap from="points" to="/stereo_inertial_publisher/stereo/points"/>code>
</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.launch
2. 打开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
https://www.bilibili.com/video/BV1WZ4y167me/?spm_id_from=333.1007.top_right_bar_window_custom_collection.content.click&vd_source=7efc9d03142aa088952c10884de6dadfZJU-FAST-Lab/Fast-Drone-250: hardware and software design of the 250mm autonomous drone (github.com)
https://github.com/ZJU-FAST-Lab/Fast-Drone-250
1）启动Vins程序，查看输出话题；
<code>/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_restart
2）终端输入，查看程序输出position的漂移情况
rostopic echo /vins_fusion/imu_propagate
header:
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"/>code>
2）修改相机参数
<arg name="cx" value="319.7215270996094"/>code>
<arg name="cy" value="239.18408203125"/>code>
<arg name="fx" value="385.1890869140625"/>code>
<arg name="fy" value="385.1890869140625"/>code>
3）设置地图尺寸及vins里程计话题
<arg name="map_size_x" value="30"/>code>
<arg name="map_size_y" value="30"/>code>
<arg name="map_size_z" value="3.0"/>code>
<arg name="odom_topic" value="/vins_fusion/imu_propagate"/>code>
2. 参数配置文件(advanced_param_exp.xml)
根据自己的无人机设定配置参数
<param name="grid_map/resolution" value="0.2" /> <!-- 分辨率 -->code>
<param name="grid_map/map_size_x" value="$(arg map_size_x_)" /> code>
<param name="grid_map/map_size_y" value="$(arg map_size_y_)" /> code>
<param name="grid_map/map_size_z" value="$(arg map_size_z_)" /> code>
<param name="grid_map/local_update_range_x" value="5.5" /> code>
<param name="grid_map/local_update_range_y" value="5.5" /> code>
<param name="grid_map/local_update_range_z" value="4.5" /> code>
<param name="grid_map/obstacles_inflation" value="0.4" /> <!-- 膨胀系数，最大以分辨率膨胀4倍-->code>
<param name="grid_map/local_map_margin" value="10"/>code>
<!-- 地面高度 -->
<param name="grid_map/ground_height" value="-0.01"/>code>
测试ego-planner建图效果
启动两个终端，分别输入：
roslaunch ego_planner single_run_in_exp.launch
roslaunch ego_planner rviz.launch
即可看到：(如果没有障碍物，检查深度图话题是否正常输出)
为方便启动，编写脚本，一次启动！
<code>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.5
4. 自主导航飞行(启动顺序)
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.launch
8）自动起飞(运行起飞脚本)
首先，确定打开飞机遥控器，进入板载模式，解锁。（可能不同的遥控器要求不同）
sh shfiles/takeoff.sh
起飞拒绝，可能原因，vins里程计漂移较大，遥控器模式不正确，具体可查看源码文件
(src/planner/plan_manage/src/ego_replan_fsm.cpp)
9）设定导航目标
选中rviz界面，按下G键，在地图中点击，设定导航目标。
声明
本文内容仅代表作者观点，或转载于其他网站，本站不以此文作为商业用途
如有涉及侵权，请联系本站进行删除
转载本站原创文章，请注明来源及作者。