Add feedback during monocular initialisation to diagnose initialisation status

[martinakos]

What issue is the feature request related to?

Add feature tracks during monocular initialisation as feedback to diagnose initialisation issues.

Describe the solution you'd like

As I describe in this discussion #557
I'm experiencing very long monocular initialisation compared with ORBSLAM2 in the same conditions.
I would like to see the same feature tracks that ORBSLAM2 shows during monocular initialisation to discard that the long initialisation is due to bad feature tracking. stella_vslam doesn't show these feature tracks during monocular initialisation so I don't know if the issue is bad feature tracking or how these tracks are used to initialise the map.

[ymd-stella]

I don't think the addition of the drawing feature will solve that problem. what you really need is a status indicator.

You can implement the following. (The related code is attached.)

• Get the reason why initializer returns false

  • stella_vslam/src/stella_vslam/initialize/base.cc

    Lines 64 to 86 in 1cf42b9

    	// reject if the number of valid points does not fulfill the threshold
    	if (*max_num_valid_pts_iter < min_num_valid_pts_) {
    	return false;
    	}
    	// reject if most plausible hypothesis is unclear
    	const auto num_similars = std::count_if(nums_valid_pts.begin(), nums_valid_pts.end(),
    	[max_num_valid_pts_iter](unsigned int num_valid_pts) {
    	return 0.8 * (*max_num_valid_pts_iter) < num_valid_pts;
    	});
    	if (1 < num_similars) {
    	return false;
    	}
    	// reject if the parallax is too small
    	if (init_parallax.at(max_num_valid_index) > std::cos(parallax_deg_thr_ / 180.0 * M_PI)) {
    	return false;
    	}
    	// reject if the number of 3D points does not fulfill the threshold
    	if (num_triangulated_pts.at(max_num_valid_index) < min_num_triangulated_) {
    	return false;
    	}

• Set it to frame_publisher

  • stella_vslam/src/stella_vslam/publish/frame_publisher.cc

    Lines 148 to 163 in 1cf42b9

    	void frame_publisher::update(const std::vector<std::shared_ptr<data::landmark>>& curr_lms,
    	bool mapping_is_enabled,
    	tracker_state_t tracking_state,
    	std::vector<cv::KeyPoint>& keypts,
    	const cv::Mat& img,
    	double elapsed_ms) {
    	std::lock_guard<std::mutex> lock(mtx_);
    	img.copyTo(img_);
    	curr_keypts_ = keypts;
    	elapsed_ms_ = elapsed_ms;
    	mapping_is_enabled_ = mapping_is_enabled;
    	tracking_state_ = tracking_state;
    	curr_lms_ = curr_lms;
    	}

• Get it and display it as status in viewer
  • https://github.com/stella-cv/iridescence_viewer/blob/3dc801bc284cf0e54b806ac8e1887dffe90620db/src/viewer.cc#L62-L63