Check failed: it != map.end() when trying to do optv

[B-Meg]

I load my map into the maplab console. Optimizing the map with optvi works perfect. But when I try to do the BA without IMU by using optv or optvi --ba_include_inertial=false I get the following Error:

F0918 21:58:51.689087 21734 accessors.h:46] Check failed: it != map.end()
*** Check failure stack trace: ***
*** Aborted at 1631995131 (unix time) try "date -d @1631995131" if you are using GNU date ***
PC: @ 0x7f300a91ffb7 gsignal
*** SIGABRT (@0x3e8000054e6) received by PID 21734 (TID 0x7f300bc88b40) from PID 21734; stack trace: ***
@ 0x7f300a920040 (unknown)
@ 0x7f300a91ffb7 gsignal
@ 0x7f300a921921 abort
@ 0x5618333d8255 (unknown)
@ 0x7f300b498b0d google::LogMessage::Fail()
@ 0x7f300b49a9b1 google::LogMessage::SendToLog()
@ 0x7f300b49863d google::LogMessage::Flush()
@ 0x7f300b49b389 google::LogMessageFatal::~LogMessageFatal()
@ 0x7f2fee0af4c8 map_optimization::OptimizationProblem::applyGaugeFixesForInitialVertices()
@ 0x7f2fee0f5b4f map_optimization::constructOptimizationProblem()
@ 0x7f2fee0ed580 map_optimization::VIMapOptimizer::optimize()
@ 0x7f2feb1ce13d map_optimization_plugin::OptimizerPlugin::optimize()
@ 0x7f2feb1ce386 _ZZN23map_optimization_plugin15OptimizerPluginC4EPN6common7ConsoleEPN13visualization21ViwlsGraphRvizPlotterEENKUlvE0_clEv
@ 0x7f300b8d609e common::CommandRegisterer::processCommand()
@ 0x7f300b8d8ca4 common::Console::RunCommand()
@ 0x7f300b8da34e common::Console::RunCommandPrompt()
@ 0x5618333d7df6 (unknown)
@ 0x7f300a902bf7 __libc_start_main
@ 0x5618333d7e8a (unknown)
Aborted (core dumped)

Why do I get it and why only when I am doing the BA without IMU?

[smauq]

Ah, that's the part of the code that fixes the scale or other things in the map. If you remove the IMU you need to fix the scale (usually by fixing the distance between the first two vertices or the distance to the first keypoint observation for example). I think there's just a bug in that part of the code for the main release, since at that time it wasn't a very tested feature. In our development branch of the code we rewrote that whole part and it should be fixed now.

The function that crashes is applyGaugeFixesForInitialVertices but I think the problem solution was in this part of the code

maplab/algorithms/map-optimization/src/vi-optimization-builder.cc

Lines 98 to 128 in 618a0e0

	// Fixing open DoF of the visual(-inertial) problem. We assume that if there
	// is inertial data, that all missions will have them.
	const bool visual_only =
	options.add_visual_constraints && !options.add_inertial_constraints;
	// Determine and apply the gauge fixes.
	MissionClusterGaugeFixes fixes_of_mission_cluster;
	if (!visual_only) {
	fixes_of_mission_cluster.position_dof_fixed = true;
	fixes_of_mission_cluster.rotation_dof_fixed = FixedRotationDoF::kYaw;
	fixes_of_mission_cluster.scale_fixed = false;
	} else {
	fixes_of_mission_cluster.position_dof_fixed = true;
	fixes_of_mission_cluster.rotation_dof_fixed = FixedRotationDoF::kAll;
	fixes_of_mission_cluster.scale_fixed = true;
	}
	const size_t num_clusters = problem->getMissionCoobservationClusters().size();
	std::vector<MissionClusterGaugeFixes> vi_cluster_fixes(
	num_clusters, fixes_of_mission_cluster);
	// Merge with already applied fixes (if necessary).
	const std::vector<MissionClusterGaugeFixes>* already_applied_cluster_fixes =
	problem->getAppliedGaugeFixesForInitialVertices();
	if (already_applied_cluster_fixes) {
	std::vector<MissionClusterGaugeFixes> merged_fixes;
	mergeGaugeFixes(
	vi_cluster_fixes, *already_applied_cluster_fixes, &merged_fixes);
	problem->applyGaugeFixesForInitialVertices(merged_fixes);
	} else {
	problem->applyGaugeFixesForInitialVertices(vi_cluster_fixes);
	}

versus

maplab/algorithms/map-optimization/src/vi-optimization-builder.cc

Lines 234 to 344 in c7b0d78

	// We analyze the available constraints in each mission clusters, i.e.
	// missions that are connected through either visual constraints or loop
	// closure edges, and determine the gauge fixes.
	const std::vector<vi_map::MissionIdSet>& mission_clusters =
	problem->getMissionCoobservationClusters();
	const size_t num_clusters = mission_clusters.size();
	std::vector<MissionClusterGaugeFixes> mission_cluster_gauge_fixes(
	num_clusters);
	CHECK_EQ(mission_clusters.size(), mission_cluster_gauge_fixes.size());
	for (size_t cluster_idx = 0u; cluster_idx < num_clusters; ++cluster_idx) {
	MissionClusterGaugeFixes& mission_cluster_gauge_fix =
	mission_cluster_gauge_fixes[cluster_idx];
	const vi_map::MissionIdSet& mission_cluster = mission_clusters[cluster_idx];
	const size_t cluster_num_absolute_6dof_present =
	vi_map_helpers::getNumAbsolute6DoFConstraintsForMissionCluster(
	*map, mission_cluster);
	const size_t cluster_num_absolute_6dof_used = std::min(
	cluster_num_absolute_6dof_present, num_absolute_6dof_constraints_added);
	const bool cluster_has_inertial =
	vi_map_helpers::hasInertialConstraintsInAllMissionsInCluster(
	*map, mission_cluster) &&
	(num_inertial_constraints_added > 0u);
	const bool cluster_has_visual =
	vi_map_helpers::hasVisualConstraintsInAllMissionsInCluster(
	*map, mission_cluster) &&
	(num_visual_constraints_added > 0u);
	const bool cluster_has_wheel_odometry =
	vi_map_helpers::hasWheelOdometryConstraintsInAllMissionsInCluster(
	*map, mission_cluster) &&
	(num_wheel_odometry_constraints_added > 0u);
	const bool cluster_has_6dof_odometry =
	vi_map_helpers::has6DoFOdometryConstraintsInAllMissionsInCluster(
	*map, mission_cluster) &&
	(num_6dof_odometry_constraints_added > 0u);
	// Note that if there are lc edges they always are within the cluster,
	// because otherwise the other mission would have been part of the cluster.
	const bool cluster_has_lc_edges =
	vi_map_helpers::hasLcEdgesInMissionCluster(*map, mission_cluster) &&
	(num_lc_edges > 0u);
	CHECK(
	cluster_has_inertial || cluster_has_visual ||
	cluster_has_wheel_odometry || cluster_has_6dof_odometry)
	<< "Either inertial, visual or wheel odometry constraints need to be "
	"available to form a stable graph.";
	// Determine observability of scale, global position and global orientation.
	const bool scale_is_observable =
	cluster_has_inertial || cluster_has_wheel_odometry ||
	cluster_has_6dof_odometry ||
	(cluster_has_visual && cluster_num_absolute_6dof_used > 1u);
	const bool global_position_is_observable =
	cluster_num_absolute_6dof_used > 0u;
	const bool global_yaw_is_observable = cluster_num_absolute_6dof_used > 0u;
	const bool global_roll_pitch_is_observable =
	cluster_num_absolute_6dof_used > 0u || cluster_has_inertial;
	std::stringstream ss;
	ss << "\nMission Cluster: ";
	for (const vi_map::MissionId& mission_id : mission_cluster) {
	ss << "\n\t" << mission_id;
	}
	ss << "\n\nConstraints:";
	ss << "\n\tInertial constraints:\t\t"
	<< ((cluster_has_inertial) ? "on" : "off");
	ss << "\n\tVisual constraints:\t\t"
	<< ((cluster_has_visual) ? "on" : "off");
	ss << "\n\tWheel odometry constraints:\t"
	<< ((cluster_has_wheel_odometry) ? "on" : "off");
	ss << "\n\t6DoF odometry constraints:\t"
	<< ((cluster_has_6dof_odometry) ? "on" : "off");
	ss << "\n\tAbsolute 6DoF constraints:\t"
	<< ((cluster_num_absolute_6dof_used > 0) ? "on" : "off");
	ss << "\n\tLoop closure edge constraints:\t"
	<< ((cluster_has_lc_edges > 0) ? "on" : "off");
	ss << "\n\nIs observable:";
	ss << "\n\tScale: " << ((scale_is_observable) ? "yes" : "no");
	ss << "\n\tGlobal position: "
	<< ((global_position_is_observable) ? "yes" : "no");
	ss << "\n\tGlobal yaw: " << ((global_yaw_is_observable) ? "yes" : "no");
	ss << "\n\tGlobal roll/pitch: "
	<< ((global_roll_pitch_is_observable) ? "yes" : "no");
	ss << "\n";
	VLOG(1) << ss.str();
	// Apply gauge fixes for this cluster.
	mission_cluster_gauge_fix.position_dof_fixed =
	!global_position_is_observable;
	mission_cluster_gauge_fix.scale_fixed = !scale_is_observable;
	// Currently there is no scenario where yaw is observable but roll and
	// pitch, this could change if magnetometer are added. This check should
	// make sure that the logic below does not do weird stuff it add such a
	// scenario, but forget to change code here.
	CHECK(!(global_yaw_is_observable && !global_roll_pitch_is_observable));
	mission_cluster_gauge_fix.rotation_dof_fixed =
	global_roll_pitch_is_observable
	? (global_yaw_is_observable ? FixedRotationDoF::kNone
	: FixedRotationDoF::kYaw)
	: FixedRotationDoF::kAll;
	}