vins 학습 코드 논리
17225 단어 vins-mono
문서 목록
1. 로스의 논리 구조
1、std_msgs/Header
이미지/PointCoud/IMU 등 각종 센서 데이터 구조에서의 헤더 정보uint32 seq //ID
time stamp //
string frame_id // ID
2、sensor_msgs::ImageConstPtr
피처링에서...trackers_node.cpp에서 콜백 함수img콜백의 입력으로 그림을 표시합니다.std_msgs/Header header //
uint32 height //
uint32 width //
string encoding // —— 、 、
uint8 is_bigendian //
uint32 sted //
uint8[] data // ,size is (step * rows)
3、sensor_msgs::PointCloudPtr featur_points
피처링에서...trackers_node.cpp에서 생성하고 봉인하여main()에서 화제'/feature tracker/feature'를 발표합니다. 한 프레임의 이미지의 특징점 정보를 포함합니다.sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
pub_img.publish(feature_points);
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
Definition std_msgs/Header header #
feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";
geometry_msgs/Point32[] points #3D (x,y,z)
sensor_msgs/ChannelFloat32[] channels #[ ID, u,v, vx,vy]
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);
sensor_msgs::PointCloud msg_match_points
이 물건은 데이터 형식과 이전의featurepoints는 같지만 채널스는 다르다.피처링에서...trackers_node.cpp에서 msg 로 생성 및 봉인match_points, posegraph_node.cpp의main()에서 발표된 화제'/pose graph/match points'는 주로 재배치된 두 프레임 사이의 일치점과 일치 관계(변환 행렬)를 포함한다.sensor_msgs::PointCloud msg_match_points;
pub_match_points.publish(msg_match_points);
pub_match_points = n.advertise<sensor_msgs::PointCloud>("match_points", 100);
estimatornode.cpp의main()에서 이 화제가 구독되고 리셋 함수는relocalizationcallback() ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)
Definition std_msgs/Header header #
#msg_match_points.header.stamp = ros::Time(time_stamp);
geometry_msgs/Point32[] points #3D (x,y,z)
sensor_msgs/ChannelFloat32[] channels #[ T x,y,z, w,x,y,z ]
#t_q_index.values.push_back(T.x());
#t_q_index.values.push_back(T.y());
#t_q_index.values.push_back(T.z());
#t_q_index.values.push_back(Q.w());
#t_q_index.values.push_back(Q.x());
#t_q_index.values.push_back(Q.y());
#t_q_index.values.push_back(Q.z());
#t_q_index.values.push_back(index);
#msg_match_points.channels.push_back(t_q_index);
4、sensor_msgs::ImuConstptr
IMU 정보의 표준 데이터 구조Header header #
geometry_msgs/Quaternion orientation # [x,y,z,w]
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity # [x,y,z]
float64[9] angular_velocity_covariance # ,Row major( ) about x, y, z axes
geometry_msgs/Vector3 linear_acceleration # [x,y,z]
float64[9] linear_acceleration_covariance # Row major x, y z
코드의 조합 구조
1、measurements std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
estimator_node.cpp의 getMeasurements () 함수는imu와 이미지 데이터를 초보적으로 정렬하여 얻은 데이터 구조를 확보하고 이미지가 대응하는 시간 스탬프 내의 모든imu 데이터sensormsgs::PointCloudConstPtr는 프레임 이미지의featurepoints std::vector <:imuconstptr>는 현재 프레임과 이전 프레임의 시간 간격에 있는 모든 IMU 데이터가 둘을 하나의 데이터 구조로 조합하고 요소를 구축하여 이 구조의vector에 저장한다. 2, map>>> 이미지는estimator에 저장된다.cpp의process()에서 구축되고Estimator::processImage()에서 호출되는 역할은 각 특징점(camera id, [x,y,z,u,v,vx,vy])으로 구성된 맵이고 인덱스는featureid 3、map all_image_frame
estimator에 있습니다.h에서class Estimator로서의 속성 키는 이미지 프레임의 시간 스탬프이고 값은 이미지 프레임 클래스의 이미지 프레임 클래스는 이미지 프레임의 특징점과 시간 스탬프로 구성할 수 있으며 위치 Rt, 선적분 대상pre 도 저장되어 있다.integration, 키프레임인지 여부입니다.
2. 시각 추적 피처trackers
전언
VINS의 비주얼 프로세싱 전면에 있는 비주얼 추적 모듈에 대해 살펴보겠습니다. 주요 내용은 다음과 같습니다.
uint32 seq //ID
time stamp //
string frame_id // ID
std_msgs/Header header //
uint32 height //
uint32 width //
string encoding // —— 、 、
uint8 is_bigendian //
uint32 sted //
uint8[] data // ,size is (step * rows)
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
pub_img.publish(feature_points);
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
std_msgs/Header header #
feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";
geometry_msgs/Point32[] points #3D (x,y,z)
sensor_msgs/ChannelFloat32[] channels #[ ID, u,v, vx,vy]
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);
sensor_msgs::PointCloud msg_match_points;
pub_match_points.publish(msg_match_points);
pub_match_points = n.advertise<sensor_msgs::PointCloud>("match_points", 100);
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)
std_msgs/Header header #
#msg_match_points.header.stamp = ros::Time(time_stamp);
geometry_msgs/Point32[] points #3D (x,y,z)
sensor_msgs/ChannelFloat32[] channels #[ T x,y,z, w,x,y,z ]
#t_q_index.values.push_back(T.x());
#t_q_index.values.push_back(T.y());
#t_q_index.values.push_back(T.z());
#t_q_index.values.push_back(Q.w());
#t_q_index.values.push_back(Q.x());
#t_q_index.values.push_back(Q.y());
#t_q_index.values.push_back(Q.z());
#t_q_index.values.push_back(index);
#msg_match_points.channels.push_back(t_q_index);
Header header #
geometry_msgs/Quaternion orientation # [x,y,z,w]
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity # [x,y,z]
float64[9] angular_velocity_covariance # ,Row major( ) about x, y, z axes
geometry_msgs/Vector3 linear_acceleration # [x,y,z]
float64[9] linear_acceleration_covariance # Row major x, y z
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
전언
VINS의 비주얼 프로세싱 전면에 있는 비주얼 추적 모듈에 대해 살펴보겠습니다. 주요 내용은 다음과 같습니다.
코드 논리
img_calback
FeatureTracker::readImage() 이미지 데이터 읽기 처리
3. 상태 평가기 절차
주요 내용
입력:
구체적 절차
1,vinsestimator/estimator_node.cpp 1.
다중 스레드 공유, 상호 배척 잠금, 조건 잠금 설정
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
estimator_node.cpp에서 getMeaturement () 함수는imu와 이미지 데이터를 초보적으로 정렬한 데이터 구조로 이미지가 대응하는 시간 스탬프 내의imu 데이터와 연결되어 있는지 확인합니다.sensor_msgs::PointCloudConstPtr는 프레임 이미지의featurepoints std::vector <:imuconstptr > 는 현재 프레임과 이전 시간 간격의 모든 IMU 데이터가 둘을 하나의 데이터 구조로 조합하고 요소를 구축하여 이런 Vector로 저장합니다.
주 스레드voidprocess()
while를 통해 끊임없이 순환합니다. 주요 기능은 기다리고measurements를 가져오고 dt를 계산한 다음 함수를 실행하는 것입니다:estimator.processIMU()에서 사전 적분 estimator.setReloFrame() 설정 재배치 프레임 estimator.processimage () 처리 이미지 프레임: 초기화, 결합된 비선형 최적화
주요 함수 소개
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
...
}
imu와 이미지 데이터를 정렬하고 조합하면 (IMUs, img msg), 즉 이미지에 대응하는 모든 imu 데이터를 용기vector에 넣습니다.IMU와 이미지 프레임의 대응 관계는 새 버전의 코드에서 변경됩니다: 이미지 프레임 j에 대해 매번 imubuf의 모든 시간 스탬프가 그보다 작은imumsg 및 첫 번째 타임 스탬프가 이미지 타임 스탬프보다 큰 imumsg (동기화 시간이 존재하는 지연 td를 추가해야 합니다.)