vins 학습 코드 논리

17225 단어 vins-mono
참조 블로그 링크 @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를 추가해야 합니다.)

    좋은 웹페이지 즐겨찾기