Marker integration

[ymd-stella]

What issue is the feature request related to?

The application of monocular Visual SLAM is limited because the scale is not known. Marker integration is an inexpensive solution to this problem without the need for additional cameras. This feature is inspired by UcoSLAM.

In addition, by applying marker integration to an equirectangular model, robust Visual SLAM with scale can be realized.

Describe the solution you'd like

See draft.

How to achieve this

• Create the first available release (Marker integration #241)
• Update the marker position during loop closing.
• Update docs (Installation, Overview, Tutorial)
• Save/Load marker data
• Visualize marker
• Detect loop by marker

Additional context

[mamo3gr]

I'm interested in scale-specified VSLAM and this feature. I have read #241 and wonder how to employ the scale information, that is, the physical size of the ArUco marker. I think marker_model::aruco::width_ would be used to determine the scale of the camera's trajectory (translation vector) or make some kind of constraints, but it seems not to appear in codes. Would you mind pointing out the corresponding section?

Thank you for your cooperation.

[ymd-stella]

In the first attempt, distance constraints was added between the corners of the markers, but adding such constraints was computationally inefficient. So instead, the corners of the markers are now fixed. This determines the scale.
(When the coordinates of the reference keyframe change, the position of the marker should change, but it does not. This is a bug. I will fix it soon. -> #311)

1. Calculates the relative coordinates of corners.

stella_vslam/src/stella_vslam/marker_model/base.cc

Lines 15 to 18 in 3d178c0

	corners_pos_.at(0) << -width / 2.0, width / 2.0, 0.0;
	corners_pos_.at(1) << width / 2.0, width / 2.0, 0.0;
	corners_pos_.at(2) << width / 2.0, -width / 2.0, 0.0;
	corners_pos_.at(3) << -width / 2.0, -width / 2.0, 0.0;

2. Calculate the relative coordinates between the camera and the marker by referring to the relative coordinates of the corners.

stella_vslam/src/stella_vslam/marker_detector/base.cc

Line 42 in 3d178c0

double reproj_error = solve::pnp_solver::compute_pose(bearings, marker_model_->corners_pos_, rot_cm, trans_cm, num_iter_);

3. Calculates the position of the corners on the map.

stella_vslam/src/stella_vslam/module/initializer.cc

Line 250 in c7feabd

eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(keyfrm->get_cam_pose_inv(), mkr2d.marker_model_->corners_pos_);

stella_vslam/src/stella_vslam/module/keyframe_inserter.cc

Line 111 in c7feabd

eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(keyfrm->get_cam_pose_inv(), mkr2d.marker_model_->corners_pos_);

stella_vslam/src/stella_vslam/data/marker2d.cc

Lines 10 to 18 in 3d178c0

	eigen_alloc_vector<Vec3_t> marker2d::compute_corners_pos_w(const Mat44_t& cam_pose_wc, const eigen_alloc_vector<Vec3_t>& corners_pos) const {
	eigen_alloc_vector<Vec3_t> corners_pos_w;
	for (const Vec3_t& corner_pos : corners_pos) {
	const Mat33_t rot_wc = cam_pose_wc.block<3, 3>(0, 0);
	const Vec3_t trans_wc = cam_pose_wc.block<3, 1>(0, 3);
	corners_pos_w.push_back(rot_wc * (rot_cm_ * corner_pos + trans_cm_) + trans_wc);
	}
	return corners_pos_w;
	}

4. Optimization is performed by reprojection edges.

stella_vslam/src/stella_vslam/optimize/internal/marker_vertex_container.h

Line 66 in 3d178c0

vertices.push_back(create_vertex(mkr->id_, i, mkr->corners_pos_w_[i], is_constant));

stella_vslam/src/stella_vslam/optimize/global_bundle_adjuster.cc

Lines 129 to 164 in c7feabd

	// Connect marker vertices
	for (unsigned int marker_idx = 0; marker_idx < markers.size(); ++marker_idx) {
	auto mkr = markers.at(marker_idx);
	if (!mkr) {
	continue;
	}
	// Convert the corners to the g2o vertex, then set it to the optimizer
	auto corner_vertices = marker_vtx_container.create_vertices(mkr, true);
	for (unsigned int corner_idx = 0; corner_idx < corner_vertices.size(); ++corner_idx) {
	const auto corner_vtx = corner_vertices[corner_idx];
	optimizer.addVertex(corner_vtx);
	for (const auto& keyfrm : mkr->observations_) {
	if (!keyfrm) {
	continue;
	}
	if (keyfrm->will_be_erased()) {
	continue;
	}
	if (!keyfrm_vtx_container.contain(keyfrm)) {
	continue;
	}
	const auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm);
	const auto& mkr_2d = keyfrm->markers_2d_.at(mkr->id_);
	const auto& undist_pt = mkr_2d.undist_corners_.at(corner_idx);
	const float x_right = -1.0;
	const float inv_sigma_sq = 1.0;
	auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, nullptr, corner_vtx,
	0, undist_pt.x, undist_pt.y, x_right,
	inv_sigma_sq, 0.0, false);
	reproj_edge_wraps.push_back(reproj_edge_wrap);
	optimizer.addEdge(reproj_edge_wrap.edge_);
	}
	}
	}

stella_vslam/src/stella_vslam/optimize/local_bundle_adjuster.cc

Lines 213 to 252 in 3d178c0

	// Container of the reprojection edges for corners of markers
	internal::marker_vertex_container marker_vtx_container(vtx_id_offset, local_mkrs.size());
	std::vector<reproj_edge_wrapper> mkr_reproj_edge_wraps;
	mkr_reproj_edge_wraps.reserve(all_keyfrms.size() * local_mkrs.size());
	for (auto& id_local_mkr_pair : local_mkrs) {
	auto mkr = id_local_mkr_pair.second;
	if (!mkr) {
	continue;
	}
	// Convert the corners to the g2o vertex, then set it to the optimizer
	auto corner_vertices = marker_vtx_container.create_vertices(mkr, true);
	for (unsigned int corner_idx = 0; corner_idx < corner_vertices.size(); ++corner_idx) {
	const auto corner_vtx = corner_vertices[corner_idx];
	optimizer.addVertex(corner_vtx);
	for (const auto& keyfrm : mkr->observations_) {
	if (!keyfrm) {
	continue;
	}
	if (keyfrm->will_be_erased()) {
	continue;
	}
	if (!keyfrm_vtx_container.contain(keyfrm)) {
	continue;
	}
	const auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm);
	const auto& mkr_2d = keyfrm->markers_2d_.at(mkr->id_);
	const auto& undist_pt = mkr_2d.undist_corners_.at(corner_idx);
	const float x_right = -1.0;
	const float inv_sigma_sq = 1.0;
	auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, nullptr, corner_vtx,
	0, undist_pt.x, undist_pt.y, x_right,
	inv_sigma_sq, 0.0, false);
	mkr_reproj_edge_wraps.push_back(reproj_edge_wrap);
	optimizer.addEdge(reproj_edge_wrap.edge_);
	}
	}
	}

[mamo3gr]

Thank you very much for your rapid response and detailed description.

I understand that the scale information is employed as below:

• Assume the corners of the markers are in the marker coordinate, where the physical length of the markers is employed
  • I'm sorry to miss that the marker_model::base constructor employs a marker's width
• Transform the marker coordinate into the camera coordinate and finally the world coordinate
• Fix the corners of the markers in the world coordinate and optimize the other parameters (camera poses) with reprojection error through global/local bundle adjustment

By the way, without the size-known marker, is there any rule to determine the unit length of the estimated trajectory in this software? For example, I have involved a Structure-from-Motion software, and they used a rule that the translation vector between the first and second camera is a unit vector.

[ymd-stella]

See

stella_vslam/src/stella_vslam/module/initializer.cc

Lines 267 to 277 in f5cbbf5

	if (indefinite_scale) {
	// scale the map so that the median of depths is 1.0
	const auto median_depth = init_keyfrm->compute_median_depth(init_keyfrm->camera_->model_type_ == camera::model_type_t::Equirectangular);
	const auto inv_median_depth = 1.0 / median_depth;
	if (curr_keyfrm->get_num_tracked_landmarks(1) < min_num_triangulated_ && median_depth < 0) {
	spdlog::info("seems to be wrong initialization, resetting");
	state_ = initializer_state_t::Wrong;
	return false;
	}
	scale_map(init_keyfrm, curr_keyfrm, inv_median_depth * scaling_factor_);
	}

[mamo3gr]

scale the map so that the median of depths is 1.0

Thank you again and sorry for my laziness!

[ymd-stella]

When the coordinates of the reference keyframe change, the position of the marker should change, but it does not. This is a bug. I will fix it soon. -> #311

Reverted due to incorrect update method; multiple updates were made to a marker. Additionally, if the marker is fixed during optimization, their positions should not be updated after optimization. (In particular, updating after local BA makes the mapping unstable.)

During loop closing, the marker position needs to be updated.

The plan is as follows:

• Retain the keyframe used to create the marker as a reference keyframe of the marker
• Update the marker pose when the reference keyframe pose is updated.

[AdonaiVera]

Hi @ymd-stella
I want to ask you about getting the marker in the world coordinate. I'm working on Python using the msg file.
I use an algorithm to detect the markers, and now I have the X, and Y, and I need to project these points to the world coordinate.
My first step is to go from point to bearing.

Vec3_t equirectangular::convert_point_to_bearing(const cv::Point2f& undist_pt) const { // "From Google Street View to 3D City Models (ICCVW 2009)." // convert to unit polar coordinates const double lon = (undist_pt.x / cols_ - 0.5) * (2.0 * M_PI); const double lat = -(undist_pt.y / rows_ - 0.5) * M_PI; // convert to equirectangular coordinates return Vec3_t{std::cos(lat) * std::sin(lon), -std::sin(lat), std::cos(lat) * std::cos(lon)}; }

And then I'm unsure how to go from bearing to the world coordinate. I used ten 3d points of the same images; I made a projection using the bearing and the 3d point in world coordinates. ( Rigidly (+scale) aligns two point clouds with known point-to-point correspondences with least-squares error. ) I get a transformation and rotation matrix; then, I transform my marker points.

However, the results are inaccurate; how can I go from bearing to the world coordinate if I'm in Python? I have the trans_wc rot_wc, trans_cw and rot_cw

Thank you in Advance !

[AmineDh98]

Hello @ymd-stella, I noticed that the marker integration was reverted due to some issues with the update method. I would like to know if the main branch of the library currently contains the marker integration and if it is working well. If not, could you please provide some guidance on what needs to be added or changed to align with the UCOSlam algorithm?

[ymd-stella]

@AmineDh98
The pose of marker is not updated from the first detected pose, but it is working. It is used for bundle adjustment and to estimate scale when using a monocular camera.

Please set the MarkerModel parameter to enable detection.
https://stella-cv.readthedocs.io/en/latest/parameters.html#markermodel

[AmineDh98]

Hi @ymd-stella , Thank you for your previous answer.
Now I would like to request the addition of markers and their positions to the message file generated by the map_database_io_msgpack module in the stella_vslam library. Currently, the message file contains information about keyframes and landmarks, but I would like to extend it to include markers as well.
I suggest the below snippet:

// Inside map_database::to_json function
// Save each marker position as json
std::map<std::string, nlohmann::json> markers;
for (const auto& id_marker : markers_) {
    const auto id = id_marker.first;
    const auto& marker = id_marker.second;
    assert(marker);
    assert(id == marker->id_);
    assert(!marker->will_be_erased());
    assert(!markers.count(std::to_string(id)));
    markers[std::to_string(id)] = marker->to_json();
}
json_markers = markers;

// Inside map_database::from_json function
// Parse and load marker data from JSON
const auto json_markers = json.at("markers");
for (const auto& json_marker : json_markers) {
    const auto id = std::stoi(json_marker.at("id").get<std::string>());
    // Parse and create marker object from JSON
    // ...
    markers_[id] = std::make_shared<Marker>(/* marker parameters */);
}

To incorporate markers into the existing message file format, I propose adding a new JSON object or array within the existing JSON structure. The map_database::to_json function can iterate over the markers and store their positions in this new JSON object or array, similar to how keyframes and landmarks are currently handled.

For the map_database::from_json function, you would need to update the parsing logic to correctly extract and create marker objects from the JSON representation.

I believe this addition would enhance the functionality of the stella_vslam library and provide valuable information about markers' positions in the VSLAM frame, enabling further analysis and visualization.

Another suggestion could be, as I am using ROS.
The Aruco markers can be also published using a ROS topic! Do you have any hint ?

I would greatly appreciate any guidance or suggestions on how to implement this feature effectively and maintain compatibility with the existing codebase.

[ymd-stella]

No particular problem is found from the snippet. Pull requests are welcome. Saved maps will no longer be compatible, but I think it is sufficient to create conversion scripts if needed.

Another suggestion could be, as I am using ROS.
The Aruco markers can be also published using a ROS topic! Do you have any hint ?

I had never thought about it. Is there any benefit to addressing it? I do not intend to work on it, as I am not currently inconvenienced.

[AmineDh98]

Hi @ymd-stella ,

When I used the marker integration the result in the socket viewer visualizer is bad and the problem seems to be the scale (the keyframes are too far from each other and the camera is moving very fast)
I assume that I am not giving the correct width of the marker.
My real marker is 4 meters width
I have in the yml file as below:
'''
MarkerModel:
type: "aruco"
width: 4
marker_size: 4
max_markers: 50
'''

so what should I write in the yml file ? What is the physical unit used to read the yml file ? Is it in meter, cm, mm ... ?
Thank you

[ymd-stella]

This library is not dependent on a specific system of units. As long as the settings do not contradict each other, there is no problem. However, the visualizers are tuned in meter, so it is recommended to use meter.

[aschnerring]

Hello @ymd-stella,

I'm currently working with @AmineDh98 on the marker integration. We are interested in getting the marker position in high precision in order to transform the point cloud and camera trajectory to a coordinate system in which the marker positions are known.

In the following I would like to describe the results of the discussions I had with @AmineDh98 and ask you (or someone else who is familiar with the code) to confirm if this is valid. Note that the hints I provide are not intended for @ymd-stella or other developers familiar to the code as I'm sure you know all of this already as you coded it. It's intended to be sure that we understood correctly and to enable other users to understand better the code involved in this issue.

Approach as intended by @ymd-stella

Aruco marker detection is already implemented in stella_vslam. However, there seem to be some false assumptions and/or bugs in the implementation.

Current appraoch to computing the marker positions in the world coordinate system

In the current implementation, the marker positions in the world frame are computed in the following way:

1. Set the marker corner positions in the marker coordinate system (stella_vslam/src/stella_vslam/marker_model/base.cc)
2. Estimate the relative coordinates between camera and marker (stella_vslam/src/stella_vslam/marker_detector/base.cc). This involves:
   1. Detection of 2d markers in the image
   2. Estimate marker pose w.r.t. camera (i.e. rot_cm and trans_cm) using a PNP solver
3. Compute marker position in world coordinate system (WorldCS<-CameraCS<-MarkerCS) using compute_corners_pos_w in stella_vslam/src/stella_vslam/data/marker2d.cc. This is used during
   1. Initialization: stella_vslam/src/stella_vslam/module/keyframe_inserter.cc
   2. Whenever a new keyframe is inserted: stella_vslam/src/stella_vslam/module/keyframe_inserter.cc in the method create_new_keyframe()

The idea (which is not yet implemented) is then to store exactly one reference keyframe for each marker (i.e. the first keyframe by which this marker was seen by). Then the marker position in the WorldCS is updated during global bundle adjustment whenever this reference keyframe is changed. This approach allows to determine the correct scale of the map, as the marker dimension is set in its own CS and only transformed. Then, the scale information is employed implicitly during local bundle adjustment, where the landmarks and keyframes are adjusted in such a way that the marker corner positions stay fixed in the WorldCS.

Problems with this approach

Noisy initial marker pose estimation

However, a problem of this approach is that it relies on the initial PNP estimation of the marker pose w.r.t. the camera, which is never changed after it is computed upon the first marker detection. This pose estimation is potentially noisy and even if the reference keyframe poses are correct, the marker positions in the WorldCS might still be wrong as their computation assume a noisy initial PNP estimate.

Fixing several markers during local bundle adjustment (not sure about this)

Another potential problem with this approach is the presence of several markers:

Note that all marker positions are fixed during the local bundle adjustment. See the create_vertex method for the marker vertex called in stella_vslam/src/stella_vslam/optimize/local_bundle_adjuster_g2o.cc with is_constant=true to confirm this. Also note that is_constant=true is used in stella_vslam/src/stella_vslam/optimize/internal/marker_vertx_container.h, where it is provided to the vertex, setting it as fixed.

Given that all initial estimates for the markers in world coordinates are potentially noisy (due to the aforementioned PNP solver and the issue that the marker->camera transform is never adjusted), this might result in inconsistent/noisy map information during the local bundle adjustment. Even when the marker positions are updated during a global bundle adjustment, the noise of the initial PNP marker pose estimation will still be present and contribute to a suboptimal local bundle adjustment.

Suggested alternative approach

We instead suggest to include the markers in the local bundle adjustment and treat them just like any other landmark (except that we can identify reference points in the VSLAM point cloud which allow us to transform it and the camera trajectory ). Note that the approach of including the marker corners in the local bundle adjustment is also the approach that was chosen in UcoSlam (except that in UcoSlam, more weight is given to the marker positions, depending on the number of valid markers).

This comes with two advantages:

• The marker position is subject to optimizations where multiple keyframes see the same marker, most likely resulting in better precision compared to relying on a noisy initial PNP pose estimation based on only one frame
• We don't depend on a global bundle adjustment to gain precise marker positions (although global bundle adjustment will increase marker position precision)

It also comes with two disadvantages:

• The corner positions of one marker may not be consistent (i.e. not be transformable via Sim3 into co-planar points on square edges)
• The scale information will not be employed perfectly

We accept these disadvantages for the following reason:

For us, the markers are a tool to transform the camera trajectory and point cloud in the VSLAM coordinate system to a coordinate system in which the marker position is exactly known. We compute this Sim3 transform using the Kabsch algorithm and can hence tolerate inconsistent marker corner positions as this algorithm aims to minimize root mean squared deviation of reference points and should be robust against a few outliers (at least when Gaussian distributed).

As the Kabsch algorithm provides the scale, we also don't depend on the map to be precisely scaled.

Where we need help

We would like to implement the approach described above (we are already working on it with promising intermediate results).

However, we want to make sure that we understood correctly the current approach, if the problems we identified are indeed problems or if maybe we overlooked a detail.

We can report that the current approach leads to worse results than our approach to include the marker corners in the local bundle adjustment. We believe that this is due to the points described above but of course it could be for different reasons. We can also provide the data that led to our assumptions (it was generated in Unreal Engine, using AirSim to control a drone in a virtual environment with known marker positions).

[ymd-stella]

It looks fine to me.

I tried to give a constraint on the shape, but gave up due to the difficulty of weighting. I do not have enough time to spend on this feature.

• 0aeba4a

Also about updating the marker poses.

• https://github.com/stella-cv/stella_vslam/pull/311/files
• Marker integration #242 (comment)