SLAM Front-end with example code

Figure1. Graph-based SLAM's Front-End and Back-End

SLAM 공부할때 백날 논문을 읽으면서 이해도 되지 않는 내용을 머리에 때려박는것에 회의감을 느껴 실제 코드를 돌려보고 코드리뷰를 해보려합니다. SLAM이 Kalman Filter, Particle Filter을 거쳐 최근 Graph-based SLAM이 트렌드가 되면서 Front-end와 Back-end로 나뉘게 되었는데 Front-end는 Odometry Prediction, Backend는 Optimization을 담당합니다. 

• Front-end: The process of determining of a robot by analyzing the associated sensor measurement
• Back-end: The process of registering pose graph and graph optimization.

만약 목표가 VO(Visual Odometry)라면 카메라를 이용해 로봇의 pose, orientation을 front-end 에서 추정하고 VIO(Visual-Inertial Odometry)라면 관성을 읽을 수 있는 센서가 추가로 필요하겠죠.

 

SLAM Front-end

GitHub - ut-amrl/vision_slam_frontend: SLAM frontend for a ground robot using vision and odometry

현재 보고있는 예제 코드를 기준으로 SLAM Front-end가 어떤식으로 구현되는지 보도록 하겠습니다. 위의 링크는 제가 보고있는 코드입니다. 이런 코드를 보기 위해선 실제 연산 뿐 아니라 ros와 같은 라이브러리 코드들도 어느정도는 이해를 해야 읽히기 때문에 조금 더 번거로운것 같습니다..

 

DEFINE_string(left_image_topic, "stereo/left/image_raw/compressed", "Name of topic for left cam");
DEFINE_string(right_image_topic, "stereo/left/image_raw/compressed", "Name of topic for right cam");
DEFINE_string(odom_topic, "/odometry/filtered", "Name of topic for odometry");

void ProcessBagfile(const char* filename, ros::NodeHandle*n){
    // ...

    bag.open(filename, rosbag::bagmode::Read);

    vector<string> topics;
    topics.push_back(FLAGS_left_image_topic.c_str());
    topics.push_back(FLAGS_right_image_topic.c_str());
    topics.push_back(FLAGS_odom_topic.c_str());
    rosbag::View view(bag,rosbag::TopicQuery(topics));

    // ...
}

 

Figure2. Part of topics printed by rosbag info

우선 input 파일인 bagfile을 읽어야합니다. 우선 필요한 topic들의 이름을 topics 벡터에 넣고 view class instance를 생성해줍니다. 이 view라는 녀석은 MassageInstance를 가리키는 Iterator입니다. 즉 우리가 필요한 topic을 고르고 topic의 Message를 view를 통해서 꺼내본다고 생각하면 될 것 같습니다.

 

double bag_t_start = -1;
for(rosbag::View::iterator it = view.begin(); ros::ok() && it != view.end() && max_poses_processed; ++it) {
    const rosbag::MessageInstance& message = *it;
    if(bag_t_start < 0.0) {
    	bag_t_start = message.getTime().toSec();
    }
    // process image messages
    {
        sensor_msgs::CompressedImagePtr image_msg = message.instantiate<sensor_msgs::CompressedImage>();
        if(image_msg){
        	// get stereo images...
        	new_pose_added = CompressedImageCallback(curr_stereo_image_pair, &slam_frontend);
    	}
    	if(new_pose_added == true){
    		// publish stereo images
    	}
    }
    // process odometry messages
    {
        nav_msgs::OdometryPtr odom_msg = message.instantiate<nav_msgs::Odometry>();
        if(odom_msg){
        	OdometryCallback(*odom_msg, &slam_frontend);
        }
    }
    // do something with SLAM..?
    if(new_pose_added == true){
        slam_frontend.GetSLAMProblem(&problem);
        PublishVisualization( /* publishers... */);
        ros::spinOnce();
    }
}

ros message parsing 하는 부분을 아주 간략하게 요약한 코드입니다. 반복문을 돌면서 msg의 데이터를 이용해 필요한 연산을 하게됩니다. 하나 짚고 넘어갈 것은 각 topic마다 message의 개수가 다릅니다. Topic은 비동기적 통신 채널이기 때문에 매 timestamp(?)에 모든 message가 대응되는것이 아닙니다. 위 예시에선 msg를 까봤는데 image 데이터가 없을수도 있고 odometry 데이터가 없을 수도 있고 둘다 없을 수도 있는거죠.

 

코드에서 사용하는 정보는 stereo image와 odometry data가 있는데 자세히 들어가기 전에 어떤 연산을 하는지 추측을 해볼 수 있습니다. 

1. Image data에서 keypoint 추출, 각 point들의 3d point 계산. 계산된 point들은 map에 등록. 어쩌면 각각의 frame을 통해 Odometry까지 계산.
2. Odometry data는 그대로 이용?

일단 Odometry msg가 없었다면 stereo 이미지를 이용해 localization, mapping 까지 전부 한다고 생각하겠지만 Odometry 데이터가 어떻게 구현되었는지는 더 자세히 봐야할 것 같습니다.

---

위에서 main 프로그램이 크게 어떻게 돌아가는지 훑어봤습니다. 그럼 실제로 데이터를 어떤 방식으로 처리해 SLAM Front-end를 처리하는지 보도록 하겠습니다

Observing Image

bool Frontend::ObserveImage(const cv::Mat& left_image, cv::Mat& right_image, double time) {
    Frame curr_frame, right_temp_frame;
    ExtractFeatures(left_image, &curr_frame);
    ExtractFeatures(right_frame, &right_temp_frame);

    std::vector<cv::DMatch> stereo_matches = GetMatches(curr_frame, right_temp_frame, config_.nn_match_ratio_);
    RemoveAmbigStereo(&curr_frame, &right_temp_frame, stereo_matches);

    for(Frame& past_frame : frame_list_){
        VisionFactor* matches;
        matches = GetFeatureMatches(&past_frame, &curr_frame);

        vision_factors_.push_back(*matches);
        free(matches);
    }

    // Calculate the depths of the points
    vector<Vector3f> points;
    Calculate3DPoints(&curr_frame, &right_temp_frame, &points);
    vector<VisionFeature> features;
    for(uint64_t i = 0; i < curr_frame.keypoints_.size(); i++){
    	features.push_back(VisionFeature(i, OpenCVToEigen(curr_frame.keypoints_[i].pt), points[i]));
    }
    UndistortFeaturePoints(&features);
    const Vector3f loc = init_odom_rotation_.inverse()*(odom_translation_ - init_odom_translation_);
    const Quaternionf angle = odom_rotation_ * init_odom_rotation_.inverse();
    nodes_.push_back(SLAMNode(curr_frame_ID_, odom_timestamp_, RobotPose(loc, angle), features));
    AddOdometryFactor();
}

이미지 처리하는 함수의 코드 일부입니다. 우선 예제 코드는 스테레오 이미지를 처리하기 때문에 left_image, right_image 그리고 time을 파라미터로 받습니다. 위 함수에서 가장 처음 수행하는 것은 ExtractFeatures 입니다.

 

ExtractFeatures

Figure3. Example of Keypoint

ExtractFeatures 에선 이미지에서 keypoint(특징점) 와 descriptor(특징량)를 추출해줍니다. keypoint란 이미지에서 특징이 되는 점을 의미합니다. Conventional Computer Vision에서 keypoint를 찾는것은 Corner Detection을 한다는 의미입니다. 이미지에서 object 나 camera의 state 또는 조명과 같은 조건들이 변해도 식별이 쉽게 가능한 점이 바로 corner point이기 때문입니다.  Descriptor는 keypoint의 위치에서 해당되는 keypoint를 나타내기 위해 visual feature를 인코딩한 정보입니다. 

 

Keypoint를 추출하는 방법과 descriptor 연산을 위해선 여러 알고리즘이 있는데 예제 코드에서는 keypoint detection으로 FAST, descriptor는 AKAZE, ORB, BRISK, 등 옵션으로 선택할 수 있게 되어있는데 default는 AKAZE입니다. 

 

추출한 keypoint, descriptor를 이용해 Frame 이라는 데이터를 생성합니다.

vector<cv::DMatch> Frontend::GetMatches(const Frame& frame_query, const Frame& frame_train, double nn_match_ratio){
    vector<vector<cv::DMatch>> matches;
    matcher_->knnMatch(frame_query.descriptors_, frame_train.descriptors_, fmatches, 2);

    vector<cv::DMatch> best_matches;
    for(size_t i = 0; i < matches.size(); i++){
        cv::DMatch first = matches[i][0];
        float dist1 = matches[i][0].distance;
        float dist2 = matches[i][1].distance;
        if(dist <nn_match_ratio * dist2){
        	best_matches.push_back(first);
        }
    }
    return best_matches;
}

 

이렇게 뽑은 descriptor를 이용해 feature matching을 하게됩니다. Feature matching이 필요한 이유는 두가지인데 하나는 left, right 이미지간의 feature matching, 다른 하나는 이전 frame과 현재 frame간의 matching입니다.

 

feature matching을 위한 알고리즘은 KNN(K-Nearest Neighbor) 알고리즘을 사용하고 있습니다. knn이 어떻게 돌아가는지 정확하게는 모르겠지만 knnMatch를 돌리게 되면 frame_query의 descriptor와 가장 유사한 descriptor를 frame_train에서 k개를 뽑아주는것 같습니다. 그 중 가장 distance가 짧은 descriptor 쌍을 골라줍니다.

 

실행해보면 12000개 가량의 descriptor중에서 4000개쯤의 descriptor쌍이 선택됩니다. 결국 Frame에는 선택된 descriptor쌍, 그리고 대응하는 keypoint를 저장합니다.

 

Current, past frame간의 matching에선 한가지 정보를 더 저장하는데, 해당 feature가 언제 처음 보였는지, feature가 처음 추출된 inital frame ID를 저장해줍니다. Current, Past frame matching을 하기 위해서는 일정 개수의 frame을 저장하고 있어야하는데 예제 코드에서는 10개의 frame을 들고 있습니다. 이전 frame들과의 matching이 끝나면 SLAM frontend에 Vision Factor라는 데이터 구조로 정보를 넘겨주게 됩니다. 아래와 같이 현재 frame의 feature들과 처음 발견된 initial frame Id, 그리고 현재 frame Id를 저장합니다.

Figure4. Block Diagram of Example code

Triangulations

Figure5. Concept of Triangulation

이미지에서 keypoint를 뽑는 이유는 map을 만들기 위해서도 있지만 2D point에서 3D point를 복원하고 3D point와의 relative pose를 계산해 Localization을 하려는 이유도 있습니다. 이 일련의 과정을 Triangulation 이라고 하는데 왼쪽 카메라와 오른쪽 카메라의 disparity(시차)를 이용합니다.

 

void Frontend::Calculate3DPoints(Frame* left_frame, Frame* right_frame, vector<Vector3f>* points){
    std::vector<cv::Point2f> left_points;
    std::vector<cv::Point2f> right_points;
    VisionFactor* matches;

    float best_percent = config_.best_percent_;
    config_.best_percent_ = 1.0;
    matches = GetFeatureMatches(right_frame, left_frame);
    config_.best_percent_ = best_percent;

    for(FeatureMatch match : matches->feature_matches){
        const auto& left_pt = left_frame->keypoints_[match.feature_idx_current].pt;
        cosnt auto& right_pt = right_frame->keypoints_[match.feature_idx_initial].pt;
        right_points.push_back(right_pt);
        left_points.push_back(left_pt);
    }
    cv::Mat triangulated_points;
    cv::triangulatePoints(config_.projection_left, config_.projection_right,
    left_points, righ_points,
    triangulated_points);

    for(int64_t c = 0; c < triangulated_points.cols; ++c){
        cv::Mat col = triangulated_points.col(c);
        points->push_back(
            Vector3f(cof.at<float>(0,0),
            col.at<float>(1,0),
            col.at<float>(2,0))/col.at<float>(3,0));
        );
    }

    free(matches);
}

Stereo image에서 구해진 keypoint들에 대해 triangulation 연산을 통해 3D point를 계산합니다. Opencv에서 제공하는 function의 파라미터를 잠깐 보면 left/right projection matrix, keypoints, 그리고 output을 저장하는 벡터입니다. 여기서 말하는 projection matrix는 intrinsic matrix와 extrinsic matrix를 곱한 matrix입니다. Intrinsic matrix란 카메라 파라미터로 고정된 값을 갖습니다. Extrinsic parameter는 카메라의 rotation, translation 을 표현하기 때문에 매 프레임마다 값이 변합니다.  Intrinsic parameter, extrinsic parameter의 곱인 projection matrix를 이용하면 3D point의 점을 이미지로사영할 수 있습니다.

 

config_.projection_left을 찍어봤을때 값이 바뀌지 않는걸 보니 intrinsic matrix가 잘못 들어가고 있는줄 알았는데 생각해보니 카메라가 움직이더라도 카메라 초점을 원점으로 두고 projection 하기 때문인 것 같습니다.

 

3D point가 구해지면 VisionFeature라는 데이터구조로 저장되는데 이는 Undistortion에 사용됩니다. 카메라로 사진을 찍게되면 이미지에 왜곡이 생기기 때문에 왜곡된 부분을 보정해주는데 여기서 3d point가 구해지기 전에 이미지를 먼저 undistort 했어야 하는것이 아닌가하는 의문이 있습니다. 현재는 1) (3d point 추출)->(점들의 왜곡보정) 순서로 구현되어 있는데 2) (이미지 왜곡보정)->(3d point 추출) 순서가 좀더 정확하지 않을까합니다. 어쩌면 calculation cost 차이가 있을지도 모르겠습니다. 

 

odometry의 경우 msg에서 받은 odometry 값을 그대로 이용하기 때문에 따로 처리하는 프로세스가 없네요...

Figure6. Trajectory printed by rviz

위 그림은 rviz로 그린 trajectory와 주변 map입니다. 사실 poseGraph가 행렬로 어떻게 만들어지는지 코드로 구현되어있는 것을 기대했는데 원래 그렇게 접근을 하지 않는지 그런 부분은 없습니다. 이 예제 코드에 Backend를 붙여보는것을 1차 목표로 해야겠습니다.