vins-get-pose/camera

本文介绍了一种使用ROS环境下的vins_estimator节点提取相机位姿数据并保存为CSV文件的方法。通过订阅camera_pose话题,获取到的时间戳、位置及四元数数据被记录下来,便于后续的数据分析和处理。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >


#!/usr/bin/env python2
import rospy
from nav_msgs.msg import Odometry

import os
import argparse
from tf.transformations import euler_from_quaternion, quaternion_from_euler

pitch = yaw = roll =  0.0
f = open('./vin_camera_pose.csv', 'w')
#f.write('timestamp,x,y,z,pitch,roll,yaw\n')
f.write('timestamp,x,y,z,q1,q2,q3,q0\n')

def extract():

    rospy.init_node('get_vins_path', anonymous=True)
    rospy.Subscriber('/vins_estimator/camera_pose', Odometry, callback)
    rospy.spin()

def callback(data):

    orientation_list = [data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w]
    (roll, pitch, yaw) = euler_from_quaternion (orientation_list)

    f.write('%.12f,%.12f,%.12f,%.12f,%.12f,%.12f,%.12f,%.12f\n' %
            (
            data.header.stamp.to_sec(),
            data.pose.pose.position.x,
            data.pose.pose.position.y,
            data.pose.pose.position.z,
            data.pose.pose.orientation.x,
            data.pose.pose.orientation.y,
            data.pose.pose.orientation.z,
            data.pose.pose.orientation.w
#                       roll,
#			pitch,            
#			yaw 
			
            )
            )


if __name__ == '__main__':

    extract()

 

[ 71%] Building CXX object VINS-Fusion/vins_estimator/CMakeFiles/vins_lib.dir/src/estimator/feature_manager.cpp.o /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/pose_graph.cpp: In member function ‘int PoseGraph::detectLoop(KeyFrame*, int)’: /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/pose_graph.cpp:409:47: warning: comparison of integer expressions of different signedness: ‘DBoW2::EntryId’ {aka ‘unsigned int’} and ‘int’ [-Wsign-compare] 409 | if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015)) In file included from /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/ThirdParty/DBoW/DBoW2.h:63, from /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/keyframe.h:24, from /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/pose_graph.h:28, from /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/pose_graph.cpp:12: /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/ThirdParty/DBoW/TemplatedVocabulary.h: In instantiation of ‘void DBoW2::TemplatedVocabulary<TDescriptor, F>::loadBin(const string&) [with TDescriptor = boost::dynamic_bitset<>; F = DBoW2::FBrief; std::string = std::__cxx11::basic_string<char>]’: /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/ThirdParty/DBoW/TemplatedVocabulary.h:439:5: required from ‘DBoW2::TemplatedVocabulary<TDescriptor, F>::TemplatedVocabulary(const string&) [with TDescriptor = boost::dynamic_bitset<>; F = DBoW2::FBrief; std::string = std::__cxx11::basic_string<char>]’ /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/pose_graph.cpp:64:39: required from here /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/ThirdParty/DBoW/TemplatedVocabulary.h:1529:29: warning: comparison of integer expressions of different signedness: ‘unsigned int’ and ‘int32_t’ {aka ‘int’} [-Wsign-compare] 1529 | for(unsigned int i = 0; i < voc.nNodes; ++i) | ~~^~~~~~~~~~~~ /home/y/vins-fusion/src/VINS-Fusion/loop_fusion/src/ThirdParty/DBoW/TemplatedVocabulary.h:1553:29: warning: comparison of integer expressions of different signedness: ‘unsigned int’ and ‘int32_t’ {aka ‘int’} [-Wsign-compare] 1553 | for(unsigned int i = 0; i < voc.nWords; ++i) | ~~^~~~~~~~~~~~ [ 72%] Building CXX object VINS-Fusion/vins_estimator/CMakeFiles/vins_lib.dir/src/factor/pose_local_parameterization.cpp.o [ 73%] Building CXX object VINS-Fusion/vins_estimator/CMakeFiles/vins_lib.dir/src/factor/projectionTwoFrameOneCamFactor.cpp.o [ 75%] Building CXX object VINS-Fusion/vins_estimator/CMakeFiles/vins_lib.dir/src/factor/projectionTwoFrameTwoCamFactor.cpp.o [ 76%] Building CXX object VINS-Fusion/
最新发布
03-26
<think>嗯,用户给了一个编译输出的日志,里面有几个警告,还有构建进度。看起来用户可能是在编译VINS-Fusion项目时遇到了问题,需要帮助解决这些警告或者理解这些信息。首先,我需要分析这些警告的具体内容和原因。 首先,第一个警告是在pose_graph.cpp的第409行,比较了DBoW2::EntryId(无符号整数)和int类型的min_index。这里可能是有符号和无符号整数比较的问题,编译器提示可能会引发意外行为,因为无符号数永远不会是负数,而min_index可能被赋值为-1,这在比较时会被解释为很大的正数,导致条件判断错误。所以用户需要修改这里的比较逻辑,确保类型一致。 然后,在TemplatedVocabulary.h的1529和1553行,循环变量i是unsigned int,而比较的voc.nNodes和voc.nWords是int32_t(有符号整数)。这同样会导致有符号和无符号比较的警告。需要将循环变量改为int或者将nNodes和nWords转为unsigned,或者强制类型转换。 接下来,用户可能需要知道如何解决这些警告。比如,在第一个警告中,可以将min_index的类型改为DBoW2::EntryId,或者在比较前进行显式类型转换。但要注意,如果min_index初始化为-1,转为无符号会变成最大值,可能导致问题。所以可能需要检查逻辑,是否有必要使用-1作为无效值,或者改用其他方式表示无效索引。 另外,用户可能不太清楚这些警告的严重性。需要说明这些是编译器提示的潜在问题,虽然不会直接导致编译失败,但可能引起运行时错误,特别是在涉及索引和循环时。建议用户修复这些警告以确保代码的健壮性。 还要考虑到用户可能是在进行项目构建或开发,遇到问题需要解决。可能需要指导用户如何修改代码,比如具体在哪些文件的行号进行改动,或者建议他们查看相关的DBoW2库文档,确认变量类型是否匹配。 最后,用户可能没有提供完整的错误信息,但根据现有的日志,可以推断出问题主要在于有符号和无符号整数的比较。需要逐步解释每个警告的原因和解决方法,帮助用户理解并修复代码。</think>这个编译输出信息显示的是VINS-Fusion项目在构建过程中遇到的代码警告和编译进度。以下是逐层解析: --- ### 一、核心问题分析 #### 1. 符号性不一致警告(关键问题) 在文件 `pose_graph.cpp:409` 处出现: ```cpp if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015)) ``` - **警告原因**:`DBoW2::EntryId` 是无符号整型(`unsigned int`),而 `min_index` 是 `int` 类型。在比较无符号和有符号整型时,可能产生意外行为(如负数会被解析为大正数)。 - **修复建议**: - 将 `min_index` 的类型改为 `DBoW2::EntryId` - 或使用显式类型转换:`ret[i].Id < static_cast<unsigned>(min_index)` #### 2. 模板库中的循环边界警告 在 `TemplatedVocabulary.h:1529` 和 `1553` 处出现: ```cpp for(unsigned int i = 0; i < voc.nNodes; ++i) // nNodes是int32_t ``` - **警告原因**:循环变量 `i` 是 `unsigned int`,而 `voc.nNodes` 是 `int32_t`(有符号整型)。不同符号类型的比较可能引发越界风险。 - **修复建议**: - 修改循环变量类型:`for(int i = 0; i < voc.nNodes; ++i)` - 或强制转换边界:`i < static_cast<int>(voc.nNodes)` --- ### 二、编译进度解读 ```bash [71%] Building CXX object VINS-Fusion/vins_estimator/CMakeFiles/vins_lib.dir/src/estimator/feature_manager.cpp.o [72%] Building... [73%] Building... [75%] Building... [76%] Building... ``` - **进度含义**:显示项目编译进度百分比,当前在编译 `vins_lib` 库的多个源文件(如特征管理、位姿参数化、投影因子等模块)。 --- ### 三、潜在风险说明 1. **警告的严重性**: - 这些警告不会直接导致编译失败,但可能引发运行时逻辑错误(如循环条件失效、索引越界)。 - 在SLAM系统中,闭环检测(`detectLoop`)和词袋模型(`DBoW2`)是核心模块,建议修复。 2. **代码库关联**: - 警告涉及第三方库 `DBoW2`,可能需要同步修改库代码或检查参数传递一致性。 --- ### 四、建议操作步骤 1. **优先修复符号性警告**: ```cpp // 原问题代码(pose_graph.cpp) if (ret[i].Id < min_index) → 改为 if (ret[i].Id < static_cast<unsigned>(min_index)) ``` 2. **同步检查DBoW2配置**: - 确认使用的 `DBoW2` 版本是否与VINS-Fusion兼容 - 检查词袋模型加载路径(`TemplatedVocabulary.h` 中的二进制文件加载) 3. **重新编译验证**: ```bash rm -rf build/ && mkdir build && cd build cmake .. && make -j4 # 观察是否仍有同类警告 ``` --- ### 五、技术背景扩展 1. **DBoW2的作用**: - 用于视觉词袋模型构建,通过二进制特征(如BRIEF)实现快速图像检索,在VINS中用于闭环检测。 2. **符号性问题本质**: - C/C++中无符号数的特殊行为:`unsigned(0) - 1 = 4294967295`(32位系统),这与有符号数的 `-1` 比较时会导致条件判断错误。 --- 若需具体代码修改示例或进一步解释,请补充说明具体需求。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

大江东去浪淘尽千古风流人物

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值