참조 블로그 링크 @Manii

문서 목록

1,ros의 논리 구조

1、std_msgs/Header

2、sensor_msgs::ImageConstPtr

3、sensor_msgs::PointCloudPtr featur_points

sensor_msgs::PointCloud msg_match_points

4、sensor_msgs::ImuConstptr

코드의 조합 구조

2. 시각 추적featuretrackers

앞말

코드 논리

img_calback

FeatureTracker::readImage()이미지 데이터를 읽고 처리

셋째, 상태 평가기 프로세스

주요 내용

세부 단계

주 스레드voidprocess()

주요 함수 소개

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의 비주얼 프로세싱 전면에 있는 비주얼 추적 모듈에 대해 살펴보겠습니다. 주요 내용은 다음과 같습니다.

매 프레임의 새로운 이미지에 대해 KLT 희소 광류 알고리즘은 기존 특징을 추적한다.

새로운 발점 특징을 검측하여 각 이미지 특징의 최소 수(100-300)를 확보한다.

두 인접 특징 사이의 픽셀의 최소 간격을 설정하여 고른 특징 분포를 집행한다.

기본 매트릭스 모델의 RANSAC 알고리즘을 이용하여 외점 제거를 한다.

특징점에 대해 기변 교정을 한 다음에 한 단위의 구면에 투영한다.키프레임 선택은 나중에 논의합니다.

코드 논리

기본 프로그램

logger의 레벨 설정하기;

readParameters();

가장자리 소음을 제거하기 위해 어안mask를 넣을지;

MAGE 가입TOPIC 실행 imgcalback;

TOPIC(추적의 특징점, 추적의 특징점도, 복위신호)를 발표한다.

ros::sqin().

img_calback

첫 번째 프레임 First 인지 판단image_flag;

판단 시간 간격(1초 이상이고 현재 프레임의 시간은 이전 프레임보다 작다) 문제가 있으면restart;

시간 간격 내의 발표 횟수를 통해 발표 주파수 제어를 한다.

FeatureTracker::readImage()가 이미지 데이터를 읽고 처리한다.

글로벌 ID 업데이트

PUBTHIS_FRAME=1은 정보를 캡슐화하고 게시합니다.

FeatureTracker::readImage() 이미지 데이터 읽기 처리

빛이 너무 밝거나 너무 어두우면 1로 직사각형의 균형화를 한다.

이전 프레임 피쳐점curpts LK 피라미드 광류 추적 진행,forw 획득pts;

status에 따라 추적 실패와 이미지의 편리한 외점을 제거한다.

기본 매트릭스를 통해 외점(RANSCA)을 제거한다. 즉, RANSCA로 외점을 제거한다.(14강 P148);

추적점의 순서를 정하고 순서대로 선점하고mask를 사용하여 유사 비대대 억제, 반경 30을 하고 밀집점을 제거하여 특징점의 분포를 고르게 한다.

새로운 특징을 찾아 보완

새로운 특징점에 대해 기변 교정과 깊이 귀일화를 하고 각 각점의 속도를 계산한다.

3. 상태 평가기 절차

주요 내용

vins 추정기의 초기화

슬라이딩 창의 비선형 최적화를 바탕으로 긴밀한 결합을 실현한다.

관건 프레임의 선택(평균 시차법, 추적 품질법);

외참표정, 가시화 등.

입력:

IMU의 교양도와 선 가속도,'/imu0'

구독

이미지 추적의 특징점, "/feature tracker/feature"

구독

리셋 신호, "/feature tracker/restart"

구독

재배치된 일치점, "/pose graph/match points"

출력:

스레드voidprocess()에서 RVIZ에 마일리지 정보 PQV, 키포인트 3차원 좌표, 카메라 위치 자세, 점 구름 정보, IMU에서 카메라로의 외부 참조, 위치 재지정 자세 등을 전송한다.

콜백 함수void imucallback(const sensor msgs::ImuConstPtr &imu msg)은 IMU에서 직접 추출한 최신 PQV를 발표합니다.

구체적 절차

1,vinsestimator/estimator_node.cpp 1.
다중 스레드 공유, 상호 배척 잠금, 조건 잠금 설정

measurements

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를 추가해야 합니다.)